From e0555f1a574a47a8576b8252b7021395a7311c48 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 8 Feb 2021 21:42:26 +0100 Subject: [PATCH] Add methods Joint::getReactionForce() and Joint::getReactionTorque() to retrieve the reaction force and torque of a joint --- CHANGELOG.md | 8 ++++-- .../components/BallAndSocketJointComponents.h | 2 +- .../components/HingeJointComponents.h | 1 + .../components/SliderJointComponents.h | 1 + .../constraint/BallAndSocketJoint.h | 6 +++++ .../reactphysics3d/constraint/FixedJoint.h | 6 +++++ .../reactphysics3d/constraint/HingeJoint.h | 6 +++++ include/reactphysics3d/constraint/Joint.h | 6 +++++ .../reactphysics3d/constraint/SliderJoint.h | 6 +++++ src/constraint/BallAndSocketJoint.cpp | 11 ++++++++ src/constraint/FixedJoint.cpp | 12 +++++++++ src/constraint/HingeJoint.cpp | 25 +++++++++++++++++ src/constraint/SliderJoint.cpp | 27 +++++++++++++++++++ src/systems/SolveSliderJointSystem.cpp | 4 +-- 14 files changed, 116 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a75803d6..d20ad340 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,18 +10,22 @@ do not hesitate to take a look at the user manual. - Method RigidBody::resetForce() to reset the accumulated external force on a rigid body has been added - Method RigidBody::resetTorque() to reset the accumulated external torque on a rigid body has been added - Constructors with local-space anchor/axis have been added to BallAndSocketJointInfo, HingeJointInfo, FixedJointInfo and SliderJointInfo classes - - Robustness of polyhedron vs polyhedron collision detection has been improved in SAT algorithm (face contacts are favored over edge-edge contacts for better stability) - Method HingeJoint::getAngle() to get the current angle of the hinge joint has been added + - Method Joint::getReactionForce() has been added to retrieve the current reaction force of a joint + - Method Joint::getReactionTorque() has been added to retrieve the current reaction torque of a joint + - Method RigidBody::setLinearLockAxisFactor() to lock the translational movement of a body along the world-space x, y and z axes + - Method RigidBody::setAngularLockAxisFactor() to lock the rotational movement of a body around the world-space x, y and z axes ### Changed - The PhysicsWorld::setGravity() method now takes a const parameter - - Rolling resistance constraint is not solved anymore in the solver. Angular damping needs to be used instead to simulate this + - Rolling resistance constraint is not solved anymore in the solver. Angular damping needs to be used instead to simulate it. - The List class has been renamed to Array - The default number of iterations for the velocity solver is now 6 instead of 10 - The default number of iterations for the position solver is now 3 instead of 5 - The raycasting broad-phase performance has been improved - The raycasting performance against HeighFieldShape has been improved (better middle-phase algorithm) + - Robustness of polyhedron vs polyhedron collision detection has been improved in SAT algorithm (face contacts are favored over edge-edge contacts for better stability) ### Removed diff --git a/include/reactphysics3d/components/BallAndSocketJointComponents.h b/include/reactphysics3d/components/BallAndSocketJointComponents.h index 75ec4081..34cfc436 100644 --- a/include/reactphysics3d/components/BallAndSocketJointComponents.h +++ b/include/reactphysics3d/components/BallAndSocketJointComponents.h @@ -314,7 +314,7 @@ RP3D_FORCE_INLINE void BallAndSocketJointComponents::setInverseMassMatrix(Entity } // Return the accumulated impulse -RP3D_FORCE_INLINE Vector3 &BallAndSocketJointComponents::getImpulse(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& BallAndSocketJointComponents::getImpulse(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulse[mMapEntityToComponentIndex[jointEntity]]; diff --git a/include/reactphysics3d/components/HingeJointComponents.h b/include/reactphysics3d/components/HingeJointComponents.h index 4ee9402f..7ade0356 100644 --- a/include/reactphysics3d/components/HingeJointComponents.h +++ b/include/reactphysics3d/components/HingeJointComponents.h @@ -412,6 +412,7 @@ class HingeJointComponents : public Components { friend class BroadPhaseSystem; friend class SolveHingeJointSystem; + friend class HingeJoint; }; // Return a pointer to a given joint diff --git a/include/reactphysics3d/components/SliderJointComponents.h b/include/reactphysics3d/components/SliderJointComponents.h index 56672259..819008c2 100644 --- a/include/reactphysics3d/components/SliderJointComponents.h +++ b/include/reactphysics3d/components/SliderJointComponents.h @@ -457,6 +457,7 @@ class SliderJointComponents : public Components { friend class BroadPhaseSystem; friend class SolveSliderJointSystem; + friend class SliderJoint; }; // Return a pointer to a given joint diff --git a/include/reactphysics3d/constraint/BallAndSocketJoint.h b/include/reactphysics3d/constraint/BallAndSocketJoint.h index ee671664..4343426a 100644 --- a/include/reactphysics3d/constraint/BallAndSocketJoint.h +++ b/include/reactphysics3d/constraint/BallAndSocketJoint.h @@ -121,6 +121,12 @@ class BallAndSocketJoint : public Joint { /// Deleted copy-constructor BallAndSocketJoint(const BallAndSocketJoint& constraint) = delete; + /// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space + virtual Vector3 getReactionForce(decimal timeStep) const override; + + /// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space + virtual Vector3 getReactionTorque(decimal timeStep) const override; + /// Return a string representation virtual std::string to_string() const override; diff --git a/include/reactphysics3d/constraint/FixedJoint.h b/include/reactphysics3d/constraint/FixedJoint.h index fa2ac350..02b990f7 100644 --- a/include/reactphysics3d/constraint/FixedJoint.h +++ b/include/reactphysics3d/constraint/FixedJoint.h @@ -111,6 +111,12 @@ class FixedJoint : public Joint { /// Deleted copy-constructor FixedJoint(const FixedJoint& constraint) = delete; + /// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space + virtual Vector3 getReactionForce(decimal timeStep) const override; + + /// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space + virtual Vector3 getReactionTorque(decimal timeStep) const override; + /// Return a string representation virtual std::string to_string() const override; diff --git a/include/reactphysics3d/constraint/HingeJoint.h b/include/reactphysics3d/constraint/HingeJoint.h index d8c85134..664dce8e 100644 --- a/include/reactphysics3d/constraint/HingeJoint.h +++ b/include/reactphysics3d/constraint/HingeJoint.h @@ -314,6 +314,12 @@ class HingeJoint : public Joint { /// Return the current hinge angle decimal getAngle() const; + /// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space + virtual Vector3 getReactionForce(decimal timeStep) const override; + + /// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space + virtual Vector3 getReactionTorque(decimal timeStep) const override; + /// Return a string representation virtual std::string to_string() const override; }; diff --git a/include/reactphysics3d/constraint/Joint.h b/include/reactphysics3d/constraint/Joint.h index dc16d311..fa87b76b 100644 --- a/include/reactphysics3d/constraint/Joint.h +++ b/include/reactphysics3d/constraint/Joint.h @@ -137,6 +137,12 @@ class Joint { /// Return the type of the constraint JointType getType() const; + /// Return the force (in Newtons) on body 2 required to satisfy the joint constraint + virtual Vector3 getReactionForce(decimal timeStep) const=0; + + /// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint + virtual Vector3 getReactionTorque(decimal timeStep) const=0; + /// Return true if the collision between the two bodies of the joint is enabled bool isCollisionEnabled() const; diff --git a/include/reactphysics3d/constraint/SliderJoint.h b/include/reactphysics3d/constraint/SliderJoint.h index 517716a9..127596f0 100644 --- a/include/reactphysics3d/constraint/SliderJoint.h +++ b/include/reactphysics3d/constraint/SliderJoint.h @@ -304,6 +304,12 @@ class SliderJoint : public Joint { /// Return the intensity of the current force applied for the joint motor decimal getMotorForce(decimal timeStep) const; + /// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space + virtual Vector3 getReactionForce(decimal timeStep) const override; + + /// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space + virtual Vector3 getReactionTorque(decimal timeStep) const override; + /// Return a string representation virtual std::string to_string() const override; }; diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index fba5cc09..87a26520 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -61,6 +61,17 @@ BallAndSocketJoint::BallAndSocketJoint(Entity entity, PhysicsWorld& world, const mWorld.mBallAndSocketJointsComponents.setLocalAnchorPointBody2(entity, anchorPointBody2LocalSpace); } +// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space +Vector3 BallAndSocketJoint::getReactionForce(decimal timeStep) const { + assert(timeStep > MACHINE_EPSILON); + return mWorld.mBallAndSocketJointsComponents.getImpulse(mEntity) / timeStep; +} + +// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space +Vector3 BallAndSocketJoint::getReactionTorque(decimal timeStep) const { + assert(timeStep > MACHINE_EPSILON); + return Vector3(0, 0, 0); +} // Return a string representation std::string BallAndSocketJoint::to_string() const { diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 5072d16c..ac7f7f86 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -71,6 +71,18 @@ FixedJoint::FixedJoint(Entity entity, PhysicsWorld& world, const FixedJointInfo& mWorld.mFixedJointsComponents.setInitOrientationDifferenceInv(mEntity, transform2.getOrientation().getInverse() * transform1.getOrientation()); } +// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space +Vector3 FixedJoint::getReactionForce(decimal timeStep) const { + assert(timeStep > MACHINE_EPSILON); + return mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity) / timeStep; +} + +// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space +Vector3 FixedJoint::getReactionTorque(decimal timeStep) const { + assert(timeStep > MACHINE_EPSILON); + return mWorld.mFixedJointsComponents.getImpulseRotation(mEntity) / timeStep; +} + // Return a string representation std::string FixedJoint::to_string() const { return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() + diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index b1542096..e520dce9 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -266,6 +266,31 @@ decimal HingeJoint::getAngle() const { return mWorld.mConstraintSolverSystem.mSolveHingeJointSystem.computeCurrentHingeAngle(mEntity, orientationBody1, orientationBody2); } +// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space +Vector3 HingeJoint::getReactionForce(decimal timeStep) const { + assert(timeStep > MACHINE_EPSILON); + return mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity) / timeStep; +} + +// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space +Vector3 HingeJoint::getReactionTorque(decimal timeStep) const { + + assert(timeStep > MACHINE_EPSILON); + + const uint32 jointIndex = mWorld.mHingeJointsComponents.getEntityIndex(mEntity); + + const Vector2& impulseRotation = mWorld.mHingeJointsComponents.mImpulseRotation[jointIndex]; + const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.mB2CrossA1[jointIndex]; + const Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.mC2CrossA1[jointIndex]; + + Vector3 impulseJoint = b2CrossA1 * impulseRotation.x + c2CrossA1 * impulseRotation.y; + Vector3 impulseLowerLimit = mWorld.mHingeJointsComponents.mImpulseLowerLimit[jointIndex] * mWorld.mHingeJointsComponents.mA1[jointIndex]; + Vector3 impulseUpperLimit = -mWorld.mHingeJointsComponents.mImpulseUpperLimit[jointIndex] * mWorld.mHingeJointsComponents.mA1[jointIndex]; + Vector3 impulseMotor = mWorld.mHingeJointsComponents.mImpulseMotor[jointIndex] * mWorld.mHingeJointsComponents.mA1[jointIndex]; + + return (impulseJoint + impulseLowerLimit + impulseUpperLimit + impulseMotor) / timeStep; +} + // Return the number of bytes used by the joint size_t HingeJoint::getSizeInBytes() const { return sizeof(HingeJoint); diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index d22d6ecb..d11a1794 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -293,6 +293,33 @@ decimal SliderJoint::getMaxMotorForce() const { decimal SliderJoint::getMaxTranslationLimit() const { return mWorld.mSliderJointsComponents.getUpperLimit(mEntity); } + +// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space +Vector3 SliderJoint::getReactionForce(decimal timeStep) const { + assert(timeStep > MACHINE_EPSILON); + + const uint32 jointIndex = mWorld.mSliderJointsComponents.getEntityIndex(mEntity); + + const Vector2 translationImpulse = mWorld.mSliderJointsComponents.mImpulseTranslation[jointIndex]; + const Vector3& n1 = mWorld.mSliderJointsComponents.mN1[jointIndex]; + const Vector3& n2 = mWorld.mSliderJointsComponents.mN2[jointIndex]; + const Vector3 impulseJoint = n1 * translationImpulse.x + n2 * translationImpulse.y; + + const Vector3& sliderAxisWorld = mWorld.mSliderJointsComponents.mSliderAxisWorld[jointIndex]; + const Vector3 impulseLowerLimit = mWorld.mSliderJointsComponents.mImpulseLowerLimit[jointIndex] * sliderAxisWorld; + const Vector3 impulseUpperLimit = -mWorld.mSliderJointsComponents.mImpulseUpperLimit[jointIndex] * sliderAxisWorld; + Vector3 impulseMotor = -mWorld.mSliderJointsComponents.mImpulseMotor[jointIndex] * sliderAxisWorld; + + return (impulseJoint + impulseLowerLimit + impulseUpperLimit + impulseMotor) / timeStep; +} + +// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space +Vector3 SliderJoint::getReactionTorque(decimal timeStep) const { + + assert(timeStep > MACHINE_EPSILON); + return mWorld.mSliderJointsComponents.getImpulseRotation(mEntity) / timeStep; +} + // Return a string representation std::string SliderJoint::to_string() const { return "SliderJoint{ lowerLimit=" + std::to_string(mWorld.mSliderJointsComponents.getLowerLimit(mEntity)) + ", upperLimit=" + std::to_string(mWorld.mSliderJointsComponents.getUpperLimit(mEntity)) + diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index e2726174..7077c305 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -367,7 +367,7 @@ void SolveSliderJointSystem::solveVelocityConstraint() { const Vector2 JvTranslation(el1, el2); // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 deltaLambda = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-JvTranslation - mSliderJointComponents.mBiasTranslation[i]); + const Vector2 deltaLambda = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-JvTranslation - mSliderJointComponents.mBiasTranslation[i]); mSliderJointComponents.mImpulseTranslation[i] += deltaLambda; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 @@ -380,7 +380,7 @@ void SolveSliderJointSystem::solveVelocityConstraint() { w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const Vector3 linearImpulseBody2 = n1 * deltaLambda.x + n2 * deltaLambda.y; + const Vector3 linearImpulseBody2 = -linearImpulseBody1; Vector3 angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y; // Apply the impulse to the body 2