Hinge Joint Fix + New Testbed Scene

.gitignore

@@ -7,6 +7,10 @@
 *.o
 *.so
 
+# The Build Folder #
+######################
+/build/
+
 # Logs #
 ######################
 *.log

src/body/RigidBody.cpp

@@ -76,6 +76,9 @@ void RigidBody::setType(BodyType type) {
         mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, Vector3::zero());
         mWorld.mRigidBodyComponents.setAngularVelocity(mEntity, Vector3::zero());
 
+        mWorld.mRigidBodyComponents.setConstrainedLinearVelocity(mEntity, Vector3::zero());
+        mWorld.mRigidBodyComponents.setConstrainedAngularVelocity(mEntity, Vector3::zero());
+
         const Transform& transform = getTransform();
         mWorld.mRigidBodyComponents.setConstrainedPosition(mEntity, transform.getPosition());
         mWorld.mRigidBodyComponents.setConstrainedOrientation(mEntity, transform.getOrientation());
@@ -1039,6 +1042,10 @@ void RigidBody::setIsSleeping(bool isSleeping) {
 
         mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, Vector3::zero());
         mWorld.mRigidBodyComponents.setAngularVelocity(mEntity, Vector3::zero());
+
+        mWorld.mRigidBodyComponents.setConstrainedLinearVelocity(mEntity, Vector3::zero());
+        mWorld.mRigidBodyComponents.setConstrainedAngularVelocity(mEntity, Vector3::zero());
+
         mWorld.mRigidBodyComponents.setExternalForce(mEntity, Vector3::zero());
         mWorld.mRigidBodyComponents.setExternalTorque(mEntity, Vector3::zero());
     }

src/collision/TriangleVertexArray.cpp

@@ -160,6 +160,7 @@ Vector3 TriangleVertexArray::getVertex(uint32 vertexIndex) const {
     else {
         assert(false);
     }
+    return Vector3::zero();
 }
 
 // Return a vertex normal of the array

src/constraint/HingeJoint.cpp

@@ -74,9 +74,8 @@ HingeJoint::HingeJoint(Entity entity, PhysicsWorld &world, const HingeJointInfo&
     mWorld.mHingeJointsComponents.setHingeLocalAxisBody1(mEntity, hingeLocalAxisBody1);
     mWorld.mHingeJointsComponents.setHingeLocalAxisBody2(mEntity, hingeLocalAxisBody2);
 
-    // Compute the inverse of the initial orientation difference between the two bodies
-    Quaternion initOrientationDifferenceInv = transform2.getOrientation() *
-                                    transform1.getOrientation().getInverse();
+    Quaternion initOrientationDifferenceInv = transform1.getOrientation().getInverse() *
+                                    transform2.getOrientation();
     initOrientationDifferenceInv.normalize();
     initOrientationDifferenceInv.inverse();
     mWorld.mHingeJointsComponents.setInitOrientationDifferenceInv(mEntity, initOrientationDifferenceInv);

src/systems/SolveHingeJointSystem.cpp

@@ -255,7 +255,8 @@ void SolveHingeJointSystem::warmstart() {
         const Vector3 limitsImpulse = (impulseUpperLimit - impulseLowerLimit) * a1;
 
         // Compute the impulse P=J^T * lambda for the motor constraint
-        const Vector3 motorImpulse = -mHingeJointComponents.mImpulseMotor[i] * a1;
+        // sign flipped to match the corrected motor impulse in solveVelocityConstraint
+        const Vector3 motorImpulse = mHingeJointComponents.mImpulseMotor[i] * a1;
 
         // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1
         Vector3 linearImpulseBody1 = -impulseTranslation;
@@ -400,13 +401,13 @@ void SolveHingeJointSystem::solveVelocityConstraint() {
             deltaLambdaMotor = mHingeJointComponents.mImpulseMotor[i] - lambdaTemp;
 
             // Compute the impulse P=J^T * lambda for the motor of body 1
-            const Vector3 angularImpulseBody1 = -deltaLambdaMotor * a1;
+            const Vector3 angularImpulseBody1 = deltaLambdaMotor * a1;
 
             // Apply the impulse to the body 1
             w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
 
             // Compute the impulse P=J^T * lambda for the motor of body 2
-            const Vector3 angularImpulseBody2 = deltaLambdaMotor * a1;
+            const Vector3 angularImpulseBody2 = -deltaLambdaMotor * a1;
 
             // Apply the impulse to the body 2
             w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
@@ -760,8 +761,10 @@ decimal SolveHingeJointSystem::computeCurrentHingeAngle(Entity jointEntity, cons
 
     decimal hingeAngle;
 
-    // Compute the current orientation difference between the two bodies
-    Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
+    // Compute the current orientation difference between the two bodies, in body 1's frame
+    // q1^-1 * q2 rather than the world-frame q2 * q1^-1
+    // In body 1's frame a hinge rotation shows up as a rotation about the local axis
+    Quaternion currentOrientationDiff = orientationBody1.getInverse() * orientationBody2;
     currentOrientationDiff.normalize();
 
     // Compute the relative rotation considering the initial orientation difference
@@ -779,7 +782,7 @@ decimal SolveHingeJointSystem::computeCurrentHingeAngle(Entity jointEntity, cons
     decimal sinHalfAngleAbs = relativeRotation.getVectorV().length();
 
     // Compute the dot product of the relative rotation axis and the hinge axis
-    decimal dotProduct = relativeRotation.getVectorV().dot(mHingeJointComponents.getA1(jointEntity));
+    decimal dotProduct = relativeRotation.getVectorV().dot(mHingeJointComponents.getHingeLocalAxisBody1(jointEntity));
 
     // If the relative rotation axis and the hinge axis are pointing the same direction
     if (dotProduct >= decimal(0.0)) {

testbed/CMakeLists.txt

@@ -127,6 +127,8 @@ set(COMMON_SOURCES
 
 # Examples scenes source files
 set(SCENES_SOURCES
+    scenes/crane/CraneScene.h
+    scenes/crane/CraneScene.cpp
     scenes/ballandsocketjointsnet/BallAndSocketJointsNetScene.h
     scenes/ballandsocketjointsnet/BallAndSocketJointsNetScene.cpp
     scenes/ballandsocketjointschain/BallAndSocketJointsChainScene.h

testbed/common/Box.cpp

@@ -81,6 +81,21 @@ Box::Box(reactphysics3d::BodyType type, bool isSimulationCollider, const openglf
     mTransformMatrix = mTransformMatrix * mScalingMatrix;
 }
 
+// Change the size of the box (visual scaling and collision shape half-extents)
+void Box::setSize(const openglframework::Vector3& size) {
+
+    mSize[0] = size.x * 0.5f;
+    mSize[1] = size.y * 0.5f;
+    mSize[2] = size.z * 0.5f;
+
+    mScalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0,
+                                              0, mSize[1], 0, 0,
+                                              0, 0, mSize[2], 0,
+                                              0, 0, 0, 1);
+
+    mBoxShape->setHalfExtents(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
+}
+
 // Destructor
 Box::~Box() {
 

testbed/common/Box.h

@@ -91,6 +91,9 @@ class Box : public PhysicsObject {
 
         /// Return the collider
         rp3d::Collider* getCollider();
+
+        /// Change the size of the box (visual scaling and collision shape half-extents)
+        void setSize(const openglframework::Vector3& size);
 };
 
 // Update the transform matrix of the object