From 564242d05c09fa2f85f53f323c7849832b264188 Mon Sep 17 00:00:00 2001 From: NewtonDynamics Date: Fri, 3 Jan 2025 08:29:16 -0800 Subject: [PATCH] re enabling vehicle (wip) --- .../ndSandbox/demos/ndBasicVehicle.cpp | 145 ++++++++---------- .../ndSandbox/toolbox/ndVehicleUI.cpp | 4 +- .../dModels/dVehicle/ndMultiBodyVehicle.cpp | 132 ++++++++-------- .../dModels/dVehicle/ndMultiBodyVehicle.h | 18 +-- .../dVehicle/ndMultiBodyVehicleMotor.cpp | 66 ++++---- .../dVehicle/ndMultiBodyVehicleMotor.h | 2 - .../sdk/dNewton/dModels/ndModelArticulation.h | 2 +- 7 files changed, 167 insertions(+), 202 deletions(-) diff --git a/newton-4.00/applications/ndSandbox/demos/ndBasicVehicle.cpp b/newton-4.00/applications/ndSandbox/demos/ndBasicVehicle.cpp index c66a46ec3..1a2e543c6 100644 --- a/newton-4.00/applications/ndSandbox/demos/ndBasicVehicle.cpp +++ b/newton-4.00/applications/ndSandbox/demos/ndBasicVehicle.cpp @@ -256,8 +256,9 @@ class ndBasicMultiBodyVehicle : public ndVehicleCommon ndDemoEntity* const vehicleEntity = vehicleEntityDummyRoot->GetFirstChild(); vehicleEntity->ResetMatrix(vehicleEntity->CalculateGlobalMatrix() * matrix); - ndPhysicsWorld* const world = scene->GetWorld(); + //ndPhysicsWorld* const world = scene->GetWorld(); + // 1- add chassis to the vehicle mode // create the vehicle chassis as a normal rigid body AddChassis(ndSharedPtr(CreateChassis(scene, vehicleEntity, m_configuration.m_chassisMass))); ndBodyDynamic* const chassis = m_chassis->GetAsBodyDynamic(); @@ -270,12 +271,7 @@ class ndBasicMultiBodyVehicle : public ndVehicleCommon com += m_localFrame.m_right.Scale(m_configuration.m_comDisplacement.m_z); chassis->SetCentreOfMass(com); - //// 1- add chassis to the vehicle mode - //SetChassis(chassis); - //ndSharedPtr bodyPtr(chassis); - //world->AddBody(bodyPtr); - - // 2- each tire to the model, + // 2- each tire // create the tire as a normal rigid body // and attach them to the chassis with a tire joints ndVehicleDectriptor::ndTireDefinition rr_tireConfiguration(m_configuration.m_rearTire); @@ -292,17 +288,9 @@ class ndBasicMultiBodyVehicle : public ndVehicleCommon ndMultiBodyVehicleTireJoint* const fr_tire = AddTire(fr_tireConfiguration, fr_tire_body); ndMultiBodyVehicleTireJoint* const fl_tire = AddTire(fl_tireConfiguration, fl_tire_body); - //ndSharedPtr rr_tire_body_Ptr(rr_tire_body); - //ndSharedPtr rl_tire_body_Ptr(rl_tire_body); - //ndSharedPtr fr_tire_body_Ptr(fr_tire_body); - //ndSharedPtr fl_tire_body_Ptr(fl_tire_body); - //world->AddBody(rr_tire_body_Ptr); - //world->AddBody(rl_tire_body_Ptr); - //world->AddBody(fr_tire_body_Ptr); - //world->AddBody(fl_tire_body_Ptr); - m_currentGear = sizeof(m_configuration.m_transmission.m_forwardRatios) / sizeof(m_configuration.m_transmission.m_forwardRatios[0]) + 1; + // 3- add differential // add the slip differential ndMultiBodyVehicleDifferential* differential = nullptr; switch (m_configuration.m_differentialType) @@ -333,78 +321,76 @@ class ndBasicMultiBodyVehicle : public ndVehicleCommon break; } } - ndTrace(("xxxxxxxxxxxxxxxxxxxxxxxxxxxx\n")); -#if 0 - // add a motor + + // 4- add a motor ndMultiBodyVehicleMotor* const motor = AddMotor(m_configuration.m_motorMass, m_configuration.m_motorRadius); motor->SetMaxRpm(m_configuration.m_engine.GetRedLineRadPerSec() * dRadPerSecToRpm); motor->SetFrictionLoss(m_configuration.m_engine.GetTorque(0.0f) * 0.5f); - // add the gear box + // 5- add the gear box ndMultiBodyVehicleGearBox* const gearBox = AddGearBox(differential); gearBox->SetIdleOmega(m_configuration.m_engine.GetIdleRadPerSec() * dRadPerSecToRpm); - switch (m_configuration.m_torsionBarType) - { - case ndVehicleDectriptor::m_noWheelAxle: - { - // no torsion bar - break; - } - - case ndVehicleDectriptor::m_rearWheelAxle: - { - ndMultiBodyVehicleTorsionBar* const torsionBar = AddTorsionBar(world->GetSentinelBody()); - torsionBar->AddAxel(rl_tire->GetBody0(), rr_tire->GetBody0()); - torsionBar->SetTorsionTorque(m_configuration.m_torsionBarSpringK, m_configuration.m_torsionBarDamperC, m_configuration.m_torsionBarRegularizer); - break; - } - - case ndVehicleDectriptor::m_frontWheelAxle: - { - ndMultiBodyVehicleTorsionBar* const torsionBar = AddTorsionBar(world->GetSentinelBody()); - torsionBar->AddAxel(fl_tire->GetBody0(), fr_tire->GetBody0()); - torsionBar->SetTorsionTorque(m_configuration.m_torsionBarSpringK, m_configuration.m_torsionBarDamperC, m_configuration.m_torsionBarRegularizer); - break; - } - - case ndVehicleDectriptor::m_fourWheelAxle: - { - ndMultiBodyVehicleTorsionBar* const torsionBar = AddTorsionBar(world->GetSentinelBody()); - torsionBar->AddAxel(rl_tire->GetBody0(), rr_tire->GetBody0()); - torsionBar->AddAxel(fl_tire->GetBody0(), fr_tire->GetBody0()); - torsionBar->SetTorsionTorque(m_configuration.m_torsionBarSpringK, m_configuration.m_torsionBarDamperC, m_configuration.m_torsionBarRegularizer); - break; - } - } + //switch (m_configuration.m_torsionBarType) + //{ + // case ndVehicleDectriptor::m_noWheelAxle: + // { + // // no torsion bar + // break; + // } + // + // case ndVehicleDectriptor::m_rearWheelAxle: + // { + // ndMultiBodyVehicleTorsionBar* const torsionBar = AddTorsionBar(world->GetSentinelBody()); + // torsionBar->AddAxel(rl_tire->GetBody0(), rr_tire->GetBody0()); + // torsionBar->SetTorsionTorque(m_configuration.m_torsionBarSpringK, m_configuration.m_torsionBarDamperC, m_configuration.m_torsionBarRegularizer); + // break; + // } + // + // case ndVehicleDectriptor::m_frontWheelAxle: + // { + // ndMultiBodyVehicleTorsionBar* const torsionBar = AddTorsionBar(world->GetSentinelBody()); + // torsionBar->AddAxel(fl_tire->GetBody0(), fr_tire->GetBody0()); + // torsionBar->SetTorsionTorque(m_configuration.m_torsionBarSpringK, m_configuration.m_torsionBarDamperC, m_configuration.m_torsionBarRegularizer); + // break; + // } + // + // case ndVehicleDectriptor::m_fourWheelAxle: + // { + // ndMultiBodyVehicleTorsionBar* const torsionBar = AddTorsionBar(world->GetSentinelBody()); + // torsionBar->AddAxel(rl_tire->GetBody0(), rr_tire->GetBody0()); + // torsionBar->AddAxel(fl_tire->GetBody0(), fr_tire->GetBody0()); + // torsionBar->SetTorsionTorque(m_configuration.m_torsionBarSpringK, m_configuration.m_torsionBarDamperC, m_configuration.m_torsionBarRegularizer); + // break; + // } + //} // set a soft or hard mode SetVehicleSolverModel(m_configuration.m_useHardSolverMode ? true : false); - // prepare data for animating suspension - m_rearAxlePivot = vehicleEntity->Find("rearAxlePivot"); - m_frontAxlePivot = vehicleEntity->Find("frontAxlePivot"); - m_rr_tire = rr_tire; - m_rl_tire = rl_tire; - m_fr_tire = fr_tire; - m_fl_tire = fl_tire; - - // load all engine sound channels - ndSoundManager* const soundManager = world->GetSoundManager(); - - m_startEngine = false; - m_startEngineMemory = false; - m_startSound = soundManager->CreateSoundChannel(engineSounds[0]); - m_engineRpmSound = soundManager->CreateSoundChannel(engineSounds[1]); - m_skipMarks = soundManager->CreateSoundChannel(engineSounds[2]); - - m_startSound->SetAttenuationRefDistance(20.0f, 40.0f, 50.0f); - m_engineRpmSound->SetAttenuationRefDistance(20.0f, 40.0f, 50.0f); - - m_engineRpmSound->SetLoop(true); - m_skipMarks->SetLoop(true); -#endif + //// prepare data for animating suspension + //m_rearAxlePivot = vehicleEntity->Find("rearAxlePivot"); + //m_frontAxlePivot = vehicleEntity->Find("frontAxlePivot"); + //m_rr_tire = rr_tire; + //m_rl_tire = rl_tire; + //m_fr_tire = fr_tire; + //m_fl_tire = fl_tire; + // + //// load all engine sound channels + //ndSoundManager* const soundManager = world->GetSoundManager(); + // + //m_startEngine = false; + //m_startEngineMemory = false; + //m_startSound = soundManager->CreateSoundChannel(engineSounds[0]); + //m_engineRpmSound = soundManager->CreateSoundChannel(engineSounds[1]); + //m_skipMarks = soundManager->CreateSoundChannel(engineSounds[2]); + // + //m_startSound->SetAttenuationRefDistance(20.0f, 40.0f, 50.0f); + //m_engineRpmSound->SetAttenuationRefDistance(20.0f, 40.0f, 50.0f); + // + //m_engineRpmSound->SetLoop(true); + //m_skipMarks->SetLoop(true); } ~ndBasicMultiBodyVehicle() @@ -464,8 +450,6 @@ class ndBasicMultiBodyVehicle : public ndVehicleCommon void SetCamera(ndDemoEntityManager* const manager, ndFloat32) { - ndAssert(0); -#if 0 ndDemoCamera* const camera = manager->GetCamera(); ndDemoEntity* const chassisEntity = (ndDemoEntity*)m_chassis->GetNotifyCallback()->GetUserData(); ndMatrix camMatrix(camera->GetNextMatrix()); @@ -477,7 +461,6 @@ class ndBasicMultiBodyVehicle : public ndVehicleCommon camOrigin -= frontDir.Scale(10.0f); camera->SetNextMatrix(camMatrix, camOrigin); -#endif } void ApplyInputs(ndWorld* const, ndFloat32) @@ -712,8 +695,8 @@ void ndBasicVehicle (ndDemoEntityManager* const scene) //world->AddModel(vehicle3); //ndBasicMultiBodyVehicle* const vehicle = (ndBasicMultiBodyVehicle*)*vehicle3; - //ndBasicMultiBodyVehicle* const vehicle = (ndBasicMultiBodyVehicle*)*vehicle0; - //vehicle->SetAsPlayer(scene); + ndBasicMultiBodyVehicle* const vehicle = (ndBasicMultiBodyVehicle*)*vehicle0; + vehicle->SetAsPlayer(scene); matrix.m_posit.m_x += 5.0f; TestPlayerCapsuleInteraction(scene, matrix); diff --git a/newton-4.00/applications/ndSandbox/toolbox/ndVehicleUI.cpp b/newton-4.00/applications/ndSandbox/toolbox/ndVehicleUI.cpp index 8295251ba..9d1b340e6 100644 --- a/newton-4.00/applications/ndSandbox/toolbox/ndVehicleUI.cpp +++ b/newton-4.00/applications/ndSandbox/toolbox/ndVehicleUI.cpp @@ -438,8 +438,6 @@ void ndVehicleUI::RenderUI() { return; } - glColor4f(1.0f, 1.0f, 1.0f, 1.0f); - ndAssert(0); //ndFloat32 gageSize = 200.0f; //ndFloat32 y = (ndFloat32)m_scene->GetHeight() - (gageSize / 2.0f + 20.0f); @@ -448,7 +446,7 @@ void ndVehicleUI::RenderUI() ndFloat32 maxRpm = m_vehicle->m_configuration.m_engine.GetRedLineRadPerSec() * dRadPerSecToRpm; maxRpm += 500.0f; - ndAssert(0); + //ndAssert(0); //ndFloat32 rpm = (m_vehicle->m_motor->GetRpm() / maxRpm) * 2.85f; // //glUseProgram(m_shaderHandle); diff --git a/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicle.cpp b/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicle.cpp index f5f99b715..90c7e1885 100644 --- a/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicle.cpp +++ b/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicle.cpp @@ -134,31 +134,6 @@ ndMultiBodyVehicleTireJoint* ndMultiBodyVehicle::AddAxleTire(const ndMultiBodyVe //return *tireJoint; } - -//ndMultiBodyVehicleMotor* ndMultiBodyVehicle::AddMotor(ndFloat32 mass, ndFloat32 radius) -ndMultiBodyVehicleMotor* ndMultiBodyVehicle::AddMotor(ndFloat32, ndFloat32) -{ - ndAssert(0); - return nullptr; - - //ndAssert(m_chassis); - //ndSharedPtr motorBody (CreateInternalBodyPart(mass, radius)); - //m_internalBodies.Append(motorBody); - //m_motor = ndSharedPtr(new ndMultiBodyVehicleMotor(motorBody->GetAsBodyKinematic(), this)); - //return *m_motor; -} - -//ndMultiBodyVehicleGearBox* ndMultiBodyVehicle::AddGearBox(ndMultiBodyVehicleDifferential* const differential) -ndMultiBodyVehicleGearBox* ndMultiBodyVehicle::AddGearBox(ndMultiBodyVehicleDifferential* const) -{ - ndAssert(0); - return nullptr; - - //ndAssert(*m_motor); - //m_gearBox = ndSharedPtr(new ndMultiBodyVehicleGearBox(m_motor->GetBody0(), differential->GetBody0(), this)); - //return *m_gearBox; -} - //ndMultiBodyVehicleTorsionBar* ndMultiBodyVehicle::AddTorsionBar(ndBodyKinematic* const sentinel) ndMultiBodyVehicleTorsionBar* ndMultiBodyVehicle::AddTorsionBar(ndBodyKinematic* const) { @@ -195,49 +170,6 @@ void ndMultiBodyVehicle::ApplyAerodynamics() //} } -//void ndMultiBodyVehicle::SetVehicleSolverModel(bool hardJoint) -void ndMultiBodyVehicle::SetVehicleSolverModel(bool) -{ - ndAssert(0); - - //ndJointBilateralSolverModel openLoopMode = hardJoint ? m_jointkinematicOpenLoop : m_jointIterativeSoft; - // - //ndAssert(m_chassis); - //const ndBodyKinematic::ndJointList& chassisJoints = m_chassis->GetJointList(); - //for (ndBodyKinematic::ndJointList::ndNode* node = chassisJoints.GetFirst(); node; node = node->GetNext()) - //{ - // ndJointBilateralConstraint* const joint = node->GetInfo(); - // const char* const className = joint->ClassName(); - // if (!strcmp(className, "ndMultiBodyVehicleTireJoint") || - // !strcmp(className, "ndMultiBodyVehicleDifferential") || - // !strcmp(className, "ndMultiBodyVehicleMotor")) - // { - // joint->SetSolverModel(openLoopMode); - // } - //} - // - //ndJointBilateralSolverModel driveTrainMode = hardJoint ? m_jointkinematicCloseLoop : m_jointIterativeSoft; - //for (ndReferencedObjects::ndNode* node = m_differentialList.GetFirst(); node; node = node->GetNext()) - //{ - // ndJointBilateralConstraint* const joint = *node->GetInfo(); - // const ndBodyKinematic::ndJointList& jointList = joint->GetBody0()->GetJointList(); - // for (ndBodyKinematic::ndJointList::ndNode* node1 = jointList.GetFirst(); node1; node1 = node1->GetNext()) - // { - // ndJointBilateralConstraint* const axle = node1->GetInfo(); - // const char* const clasName = axle->ClassName(); - // if (strcmp(clasName, "ndMultiBodyVehicleDifferential")) - // { - // axle->SetSolverModel(driveTrainMode); - // } - // } - //} - // - //if (*m_torsionBar) - //{ - // m_torsionBar->SetSolverModel(driveTrainMode); - //} -} - void ndMultiBodyVehicle::ApplyalignmentAndBalancing() { ndAssert(0); @@ -948,6 +880,11 @@ ndMultiBodyVehicle::ndMultiBodyVehicle(const ndVector& frontDir, const ndVector& ////,m_differentialList() //,m_downForce() { + m_motor = nullptr; + m_gearBox = nullptr; + m_chassis = nullptr; + m_torsionBar = nullptr; + m_tireShape->AddRef(); m_localFrame.m_front = (frontDir & ndVector::m_triplexMask).Normalize(); m_localFrame.m_up = upDir & ndVector::m_triplexMask; @@ -967,6 +904,42 @@ void ndMultiBodyVehicle::AddChassis(const ndSharedPtr& chassis) ndAssert(m_chassis); } +void ndMultiBodyVehicle::SetVehicleSolverModel(bool hardJoint) +{ + ndAssert(m_chassis); + ndJointBilateralSolverModel openLoopMode = hardJoint ? m_jointkinematicOpenLoop : m_jointIterativeSoft; + for (ndNode* node = GetRoot()->GetFirstChild(); node; node = node->GetNext()) + { + ndJointBilateralConstraint* const joint = *node->m_joint; + const char* const className = joint->ClassName(); + if (!strcmp(className, "ndMultiBodyVehicleTireJoint") || + !strcmp(className, "ndMultiBodyVehicleDifferential") || + !strcmp(className, "ndMultiBodyVehicleMotor")) + { + joint->SetSolverModel(openLoopMode); + } + } + + ndJointBilateralSolverModel driveTrainMode = hardJoint ? m_jointkinematicCloseLoop : m_jointIterativeSoft; + for (ndList::ndNode* node = m_closeLoops.GetFirst(); node; node = node->GetNext()) + { + ndModelArticulation::ndNode& closeLoop = node->GetInfo(); + ndJointBilateralConstraint* const joint = *closeLoop.m_joint; + const char* const clasName = joint->ClassName(); + if (strcmp(clasName, "ndMultiBodyVehicleDifferential") || strcmp(clasName, "ndMultiBodyVehicleGearBox")) + { + joint->SetSolverModel(driveTrainMode); + } + } + + if (m_torsionBar) + { + ndAssert(0); + //m_torsionBar->SetSolverModel(driveTrainMode); + } +} + + ndMultiBodyVehicleTireJoint* ndMultiBodyVehicle::AddTire(const ndMultiBodyVehicleTireJointInfo& desc, const ndSharedPtr& tire) { ndAssert(m_chassis); @@ -1049,7 +1022,6 @@ ndMultiBodyVehicleDifferential* ndMultiBodyVehicle::AddDifferential(ndFloat32 ma return (ndMultiBodyVehicleDifferential*) *differential; } - ndMultiBodyVehicleDifferential* ndMultiBodyVehicle::AddDifferential(ndFloat32 mass, ndFloat32 radius, ndMultiBodyVehicleDifferential* const leftDifferential, ndMultiBodyVehicleDifferential* const rightDifferential, ndFloat32 slipOmegaLock) { ndAssert(m_chassis); @@ -1079,3 +1051,25 @@ ndMultiBodyVehicleDifferential* ndMultiBodyVehicle::AddDifferential(ndFloat32 ma AddCloseLoop(rightAxle); return (ndMultiBodyVehicleDifferential*)*differential; } + +ndMultiBodyVehicleMotor* ndMultiBodyVehicle::AddMotor(ndFloat32 mass, ndFloat32 radius) +{ + ndAssert(m_chassis); + ndSharedPtr motorBody (CreateInternalBodyPart(mass, radius)); + //m_internalBodies.Append(motorBody); + //m_motor = ndSharedPtr(new ndMultiBodyVehicleMotor(motorBody->GetAsBodyKinematic(), this)); + ndSharedPtr motorJoint(new ndMultiBodyVehicleMotor(motorBody->GetAsBodyKinematic(), this)); + AddLimb(GetRoot(), motorBody, motorJoint); + m_motor = (ndMultiBodyVehicleMotor*)*motorJoint; + return m_motor; +} + +ndMultiBodyVehicleGearBox* ndMultiBodyVehicle::AddGearBox(ndMultiBodyVehicleDifferential* const differential) +{ + ndAssert(m_motor); + //m_gearBox = ndSharedPtr(new ndMultiBodyVehicleGearBox(m_motor->GetBody0(), differential->GetBody0(), this)); + ndSharedPtr gearBox(new ndMultiBodyVehicleGearBox(m_motor->GetBody0(), differential->GetBody0(), this)); + AddCloseLoop(gearBox); + m_gearBox = (ndMultiBodyVehicleGearBox*)*gearBox; + return m_gearBox; +} diff --git a/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicle.h b/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicle.h index d91f696d1..89402d03c 100644 --- a/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicle.h +++ b/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicle.h @@ -79,10 +79,11 @@ class ndMultiBodyVehicle : public ndModelArticulation D_NEWTON_API ndMultiBodyVehicle(const ndVector& frontDir, const ndVector& upDir); D_NEWTON_API virtual ~ndMultiBodyVehicle (); - + D_NEWTON_API void SetVehicleSolverModel(bool hardJoint); D_NEWTON_API void AddChassis(const ndSharedPtr& chassis); - + D_NEWTON_API ndMultiBodyVehicleMotor* AddMotor(ndFloat32 mass, ndFloat32 radius); D_NEWTON_API ndShapeInstance CreateTireShape(ndFloat32 radius, ndFloat32 width) const; + D_NEWTON_API ndMultiBodyVehicleGearBox* AddGearBox(ndMultiBodyVehicleDifferential* const differential); D_NEWTON_API ndMultiBodyVehicleTireJoint* AddTire(const ndMultiBodyVehicleTireJointInfo& desc, const ndSharedPtr& tire); D_NEWTON_API ndMultiBodyVehicleDifferential* AddDifferential(ndFloat32 mass, ndFloat32 radius, ndMultiBodyVehicleTireJoint* const leftTire, ndMultiBodyVehicleTireJoint* const rightTire, ndFloat32 slipOmegaLock); D_NEWTON_API ndMultiBodyVehicleDifferential* AddDifferential(ndFloat32 mass, ndFloat32 radius, ndMultiBodyVehicleDifferential* const leftDifferential, ndMultiBodyVehicleDifferential* const rightDifferential, ndFloat32 slipOmegaLock); @@ -92,17 +93,9 @@ class ndMultiBodyVehicle : public ndModelArticulation virtual void OnRemoveFromToWorld() { ndAssert(0); } D_NEWTON_API ndFloat32 GetSpeed() const; - - - D_NEWTON_API ndMultiBodyVehicleMotor* AddMotor(ndFloat32 mass, ndFloat32 radius); - D_NEWTON_API ndMultiBodyVehicleGearBox* AddGearBox(ndMultiBodyVehicleDifferential* const differential); - D_NEWTON_API ndMultiBodyVehicleTireJoint* AddAxleTire(const ndMultiBodyVehicleTireJointInfo& desc, ndBodyKinematic* const tire, ndBodyKinematic* const axleBody); D_NEWTON_API ndMultiBodyVehicleTorsionBar* AddTorsionBar(ndBodyKinematic* const sentinel); - - D_NEWTON_API void SetVehicleSolverModel(bool hardJoint); - D_NEWTON_API ndMultiBodyVehicle* GetAsMultiBodyVehicle(); private: @@ -134,8 +127,11 @@ class ndMultiBodyVehicle : public ndModelArticulation protected: ndMatrix m_localFrame; ndBodyDynamic* m_chassis; + ndMultiBodyVehicleMotor* m_motor; //ndIkSolver m_invDynamicsSolver; ndShapeChamferCylinder* m_tireShape; + ndMultiBodyVehicleGearBox* m_gearBox; + ndMultiBodyVehicleTorsionBar* m_torsionBar; //ndReferencedObjects m_internalBodies; //ndSharedPtr m_motor; @@ -146,7 +142,7 @@ class ndMultiBodyVehicle : public ndModelArticulation //ndReferencedObjects m_differentialList; ndDownForce m_downForce; - //friend class ndMultiBodyVehicleMotor; + friend class ndMultiBodyVehicleMotor; //friend class ndMultiBodyVehicleGearBox; friend class ndMultiBodyVehicleTireJoint; //friend class ndMultiBodyVehicleTorsionBar; diff --git a/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicleMotor.cpp b/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicleMotor.cpp index 8a28fca07..dc149504d 100644 --- a/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicleMotor.cpp +++ b/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicleMotor.cpp @@ -27,7 +27,7 @@ #include "ndMultiBodyVehicleMotor.h" #include "ndMultiBodyVehicleGearBox.h" -#if 0 + ndMultiBodyVehicleMotor::ndMultiBodyVehicleMotor() :ndJointBilateralConstraint() ,m_omega(ndFloat32(0.0f)) @@ -120,39 +120,35 @@ ndFloat32 ndMultiBodyVehicleMotor::CalculateAcceleration(ndConstraintDescritor& return accel; } -//void ndMultiBodyVehicleMotor::JacobianDerivative(ndConstraintDescritor& desc) -void ndMultiBodyVehicleMotor::JacobianDerivative(ndConstraintDescritor&) +void ndMultiBodyVehicleMotor::JacobianDerivative(ndConstraintDescritor& desc) { - ndAssert(0); - //ndMatrix matrix0; - //ndMatrix matrix1; - //CalculateGlobalMatrix(matrix0, matrix1); - // - //// two rows to restrict rotation around around the parent coordinate system - //const ndFloat32 angle0 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up); - //const ndFloat32 angle1 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right); - // - //AddAngularRowJacobian(desc, matrix1.m_up, angle0); - //AddAngularRowJacobian(desc, matrix1.m_right, angle1); - // - //// add rotor joint acceleration - //AddAngularRowJacobian(desc, matrix0.m_front * ndVector::m_negOne, ndFloat32(0.0f)); - // - //const ndFloat32 accel = CalculateAcceleration(desc); - //const ndFloat32 torque = ndMax(m_engineTorque, m_internalFriction); - //SetMotorAcceleration(desc, accel); - //SetHighFriction(desc, torque); - //SetLowerFriction(desc, -m_internalFriction); - //SetDiagonalRegularizer(desc, ndFloat32(0.1f)); - // - //// add torque coupling to chassis. - //const ndMultiBodyVehicleGearBox* const gearBox = *m_vehicelModel->m_gearBox; - //ndAssert(gearBox); - //if (gearBox && ndAbs(gearBox->GetRatio()) > ndFloat32(0.0f)) - //{ - // ndJacobian& chassisJacobian = desc.m_jacobian[desc.m_rowsCount - 1].m_jacobianM1; - // chassisJacobian.m_angular = ndVector::m_zero; - //} + ndMatrix matrix0; + ndMatrix matrix1; + CalculateGlobalMatrix(matrix0, matrix1); + + // two rows to restrict rotation around around the parent coordinate system + const ndFloat32 angle0 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up); + const ndFloat32 angle1 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right); + + AddAngularRowJacobian(desc, matrix1.m_up, angle0); + AddAngularRowJacobian(desc, matrix1.m_right, angle1); + + // add rotor joint acceleration + AddAngularRowJacobian(desc, matrix0.m_front * ndVector::m_negOne, ndFloat32(0.0f)); + + const ndFloat32 accel = CalculateAcceleration(desc); + const ndFloat32 torque = ndMax(m_engineTorque, m_internalFriction); + SetMotorAcceleration(desc, accel); + SetHighFriction(desc, torque); + SetLowerFriction(desc, -m_internalFriction); + SetDiagonalRegularizer(desc, ndFloat32(0.1f)); + + // add torque coupling to chassis. + ndMultiBodyVehicleGearBox* const gearBox = m_vehicelModel->m_gearBox; + ndAssert(gearBox); + if (gearBox && ndAbs(gearBox->GetRatio()) > ndFloat32(0.0f)) + { + ndJacobian& chassisJacobian = desc.m_jacobian[desc.m_rowsCount - 1].m_jacobianM1; + chassisJacobian.m_angular = ndVector::m_zero; + } } - -#endif diff --git a/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicleMotor.h b/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicleMotor.h index c83600666..17c45df96 100644 --- a/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicleMotor.h +++ b/newton-4.00/sdk/dNewton/dModels/dVehicle/ndMultiBodyVehicleMotor.h @@ -27,7 +27,6 @@ class ndMultiBodyVehicle; -#if 0 class ndMultiBodyVehicleMotor: public ndJointBilateralConstraint { public: @@ -60,5 +59,4 @@ class ndMultiBodyVehicleMotor: public ndJointBilateralConstraint friend class ndMultiBodyVehicle; friend class ndMultiBodyVehicleGearBox; }; -#endif #endif \ No newline at end of file diff --git a/newton-4.00/sdk/dNewton/dModels/ndModelArticulation.h b/newton-4.00/sdk/dNewton/dModels/ndModelArticulation.h index 5e119ba13..e68133923 100644 --- a/newton-4.00/sdk/dNewton/dModels/ndModelArticulation.h +++ b/newton-4.00/sdk/dNewton/dModels/ndModelArticulation.h @@ -69,7 +69,7 @@ class ndModelArticulation: public ndModel D_NEWTON_API void AddToWorld(ndWorld* const world); D_NEWTON_API void SetTransform(const ndMatrix& matrix); - private: + protected: D_NEWTON_API void ConvertToUrdf(); ndString m_name;