914 lines
29 KiB
C++
914 lines
29 KiB
C++
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#include "BaseSerialization.h"
|
|
#include "SerializationCommon.h"
|
|
|
|
#include <fstream>
|
|
#include <sstream>
|
|
|
|
namespace snippetvehicle
|
|
{
|
|
|
|
bool readAxleDescription(const rapidjson::Document& config, PxVehicleAxleDescription& axleDesc)
|
|
{
|
|
if (!config.HasMember("AxleDescription"))
|
|
return false;
|
|
|
|
const PxU32 nbAxles = config["AxleDescription"].Size();
|
|
if (nbAxles > PxVehicleLimits::eMAX_NB_AXLES)
|
|
return false;
|
|
|
|
axleDesc.setToDefault();
|
|
|
|
//Read each axle in turn.
|
|
for (PxU32 i = 0; i < nbAxles; i++)
|
|
{
|
|
const rapidjson::Value& axle = config["AxleDescription"][i];
|
|
|
|
if (!axle.HasMember("WheelIds"))
|
|
return false;
|
|
const rapidjson::Value& wheelIds = axle["WheelIds"];
|
|
|
|
const PxU32 nbWheelIds = wheelIds.Size();
|
|
if(nbWheelIds > PxVehicleLimits::eMAX_NB_WHEELS)
|
|
return false;
|
|
PxU32 axleWheelIds[PxVehicleLimits::eMAX_NB_WHEELS];
|
|
for (PxU32 j = 0; j < nbWheelIds; j++)
|
|
{
|
|
axleWheelIds[j]= wheelIds[j].GetInt();
|
|
}
|
|
axleDesc.addAxle(nbWheelIds, axleWheelIds);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeAxleDescription(const PxVehicleAxleDescription& axleDesc, rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("AxleDescription");
|
|
|
|
writer.StartArray();
|
|
for(PxU32 i = 0; i < axleDesc.getNbAxles(); i++)
|
|
{
|
|
writer.StartObject();
|
|
writer.Key("WheelIds");
|
|
writer.StartArray();
|
|
for(PxU32 j = 0; j < axleDesc.getNbWheelsOnAxle(i); j++)
|
|
{
|
|
writer.Int(axleDesc.getWheelOnAxle(j, i));
|
|
}
|
|
writer.EndArray();
|
|
writer.EndObject();
|
|
}
|
|
writer.EndArray();
|
|
|
|
return true;
|
|
}
|
|
|
|
bool readFrame(const rapidjson::Document& config, PxVehicleFrame& frame)
|
|
{
|
|
if (!config.HasMember("Frame"))
|
|
return false;
|
|
|
|
if (!config["Frame"].HasMember("LngAxis"))
|
|
return false;
|
|
if (!config["Frame"].HasMember("LatAxis"))
|
|
return false;
|
|
if (!config["Frame"].HasMember("VrtAxis"))
|
|
return false;
|
|
|
|
|
|
const PxU32 lngAxis = config["Frame"]["LngAxis"].GetInt();
|
|
const PxU32 latAxis = config["Frame"]["LatAxis"].GetInt();
|
|
const PxU32 vrtAxis = config["Frame"]["VrtAxis"].GetInt();
|
|
if ((lngAxis == latAxis) || (lngAxis == vrtAxis) || (latAxis == vrtAxis))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
frame.lngAxis = static_cast<PxVehicleAxes::Enum>(lngAxis);
|
|
frame.latAxis = static_cast<PxVehicleAxes::Enum>(latAxis);
|
|
frame.vrtAxis = static_cast<PxVehicleAxes::Enum>(vrtAxis);
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeFrame(const PxVehicleFrame& frame, rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("Frame");
|
|
writer.StartObject();
|
|
writer.Key("LngAxis");
|
|
writer.Int(static_cast<PxU32>(frame.lngAxis));
|
|
writer.Key("LatAxis");
|
|
writer.Int(static_cast<PxU32>(frame.latAxis));
|
|
writer.Key("VrtAxis");
|
|
writer.Int(static_cast<PxU32>(frame.vrtAxis));
|
|
writer.EndObject();
|
|
return true;
|
|
}
|
|
|
|
bool readScale(const rapidjson::Document& config, PxVehicleScale& scale)
|
|
{
|
|
if (!config.HasMember("Scale"))
|
|
return false;
|
|
|
|
if (!config["Scale"].HasMember("Scale"))
|
|
return false;
|
|
|
|
scale.scale = static_cast<PxReal>(config["Scale"]["Scale"].GetDouble());
|
|
return true;
|
|
}
|
|
|
|
bool writeScale(const PxVehicleScale& scale, rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("Scale");
|
|
writer.StartObject();
|
|
writer.Key("Scale");
|
|
writer.Double(static_cast<double>(scale.scale));
|
|
writer.EndObject();
|
|
return true;
|
|
}
|
|
|
|
bool readBrakeResponseParams
|
|
(const rapidjson::Document& config, const PxVehicleAxleDescription& axleDesc,
|
|
PxVehicleBrakeCommandResponseParams& brakeResponseParams)
|
|
{
|
|
if (!config.HasMember("BrakeCommandResponseParams"))
|
|
return false;
|
|
|
|
if (!readCommandResponseParams(config["BrakeCommandResponseParams"], axleDesc, brakeResponseParams))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeBrakeResponseParams
|
|
(const PxVehicleBrakeCommandResponseParams& brakeResponseParams, const PxVehicleAxleDescription& axleDesc,
|
|
rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("BrakeCommandResponseParams");
|
|
writer.StartObject();
|
|
writeCommandResponseParams(brakeResponseParams, axleDesc, writer);
|
|
writer.EndObject();
|
|
return true;
|
|
}
|
|
|
|
bool readHandbrakeResponseParams
|
|
(const rapidjson::Document& config, const PxVehicleAxleDescription& axleDesc,
|
|
PxVehicleBrakeCommandResponseParams& handbrakeResponseParams)
|
|
{
|
|
if (!config.HasMember("HandbrakeCommandResponseParams"))
|
|
return false;
|
|
|
|
if (!readCommandResponseParams(config["HandbrakeCommandResponseParams"], axleDesc, handbrakeResponseParams))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeHandbrakeResponseParams
|
|
(const PxVehicleBrakeCommandResponseParams& handrakeResponseParams, const PxVehicleAxleDescription& axleDesc,
|
|
rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("HandbrakeCommandResponseParams");
|
|
writer.StartObject();
|
|
writeCommandResponseParams(handrakeResponseParams, axleDesc, writer);
|
|
writer.EndObject();
|
|
return true;
|
|
}
|
|
|
|
bool readSteerResponseParams
|
|
(const rapidjson::Document& config, const PxVehicleAxleDescription& axleDesc,
|
|
PxVehicleSteerCommandResponseParams& steerResponseParams)
|
|
{
|
|
if (!config.HasMember("SteerCommandResponseParams"))
|
|
return false;
|
|
|
|
if (!readCommandResponseParams(config["SteerCommandResponseParams"], axleDesc, steerResponseParams))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeSteerResponseParams
|
|
(const PxVehicleSteerCommandResponseParams& steerResponseParams, const PxVehicleAxleDescription& axleDesc,
|
|
rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("SteerCommandResponseParams");
|
|
writer.StartObject();
|
|
writeCommandResponseParams(steerResponseParams, axleDesc, writer);
|
|
writer.EndObject();
|
|
return true;
|
|
}
|
|
|
|
bool readAckermannParams(const rapidjson::Document& config, PxVehicleAckermannParams& ackermannParams)
|
|
{
|
|
if (!config.HasMember("AckermannParams"))
|
|
return false;
|
|
|
|
const rapidjson::Value& corrections = config["AckermannParams"];
|
|
|
|
if (!corrections.HasMember("WheelBase"))
|
|
return false;
|
|
if (!corrections.HasMember("TrackWidth"))
|
|
return false;
|
|
if (!corrections.HasMember("Strength"))
|
|
return false;
|
|
|
|
ackermannParams.wheelBase = static_cast<PxReal>(corrections["WheelBase"].GetDouble());
|
|
ackermannParams.trackWidth = static_cast<PxReal>(corrections["TrackWidth"].GetDouble());
|
|
ackermannParams.strength = static_cast<PxReal>(corrections["Strength"].GetDouble());
|
|
|
|
if (!corrections.HasMember("WheelIds"))
|
|
return false;
|
|
const rapidjson::Value& wheelIds = corrections["WheelIds"];
|
|
const PxU32 nbWheelIds = wheelIds.Size();
|
|
if (nbWheelIds != 2)
|
|
return false;
|
|
for (PxU32 j = 0; j < 2; j++)
|
|
{
|
|
ackermannParams.wheelIds[j] = wheelIds[j].GetInt();
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeAckermannParams(const PxVehicleAckermannParams& ackermannParams, rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("AckermannParams");
|
|
writer.StartObject();
|
|
writer.Key("WheelIds");
|
|
writer.StartArray();
|
|
writer.Int(ackermannParams.wheelIds[0]);
|
|
writer.Int(ackermannParams.wheelIds[1]);
|
|
writer.EndArray();
|
|
writer.Key("WheelBase");
|
|
writer.Double(static_cast<double>(ackermannParams.wheelBase));
|
|
writer.Key("TrackWidth");
|
|
writer.Double(static_cast<double>(ackermannParams.trackWidth));
|
|
writer.Key("Strength");
|
|
writer.Double(static_cast<double>(ackermannParams.strength));
|
|
writer.EndObject();
|
|
return true;
|
|
}
|
|
|
|
bool readRigidBodyParams
|
|
(const rapidjson::Document& config, PxVehicleRigidBodyParams& rigidBodyParams)
|
|
{
|
|
if (!config.HasMember("RigidBodyParams"))
|
|
return false;
|
|
|
|
if (!config["RigidBodyParams"].HasMember("Mass"))
|
|
return false;
|
|
if (!config["RigidBodyParams"].HasMember("MOI"))
|
|
return false;
|
|
|
|
rigidBodyParams.mass = static_cast<PxReal>(config["RigidBodyParams"]["Mass"].GetDouble());
|
|
if (!readVec3(config["RigidBodyParams"]["MOI"], rigidBodyParams.moi))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeRigidBodyParams
|
|
(const PxVehicleRigidBodyParams& rigidBodyParams, rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("RigidBodyParams");
|
|
writer.StartObject();
|
|
writer.Key("Mass");
|
|
writer.Double(static_cast<double>(rigidBodyParams.mass));
|
|
writer.Key("MOI");
|
|
writeVec3(rigidBodyParams.moi, writer);
|
|
writer.EndObject();
|
|
return true;
|
|
}
|
|
|
|
bool readSuspensionParams(const rapidjson::Document& config, const PxVehicleAxleDescription& axleDesc, PxVehicleSuspensionParams* suspParams)
|
|
{
|
|
if (!config.HasMember("SuspensionParams"))
|
|
return false;
|
|
|
|
const rapidjson::Value& suspensions = config["SuspensionParams"];
|
|
const PxU32 nbSuspensions = suspensions.Size();
|
|
if (nbSuspensions != axleDesc.getNbWheels())
|
|
return false;
|
|
|
|
for (PxU32 i = 0; i < nbSuspensions; i++)
|
|
{
|
|
if(!suspensions[i].HasMember("WheelId"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("SuspensionAttachment"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("SuspensionTravelDir"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("SuspensionTravelDist"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("WheelAttachment"))
|
|
return false;
|
|
|
|
const PxU32 wheelId = suspensions[i]["WheelId"].GetInt();
|
|
PxVehicleSuspensionParams& sp = suspParams[wheelId];
|
|
|
|
if (!readTransform(suspensions[i]["SuspensionAttachment"], sp.suspensionAttachment))
|
|
return false;
|
|
if (!readVec3(suspensions[i]["SuspensionTravelDir"], sp.suspensionTravelDir))
|
|
return false;
|
|
sp.suspensionTravelDist = static_cast<PxReal>(suspensions[i]["SuspensionTravelDist"].GetDouble());
|
|
if (!readTransform(suspensions[i]["WheelAttachment"], sp.wheelAttachment))
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeSuspensionParams(const PxVehicleSuspensionParams* suspParams, const PxVehicleAxleDescription& axleDesc,
|
|
rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("SuspensionParams");
|
|
writer.StartArray();
|
|
|
|
for(PxU32 i = 0; i < axleDesc.nbWheels; i++)
|
|
{
|
|
writer.StartObject();
|
|
|
|
writer.Key("WheelId");
|
|
const PxU32 wheelId = axleDesc.wheelIdsInAxleOrder[i];
|
|
writer.Int(wheelId);
|
|
|
|
writer.Key("SuspensionAttachment");
|
|
writeTransform(suspParams[wheelId].suspensionAttachment, writer);
|
|
|
|
writer.Key("SuspensionTravelDir");
|
|
writeVec3(suspParams[wheelId].suspensionTravelDir, writer);
|
|
|
|
writer.Key("SuspensionTravelDist");
|
|
writer.Double(static_cast<double>(suspParams[wheelId].suspensionTravelDist));
|
|
|
|
writer.Key("WheelAttachment");
|
|
writeTransform(suspParams[wheelId].wheelAttachment, writer);
|
|
|
|
writer.EndObject();
|
|
}
|
|
|
|
writer.EndArray();
|
|
return true;
|
|
}
|
|
|
|
static const char* kLimitSuspensionExpansionVelocity = "LimitSuspensionExpansionVelocity";
|
|
|
|
bool readSuspensionStateCalculationParams(const rapidjson::Document& config, PxVehicleSuspensionStateCalculationParams& suspParams)
|
|
{
|
|
if (!config.HasMember("SuspensionStateCalculationParams"))
|
|
return false;
|
|
const rapidjson::Value& suspCalcParams = config["SuspensionStateCalculationParams"];
|
|
|
|
if (!suspCalcParams.HasMember("JounceCalculationType"))
|
|
return false;
|
|
|
|
if (!suspCalcParams.HasMember(kLimitSuspensionExpansionVelocity))
|
|
return false;
|
|
|
|
suspParams.suspensionJounceCalculationType = static_cast<PxVehicleSuspensionJounceCalculationType::Enum>(suspCalcParams["JounceCalculationType"].GetInt());
|
|
suspParams.limitSuspensionExpansionVelocity = suspCalcParams[kLimitSuspensionExpansionVelocity].GetBool();
|
|
return true;
|
|
}
|
|
|
|
bool writeSuspensionStateCalculationParams
|
|
(const PxVehicleSuspensionStateCalculationParams& suspParams,
|
|
rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("SuspensionStateCalculationParams");
|
|
writer.StartObject();
|
|
writer.Key("JounceCalculationType");
|
|
writer.Int(static_cast<PxU32>(suspParams.suspensionJounceCalculationType));
|
|
writer.Key(kLimitSuspensionExpansionVelocity);
|
|
writer.Bool(suspParams.limitSuspensionExpansionVelocity);
|
|
writer.EndObject();
|
|
return true;
|
|
}
|
|
|
|
bool readSuspensionComplianceParams(const rapidjson::Document& config, const PxVehicleAxleDescription& axleDesc, PxVehicleSuspensionComplianceParams* suspCompParams)
|
|
{
|
|
if (!config.HasMember("SuspensionComplianceParams"))
|
|
return false;
|
|
|
|
const rapidjson::Value& suspensions = config["SuspensionComplianceParams"];
|
|
const PxU32 nbSuspensions = suspensions.Size();
|
|
if (nbSuspensions != axleDesc.getNbWheels())
|
|
return false;
|
|
|
|
for (PxU32 i = 0; i < nbSuspensions; i++)
|
|
{
|
|
if (!suspensions[i].HasMember("WheelId"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("WheelToeAngle"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("WheelCamberAngle"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("SuspForceAppPoint"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("SuspForceAppPoint"))
|
|
return false;
|
|
|
|
const PxU32 wheelId = suspensions[i]["WheelId"].GetInt();
|
|
|
|
if (!readFloatLookupTable(suspensions[i]["WheelToeAngle"], suspCompParams[wheelId].wheelToeAngle))
|
|
return false;
|
|
if (!readFloatLookupTable(suspensions[i]["WheelCamberAngle"], suspCompParams[wheelId].wheelCamberAngle))
|
|
return false;
|
|
if (!readVec3LookupTable(suspensions[i]["SuspForceAppPoint"], suspCompParams[wheelId].suspForceAppPoint))
|
|
return false;
|
|
if (!readVec3LookupTable(suspensions[i]["TireForceAppPoint"], suspCompParams[wheelId].tireForceAppPoint))
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeSuspensionComplianceParams
|
|
(const PxVehicleSuspensionComplianceParams* suspParams, const PxVehicleAxleDescription& axleDesc,
|
|
rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("SuspensionComplianceParams");
|
|
writer.StartArray();
|
|
|
|
for (PxU32 i = 0; i < axleDesc.nbWheels; i++)
|
|
{
|
|
writer.StartObject();
|
|
|
|
writer.Key("WheelId");
|
|
const PxU32 wheelId = axleDesc.wheelIdsInAxleOrder[i];
|
|
writer.Int(wheelId);
|
|
|
|
writer.Key("WheelToeAngle");
|
|
writeFloatLookupTable(suspParams[wheelId].wheelToeAngle, writer);
|
|
|
|
writer.Key("WheelCamberAngle");
|
|
writeFloatLookupTable(suspParams[wheelId].wheelCamberAngle, writer);
|
|
|
|
writer.Key("SuspForceAppPoint");
|
|
writeVec3LookupTable(suspParams[wheelId].suspForceAppPoint, writer);
|
|
|
|
writer.Key("TireForceAppPoint");
|
|
writeVec3LookupTable(suspParams[wheelId].tireForceAppPoint, writer);
|
|
|
|
writer.EndObject();
|
|
}
|
|
|
|
writer.EndArray();
|
|
return true;
|
|
}
|
|
|
|
bool readSuspensionForceParams(const rapidjson::Document& config, const PxVehicleAxleDescription& axleDesc,
|
|
PxVehicleSuspensionForceParams* suspParams)
|
|
{
|
|
if (!config.HasMember("SuspensionForceParams"))
|
|
return false;
|
|
|
|
const rapidjson::Value& suspensions = config["SuspensionForceParams"];
|
|
const PxU32 nbSuspensions = suspensions.Size();
|
|
if (nbSuspensions != axleDesc.getNbWheels())
|
|
return false;
|
|
|
|
for (PxU32 i = 0; i < nbSuspensions; i++)
|
|
{
|
|
if (!suspensions[i].HasMember("WheelId"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("Damping"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("Stiffness"))
|
|
return false;
|
|
if (!suspensions[i].HasMember("SprungMass"))
|
|
return false;
|
|
|
|
const PxU32 wheelId = suspensions[i]["WheelId"].GetInt();
|
|
suspParams[wheelId].damping = static_cast<PxReal>(suspensions[i]["Damping"].GetDouble());
|
|
suspParams[wheelId].stiffness = static_cast<PxReal>(suspensions[i]["Stiffness"].GetDouble());
|
|
suspParams[wheelId].sprungMass = static_cast<PxReal>(suspensions[i]["SprungMass"].GetDouble());
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeSuspensionForceParams
|
|
(const PxVehicleSuspensionForceParams* suspParams, const PxVehicleAxleDescription& axleDesc,
|
|
rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("SuspensionForceParams");
|
|
writer.StartArray();
|
|
|
|
for (PxU32 i = 0; i < axleDesc.nbWheels; i++)
|
|
{
|
|
writer.StartObject();
|
|
|
|
writer.Key("WheelId");
|
|
const PxU32 wheelId = axleDesc.wheelIdsInAxleOrder[i];
|
|
writer.Int(wheelId);
|
|
|
|
writer.Key("Damping");
|
|
writer.Double(double(suspParams[wheelId].damping));
|
|
|
|
writer.Key("Stiffness");
|
|
writer.Double(double(suspParams[wheelId].stiffness));
|
|
|
|
writer.Key("SprungMass");
|
|
writer.Double(double(suspParams[wheelId].sprungMass));
|
|
|
|
writer.EndObject();
|
|
}
|
|
|
|
writer.EndArray();
|
|
return true;
|
|
}
|
|
|
|
bool readTireForceParams
|
|
(const rapidjson::Document& config, const PxVehicleAxleDescription& axleDesc,
|
|
PxVehicleTireForceParams* tireParams)
|
|
{
|
|
if (!config.HasMember("TireForceParams"))
|
|
return false;
|
|
|
|
const rapidjson::Value& tires = config["TireForceParams"];
|
|
const PxU32 nbTires = tires.Size();
|
|
if (nbTires != axleDesc.getNbWheels())
|
|
return false;
|
|
|
|
for (PxU32 i = 0; i < nbTires; i++)
|
|
{
|
|
if (!tires[i].HasMember("WheelId"))
|
|
return false;
|
|
if (!tires[i].HasMember("LongitudinalStiffness"))
|
|
return false;
|
|
if (!tires[i].HasMember("LateralStiffnessX"))
|
|
return false;
|
|
if (!tires[i].HasMember("LateralStiffnessY"))
|
|
return false;
|
|
if (!tires[i].HasMember("CamberStiffness"))
|
|
return false;
|
|
if (!tires[i].HasMember("RestLoad"))
|
|
return false;
|
|
if (!tires[i].HasMember("FrictionVsSlip"))
|
|
return false;
|
|
if (!tires[i].HasMember("TireLoadFilter"))
|
|
return false;
|
|
|
|
const PxU32 wheelId = tires[i]["WheelId"].GetInt();
|
|
tireParams[wheelId].longStiff = static_cast<PxReal>(tires[i]["LongitudinalStiffness"].GetDouble());
|
|
tireParams[wheelId].latStiffX = static_cast<PxReal>(tires[i]["LateralStiffnessX"].GetDouble());
|
|
tireParams[wheelId].latStiffY = static_cast<PxReal>(tires[i]["LateralStiffnessY"].GetDouble());
|
|
tireParams[wheelId].camberStiff = static_cast<PxReal>(tires[i]["CamberStiffness"].GetDouble());
|
|
tireParams[wheelId].restLoad = static_cast<PxReal>(tires[i]["RestLoad"].GetDouble());
|
|
|
|
const rapidjson::Value& frictionVsSlip = tires[i]["FrictionVsSlip"];
|
|
if (frictionVsSlip.Size() != 3)
|
|
return false;
|
|
for (PxU32 j = 0; j < 3; j++)
|
|
{
|
|
const rapidjson::Value& element = tires[i]["FrictionVsSlip"][j];
|
|
if (element.Size() != 2)
|
|
return false;
|
|
|
|
tireParams[wheelId].frictionVsSlip[j][0] = static_cast<PxReal>(element[0].GetDouble());
|
|
tireParams[wheelId].frictionVsSlip[j][1] = static_cast<PxReal>(element[1].GetDouble());
|
|
}
|
|
|
|
const rapidjson::Value& tireLoadFilter = tires[i]["TireLoadFilter"];
|
|
if (tireLoadFilter.Size() != 2)
|
|
return false;
|
|
for (PxU32 j = 0; j < 2; j++)
|
|
{
|
|
const rapidjson::Value& element = tires[i]["TireLoadFilter"][j];
|
|
if (element.Size() != 2)
|
|
return false;
|
|
|
|
tireParams[wheelId].loadFilter[j][0] = static_cast<PxReal>(element[0].GetDouble());
|
|
tireParams[wheelId].loadFilter[j][1] = static_cast<PxReal>(element[1].GetDouble());
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeTireForceParams
|
|
(const PxVehicleTireForceParams* tireParams, const PxVehicleAxleDescription& axleDesc,
|
|
rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("TireForceParams");
|
|
writer.StartArray();
|
|
|
|
for (PxU32 i = 0; i < axleDesc.nbWheels; i++)
|
|
{
|
|
writer.StartObject();
|
|
|
|
writer.Key("WheelId");
|
|
const PxU32 wheelId = axleDesc.wheelIdsInAxleOrder[i];
|
|
writer.Int(wheelId);
|
|
|
|
writer.Key("LongitudinalStiffness");
|
|
writer.Double(static_cast<double>(tireParams[wheelId].longStiff));
|
|
|
|
writer.Key("LateralStiffnessX");
|
|
writer.Double(static_cast<double>(tireParams[wheelId].latStiffX));
|
|
|
|
writer.Key("LateralStiffnessY");
|
|
writer.Double(static_cast<double>(tireParams[wheelId].latStiffY));
|
|
|
|
writer.Key("CamberStiffness");
|
|
writer.Double(static_cast<double>(tireParams[wheelId].camberStiff));
|
|
|
|
writer.Key("RestLoad");
|
|
writer.Double(static_cast<double>(tireParams[wheelId].restLoad));
|
|
|
|
writer.Key("FrictionVsSlip");
|
|
writer.StartArray();
|
|
for(PxU32 k = 0; k < 3; k++)
|
|
{
|
|
writer.StartArray();
|
|
writer.Double(static_cast<double>(tireParams[wheelId].frictionVsSlip[k][0]));
|
|
writer.Double(static_cast<double>(tireParams[wheelId].frictionVsSlip[k][1]));
|
|
writer.EndArray();
|
|
}
|
|
writer.EndArray();
|
|
|
|
writer.Key("TireLoadFilter");
|
|
writer.StartArray();
|
|
for (PxU32 k = 0; k < 2; k++)
|
|
{
|
|
writer.StartArray();
|
|
writer.Double(static_cast<double>(tireParams[wheelId].loadFilter[k][0]));
|
|
writer.Double(static_cast<double>(tireParams[wheelId].loadFilter[k][1]));
|
|
writer.EndArray();
|
|
}
|
|
writer.EndArray();
|
|
|
|
writer.EndObject();
|
|
}
|
|
|
|
writer.EndArray();
|
|
return true;
|
|
}
|
|
|
|
bool readWheelParams
|
|
(const rapidjson::Document& config, const PxVehicleAxleDescription& axleDesc,
|
|
PxVehicleWheelParams* wheelParams)
|
|
{
|
|
if (!config.HasMember("WheelParams"))
|
|
return false;
|
|
|
|
const rapidjson::Value& wheels = config["WheelParams"];
|
|
const PxU32 nbWheels = wheels.Size();
|
|
if (nbWheels != axleDesc.getNbWheels())
|
|
return false;
|
|
|
|
for (PxU32 i = 0; i < nbWheels; i++)
|
|
{
|
|
if (!wheels[i].HasMember("WheelId"))
|
|
return false;
|
|
if (!wheels[i].HasMember("HalfWidth"))
|
|
return false;
|
|
if (!wheels[i].HasMember("Radius"))
|
|
return false;
|
|
if (!wheels[i].HasMember("Mass"))
|
|
return false;
|
|
if (!wheels[i].HasMember("MOI"))
|
|
return false;
|
|
if (!wheels[i].HasMember("DampingRate"))
|
|
return false;
|
|
|
|
const PxU32 wheelId = wheels[i]["WheelId"].GetInt();
|
|
wheelParams[wheelId].halfWidth = static_cast<PxReal>(wheels[i]["HalfWidth"].GetDouble());
|
|
wheelParams[wheelId].radius = static_cast<PxReal>(wheels[i]["Radius"].GetDouble());
|
|
wheelParams[wheelId].mass = static_cast<PxReal>(wheels[i]["Mass"].GetDouble());
|
|
wheelParams[wheelId].moi = static_cast<PxReal>(wheels[i]["MOI"].GetDouble());
|
|
wheelParams[wheelId].dampingRate = static_cast<PxReal>(wheels[i]["DampingRate"].GetDouble());
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeWheelParams
|
|
(const PxVehicleWheelParams* wheelParams, const PxVehicleAxleDescription& axleDesc,
|
|
rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer)
|
|
{
|
|
writer.Key("WheelParams");
|
|
writer.StartArray();
|
|
|
|
for (PxU32 i = 0; i < axleDesc.nbWheels; i++)
|
|
{
|
|
writer.StartObject();
|
|
|
|
writer.Key("WheelId");
|
|
const PxU32 wheelId = axleDesc.wheelIdsInAxleOrder[i];
|
|
writer.Int(wheelId);
|
|
|
|
writer.Key("HalfWidth");
|
|
writer.Double(static_cast<double>(wheelParams[wheelId].halfWidth));
|
|
|
|
writer.Key("Radius");
|
|
writer.Double(static_cast<double>(wheelParams[wheelId].radius));
|
|
|
|
writer.Key("Mass");
|
|
writer.Double(static_cast<double>(wheelParams[wheelId].mass));
|
|
|
|
writer.Key("MOI");
|
|
writer.Double(static_cast<double>(wheelParams[wheelId].moi));
|
|
|
|
writer.Key("DampingRate");
|
|
writer.Double(static_cast<double>(wheelParams[wheelId].dampingRate));
|
|
|
|
writer.EndObject();
|
|
}
|
|
|
|
writer.EndArray();
|
|
return true;
|
|
}
|
|
|
|
|
|
bool readBaseParamsFromJsonFile(const char* directory, const char* filename,
|
|
BaseVehicleParams& baseParams)
|
|
{
|
|
rapidjson::Document config;
|
|
if (!openDocument(directory, filename, config))
|
|
return false;
|
|
|
|
//////////////////////////////
|
|
//Read the high level params
|
|
//////////////////////////////
|
|
|
|
PxMemSet(&baseParams.axleDescription, 0xff, sizeof(baseParams.axleDescription));
|
|
if (!readAxleDescription(config, baseParams.axleDescription))
|
|
return false;
|
|
|
|
PxMemSet(&baseParams.frame, 0xff, sizeof(baseParams.frame));
|
|
if (!readFrame(config, baseParams.frame))
|
|
return false;
|
|
|
|
PxMemSet(&baseParams.scale, 0xff, sizeof(baseParams.scale));
|
|
if (!readScale(config, baseParams.scale))
|
|
return false;
|
|
|
|
//////////////////////////////
|
|
//Read the rigid body params
|
|
/////////////////////////////
|
|
|
|
PxMemSet(&baseParams.rigidBodyParams, 0xff, sizeof(baseParams.rigidBodyParams));
|
|
if (!readRigidBodyParams(config, baseParams.rigidBodyParams))
|
|
return false;
|
|
|
|
//////////////////////////////
|
|
//Read the suspension state calculation params.
|
|
//////////////////////////////
|
|
|
|
PxMemSet(&baseParams.suspensionStateCalculationParams, 0xff, sizeof(baseParams.suspensionStateCalculationParams));
|
|
if (!readSuspensionStateCalculationParams(config, baseParams.suspensionStateCalculationParams))
|
|
return false;
|
|
|
|
///////////////////////////////
|
|
//Read the command responses
|
|
///////////////////////////////
|
|
|
|
PxMemSet(&baseParams.brakeResponseParams[0], 0xff, sizeof(baseParams.brakeResponseParams[0]));
|
|
if (!readBrakeResponseParams(config, baseParams.axleDescription, baseParams.brakeResponseParams[0]))
|
|
return false;
|
|
|
|
PxMemSet(&baseParams.brakeResponseParams[1], 0xff, sizeof(baseParams.brakeResponseParams[1]));
|
|
if (!readHandbrakeResponseParams(config, baseParams.axleDescription, baseParams.brakeResponseParams[1]))
|
|
return false;
|
|
|
|
PxMemSet(&baseParams.steerResponseParams, 0xff, sizeof(baseParams.steerResponseParams));
|
|
if (!readSteerResponseParams(config, baseParams.axleDescription, baseParams.steerResponseParams))
|
|
return false;
|
|
|
|
PxMemSet(&baseParams.ackermannParams, 0xff, sizeof(baseParams.ackermannParams));
|
|
if (!readAckermannParams(config, baseParams.ackermannParams[0]))
|
|
return false;
|
|
|
|
///////////////////////////////////
|
|
//Read the suspension params
|
|
///////////////////////////////////
|
|
|
|
PxMemSet(baseParams.suspensionParams, 0xff, sizeof(baseParams.suspensionParams));
|
|
if (!readSuspensionParams(config, baseParams.axleDescription, baseParams.suspensionParams))
|
|
return false;
|
|
|
|
PxMemSet(baseParams.suspensionComplianceParams, 0x00, sizeof(baseParams.suspensionComplianceParams));
|
|
if (!readSuspensionComplianceParams(config, baseParams.axleDescription, baseParams.suspensionComplianceParams))
|
|
return false;
|
|
|
|
PxMemSet(baseParams.suspensionForceParams, 0xff, sizeof(baseParams.suspensionForceParams));
|
|
if (!readSuspensionForceParams(config, baseParams.axleDescription, baseParams.suspensionForceParams))
|
|
return false;
|
|
|
|
///////////////////////////////////
|
|
//Read the tire params
|
|
///////////////////////////////////
|
|
|
|
PxMemSet(baseParams.tireForceParams, 0xff, sizeof(baseParams.tireForceParams));
|
|
if (!readTireForceParams(config, baseParams.axleDescription, baseParams.tireForceParams))
|
|
return false;
|
|
|
|
//////////////////////////
|
|
//Read the wheel params
|
|
//////////////////////////
|
|
|
|
PxMemSet(baseParams.wheelParams, 0xff, sizeof(baseParams.wheelParams));
|
|
if (!readWheelParams(config, baseParams.axleDescription, baseParams.wheelParams))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool writeBaseParamsToJsonFile(const char* directory, const char* filename, const BaseVehicleParams& baseParams)
|
|
{
|
|
rapidjson::StringBuffer strbuf;
|
|
rapidjson::PrettyWriter<rapidjson::StringBuffer> writer(strbuf);
|
|
writer.StartObject();
|
|
|
|
//////////////////////////////
|
|
//Write the high level params
|
|
//////////////////////////////
|
|
|
|
writeAxleDescription(baseParams.axleDescription, writer);
|
|
writeFrame(baseParams.frame, writer);
|
|
writeScale(baseParams.scale, writer);
|
|
|
|
//////////////////////////////
|
|
//Write the rigid body params
|
|
/////////////////////////////
|
|
|
|
writeRigidBodyParams(baseParams.rigidBodyParams, writer);
|
|
|
|
|
|
//////////////////////////////
|
|
//Write the suspension state calculation params
|
|
//////////////////////////////
|
|
|
|
writeSuspensionStateCalculationParams(baseParams.suspensionStateCalculationParams, writer);
|
|
|
|
///////////////////////////////
|
|
//Write the command responses
|
|
///////////////////////////////
|
|
|
|
writeBrakeResponseParams(baseParams.brakeResponseParams[0], baseParams.axleDescription, writer);
|
|
writeHandbrakeResponseParams(baseParams.brakeResponseParams[1], baseParams.axleDescription, writer);
|
|
writeSteerResponseParams(baseParams.steerResponseParams, baseParams.axleDescription, writer);
|
|
writeAckermannParams(baseParams.ackermannParams[0], writer);
|
|
|
|
///////////////////////////////////
|
|
//Write the suspension params
|
|
///////////////////////////////////
|
|
|
|
writeSuspensionParams(baseParams.suspensionParams, baseParams.axleDescription, writer);
|
|
writeSuspensionComplianceParams(baseParams.suspensionComplianceParams, baseParams.axleDescription, writer);
|
|
writeSuspensionForceParams(baseParams.suspensionForceParams, baseParams.axleDescription, writer);
|
|
|
|
///////////////////////////////////
|
|
//Write the tire params
|
|
///////////////////////////////////
|
|
|
|
writeTireForceParams(baseParams.tireForceParams, baseParams.axleDescription, writer);
|
|
|
|
//////////////////////////
|
|
//Write the wheel params
|
|
//////////////////////////
|
|
|
|
writeWheelParams(baseParams.wheelParams, baseParams.axleDescription, writer);
|
|
|
|
writer.EndObject();
|
|
|
|
std::ofstream myfile;
|
|
myfile.open(std::string(directory) + "/" + filename);
|
|
myfile << strbuf.GetString() << std::endl;
|
|
myfile.close();
|
|
|
|
return true;
|
|
}
|
|
|
|
}//namespace snippetvehicle
|