From bccdfa9b5803adfc81fccf725acf0b693d49239a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 27 Jan 2017 20:26:56 +0100 Subject: [PATCH 1/9] Fix issue in collision detection --- src/collision/CollisionDetection.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 345c46bb..be17e4f7 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -605,7 +605,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape computeConvexVsConcaveMiddlePhase(&pair, mMemoryAllocator, &narrowPhaseInfo); } - return nullptr; + return narrowPhaseInfo; } // Report all the bodies that overlap with the aabb in parameter @@ -976,7 +976,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // For each possible collision pair of bodies map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { OverlappingPair* pair = it->second; From 1855e1c121772bad22e68fe9da077d0b089739ee Mon Sep 17 00:00:00 2001 From: jorrit Date: Fri, 30 Jun 2017 15:46:15 +0200 Subject: [PATCH 2/9] Fixed bug in FixedJoint and SliderJoint when objects initially have a different rotation. In solvePositionConstraint the error was calculated wrongly causing incorrect simulation. --- src/constraint/FixedJoint.cpp | 51 ++++++++++++++++++++++++++-------- src/constraint/SliderJoint.cpp | 51 ++++++++++++++++++++++++++-------- 2 files changed, 78 insertions(+), 24 deletions(-) diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 57c8d8c6..ef6a1952 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -42,11 +42,18 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; - // Compute the inverse of the initial orientation difference between the two bodies - mInitOrientationDifferenceInv = transform2.getOrientation() * - transform1.getOrientation().getInverse(); - mInitOrientationDifferenceInv.normalize(); - mInitOrientationDifferenceInv.inverse(); + // Store inverse of initial rotation from body 1 to body 2 in body 1 space: + // + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q20 = initial orientation of body 2 + // q10 = initial orientation of body 1 + // r0 = initial rotation rotation from body 1 to body 2 + mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation(); } // Destructor @@ -110,9 +117,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the bias "b" for the 3 rotation constraints mBiasRotation.setToZero(); if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse(); mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV(); } @@ -300,10 +305,32 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); } - // Compute the position error for the 3 rotation constraints - Quaternion currentOrientationDifference = q2 * q1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + // Calculate difference in rotation + // + // The rotation should be: + // + // q2 = q1 r0 + // + // But because of drift the actual rotation is: + // + // q2 = qError q1 r0 + // <=> qError = q2 r0^-1 q1^-1 + // + // Where: + // q1 = current rotation of body 1 + // q2 = current rotation of body 2 + // qError = error that needs to be reduced to zero + Quaternion qError = q2 * mInitOrientationDifferenceInv * q1.getInverse(); + + // A quaternion can be seen as: + // + // q = [sin(theta / 2) * v, cos(theta/2)] + // + // Where: + // v = rotation vector + // theta = rotation angle + // + // If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is: const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); // Compute the Lagrange multiplier lambda for the 3 rotation constraints diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 99a5c834..7c61d265 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -51,11 +51,18 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; - // Compute the inverse of the initial orientation difference between the two bodies - mInitOrientationDifferenceInv = transform2.getOrientation() * - transform1.getOrientation().getInverse(); - mInitOrientationDifferenceInv.normalize(); - mInitOrientationDifferenceInv.inverse(); + // Store inverse of initial rotation from body 1 to body 2 in body 1 space: + // + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q20 = initial orientation of body 2 + // q10 = initial orientation of body 1 + // r0 = initial rotation rotation from body 1 to body 2 + mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation(); // Compute the slider axis in local-space of body 1 mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() * @@ -162,9 +169,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the bias "b" of the rotation constraint mBRotation.setToZero(); if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse(); mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); } @@ -544,10 +549,32 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); } - // Compute the position error for the 3 rotation constraints - Quaternion currentOrientationDifference = q2 * q1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + // Calculate difference in rotation + // + // The rotation should be: + // + // q2 = q1 r0 + // + // But because of drift the actual rotation is: + // + // q2 = qError q1 r0 + // <=> qError = q2 r0^-1 q1^-1 + // + // Where: + // q1 = current rotation of body 1 + // q2 = current rotation of body 2 + // qError = error that needs to be reduced to zero + Quaternion qError = q2 * mInitOrientationDifferenceInv * q1.getInverse(); + + // A quaternion can be seen as: + // + // q = [sin(theta / 2) * v, cos(theta/2)] + // + // Where: + // v = rotation vector + // theta = rotation angle + // + // If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is: const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); // Compute the Lagrange multiplier lambda for the 3 rotation constraints From 04a0efafaf9ce7b4917951f305f3bdd49779e98c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 3 Jul 2017 22:01:36 +0200 Subject: [PATCH 3/9] Edit travis configuration --- .travis.yml | 57 ++++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 41 insertions(+), 16 deletions(-) diff --git a/.travis.yml b/.travis.yml index b069cab1..c020c351 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,20 +1,45 @@ language: cpp -os: - - linux - - osx -compiler: - - gcc - - clang -install: -- if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi -addons: - apt: - sources: - - ubuntu-toolchain-r-test - packages: - - gcc-4.8 - - g++-4.8 - - clang +matrix: + + # Linux / GCC + include: + - os: linux + addons: + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - g++-4.9 + env: + - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9" + + # OS X / GCC + include: + - os: osx + osx_image: xcode8 + env: + - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9" + + # Linux / Clang + include: + - os: linux + addons: + apt: + sources: + - ubuntu-toolchain-r-test + - llvm-toolchain-precise-3.6 + packages: + - clang-3.6 + env: + - MATRIX_EVAL="CC=clang-3.6 && CXX=clang++-3.6" + + # OS X / Clang + include: + - os: osx + osx_image: xcode8 + +before_install: + - eval "${MATRIX_EVAL}" branches: only: - master From 2f1d529a68def6fb8b195a1689750045101c9128 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 3 Jul 2017 22:23:01 +0200 Subject: [PATCH 4/9] Edit travis configuration --- .travis.yml | 40 ++++++++++++++++++++-------------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/.travis.yml b/.travis.yml index c020c351..4702ce6d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,25 +5,25 @@ matrix: include: - os: linux addons: - apt: - sources: - - ubuntu-toolchain-r-test - packages: - - g++-4.9 + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - g++-4.9 env: - - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9" - - # OS X / GCC - include: - - os: osx - osx_image: xcode8 - env: - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9" + # OS X / GCC + include: + - os: osx + osx_image: xcode8 + env: + - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9" + # Linux / Clang include: - - os: linux - addons: + - os: linux + addons: apt: sources: - ubuntu-toolchain-r-test @@ -32,14 +32,14 @@ matrix: - clang-3.6 env: - MATRIX_EVAL="CC=clang-3.6 && CXX=clang++-3.6" - - # OS X / Clang - include: - - os: osx - osx_image: xcode8 + + # OS X / Clang + include: + - os: osx + osx_image: xcode8 before_install: - - eval "${MATRIX_EVAL}" + - eval "${MATRIX_EVAL}" branches: only: - master From c0fc023b7429ec019066843ba03828a87d370d18 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 3 Jul 2017 23:25:23 +0200 Subject: [PATCH 5/9] Edit travis configuration --- .travis.yml | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/.travis.yml b/.travis.yml index 4702ce6d..25642b11 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,15 +13,13 @@ matrix: env: - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9" - # OS X / GCC - include: + # OS X / GCC - os: osx osx_image: xcode8 env: - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9" - # Linux / Clang - include: + # Linux / Clang - os: linux addons: apt: @@ -33,8 +31,7 @@ matrix: env: - MATRIX_EVAL="CC=clang-3.6 && CXX=clang++-3.6" - # OS X / Clang - include: + # OS X / Clang - os: osx osx_image: xcode8 From a2705f176c4a8567b4cb9d80ebaedf98d249c11b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 3 Jul 2017 23:32:52 +0200 Subject: [PATCH 6/9] Edit travis configuration --- .travis.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 25642b11..d3445273 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,11 +5,11 @@ matrix: include: - os: linux addons: - apt: - sources: - - ubuntu-toolchain-r-test - packages: - - g++-4.9 + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - g++-4.9 env: - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9" From dfb4b811f9af79d1b53db448dcbfc57411abb52a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 1 Aug 2017 12:39:20 +0200 Subject: [PATCH 7/9] Edit user manual documentation --- .../UserManual/ReactPhysics3D-UserManual.tex | 42 +++++++++++++------ 1 file changed, 30 insertions(+), 12 deletions(-) diff --git a/documentation/UserManual/ReactPhysics3D-UserManual.tex b/documentation/UserManual/ReactPhysics3D-UserManual.tex index c7e66ccb..e1c20e5d 100644 --- a/documentation/UserManual/ReactPhysics3D-UserManual.tex +++ b/documentation/UserManual/ReactPhysics3D-UserManual.tex @@ -373,6 +373,7 @@ world.enableSleeping(false); \end{sloppypar} \subsection{Updating the Dynamics World} + \label{sec:updating_dynamics_world} The \texttt{DynamicsWorld} is used to simulate physics through time. It has to be updated each time you want to simulate a step forward in time. Most of the time, you want to update the world right before rendering a new frame in a real-time application. \\ @@ -611,19 +612,36 @@ rigidBody->applyTorque(torque); \subsection{Updating a Rigid Body} When you call the \texttt{DynamicsWorld::update()} method, the collisions between the bodies are computed and the joints are evaluated. Then, the bodies position - and orientation - are updated accordingly. After calling this method, you can get the updated position and orientation of each body to render it. To do that, you simply need to use the - \texttt{RigidBody::getInterpolatedTransform()} method to get the interpolated transform. This transform represents the current local-to-world-space transformation - of the body. \\ + and orientation are updated accordingly. \\ - Here is how to get the interpolated transform of a rigid body: \\ + Remember that in section \ref{sec:updating_dynamics_world} we were using a time accumulator in order to always have fixed physics time steps. + Now imagine a situation where the rendering frame rate is higher than the the physics frame rate. It means that at the end of most rendering + frames there will be some time left in the accumulator for the physics time that has not been simulated yet by the physics engine. + It means that we are rendering the state of the physics simulation at a time different from the rendering time which can cause a visual stuttering effect. \\ + + To solve this, the idea is to interpolate between the previous and current physics state of the simulation based on how much time is left in the + accumulator. First we compute the interpolation factor as follows: \\ \begin{lstlisting} -// Here, body is a RigidBody* pointer previously created -// Get the interpolated transform of the rigid body -rp3d::Transform transform = body->getInterpolatedTransform(); - \end{lstlisting} + // Compute the interpolation factor ("accumulator" is the time left in the accumulator and + // "dt" is the physics time step) + const float interpolationFactor = accumulator / dt; + \end{lstlisting} + + \vspace{0.6cm} + + Then we get the current transform of the rigid body and use it with the previous transform (transform at the previous frame) to + compute the interpolated transform as in the following code: \\ + + \begin{lstlisting} + + // Get the current transform of the rigid body + rp3d::Transform currentTransform = body->getTransform(); + + // Interpolate the transform between the previous one and the new one + rp3d::Transform interpolatedTransform = rp3d::Transform::interpolateTransforms(previousTransform, currentTransform, interpolationFactor); + \end{lstlisting} \vspace{0.6cm} @@ -631,9 +649,9 @@ rp3d::Transform transform = body->getInterpolatedTransform(); following code: \\ \begin{lstlisting} -// Get the OpenGL matrix array of the transform -float matrix[16]; -transform.getOpenGLMatrix(matrix); + // Get the OpenGL matrix array of the transform + float matrix[16]; + transform.getOpenGLMatrix(matrix); \end{lstlisting} \subsection{Destroying a Rigid Body} From c7f7a169f82341e7ce5c26ba29c11e5a394b0ffc Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 15 Jan 2018 07:18:00 +0100 Subject: [PATCH 8/9] Refactor the getter/setter for inertia tensor of a RigidBody --- src/body/RigidBody.cpp | 30 ++++++++++++++++------ src/body/RigidBody.h | 49 +++++++++++------------------------- src/engine/ContactSolver.cpp | 11 +++++++- 3 files changed, 47 insertions(+), 43 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 36c362dc..bad11916 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -91,14 +91,13 @@ void RigidBody::setType(BodyType type) { // Reset the inverse mass and inverse inertia tensor to zero mMassInverse = decimal(0.0); - mInertiaTensorLocal.setToZero(); mInertiaTensorLocalInverse.setToZero(); mInertiaTensorInverseWorld.setToZero(); } else { // If it is a dynamic body mMassInverse = decimal(1.0) / mInitMass; - mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse; // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); @@ -126,12 +125,26 @@ void RigidBody::setType(BodyType type) { */ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { + mUserInertiaTensorLocalInverse = inertiaTensorLocal.getInverse(); + if (mType != BodyType::DYNAMIC) return; - mInertiaTensorLocal = inertiaTensorLocal; + // Compute the inverse local inertia tensor + mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse; + + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); +} + +// Set the inverse local inertia tensor of the body (in body coordinates) +void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTensorLocal) { + + mUserInertiaTensorLocalInverse = inverseInertiaTensorLocal; + + if (mType != BodyType::DYNAMIC) return; // Compute the inverse local inertia tensor - mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse; // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); @@ -346,12 +359,13 @@ void RigidBody::recomputeMassInformation() { mInitMass = decimal(0.0); mMassInverse = decimal(0.0); - mInertiaTensorLocal.setToZero(); mInertiaTensorLocalInverse.setToZero(); mInertiaTensorInverseWorld.setToZero(); mCenterOfMassLocal.setToZero(); + Matrix3x3 inertiaTensorLocal; + inertiaTensorLocal.setToZero(); - // If it is STATIC or KINEMATIC body + // If it is a STATIC or a KINEMATIC body if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) { mCenterOfMassWorld = mTransform.getPosition(); return; @@ -403,11 +417,11 @@ void RigidBody::recomputeMassInformation() { offsetMatrix[2] += offset * (-offset.z); offsetMatrix *= shape->getMass(); - mInertiaTensorLocal += inertiaTensor + offsetMatrix; + inertiaTensorLocal += inertiaTensor + offsetMatrix; } // Compute the local inverse inertia tensor - mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse(); // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 23603bda..eaadb59c 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -81,9 +81,9 @@ class RigidBody : public CollisionBody { /// Current external torque on the body Vector3 mExternalTorque; - /// Local inertia tensor of the body (in local-space) with respect to the - /// center of mass of the body - Matrix3x3 mInertiaTensorLocal; + /// Inverse Local inertia tensor of the body (in local-space) set + /// by the user with respect to the center of mass of the body + Matrix3x3 mUserInertiaTensorLocalInverse; /// Inverse of the inertia tensor of the body Matrix3x3 mInertiaTensorLocalInverse; @@ -163,24 +163,24 @@ class RigidBody : public CollisionBody { /// Set the variable to know whether or not the body is sleeping virtual void setIsSleeping(bool isSleeping) override; - /// Return the local inertia tensor of the body (in body coordinates) - const Matrix3x3& getInertiaTensorLocal() const; - /// Set the local inertia tensor of the body (in body coordinates) void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); + /// Set the inverse local inertia tensor of the body (in body coordinates) + void setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTensorLocal); + + /// Get the inverse local inertia tensor of the body (in body coordinates) + const Matrix3x3& getInverseInertiaTensorLocal() const; + + /// Return the inverse of the inertia tensor in world coordinates. + Matrix3x3 getInertiaTensorInverseWorld() const; + /// Set the local center of mass of the body (in local-space coordinates) void setCenterOfMassLocal(const Vector3& centerOfMassLocal); /// Set the mass of the rigid body void setMass(decimal mass); - /// Return the inertia tensor in world coordinates. - Matrix3x3 getInertiaTensorWorld() const; - - /// Return the inverse of the inertia tensor in world coordinates. - Matrix3x3 getInertiaTensorInverseWorld() const; - /// Return true if the gravity needs to be applied to this rigid body bool isGravityEnabled() const; @@ -273,28 +273,9 @@ inline Vector3 RigidBody::getAngularVelocity() const { return mAngularVelocity; } -// Return the local inertia tensor of the body (in local-space coordinates) -/** - * @return The 3x3 inertia tensor matrix of the body (in local-space coordinates) - */ -inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const { - return mInertiaTensorLocal; -} - -// Return the inertia tensor in world coordinates. -/// The inertia tensor I_w in world coordinates is computed -/// with the local inertia tensor I_b in body coordinates -/// by I_w = R * I_b * R^T -/// where R is the rotation matrix (and R^T its transpose) of -/// the current orientation quaternion of the body -/** - * @return The 3x3 inertia tensor matrix of the body in world-space coordinates - */ -inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { - - // Compute and return the inertia tensor in world coordinates - return mTransform.getOrientation().getMatrix() * mInertiaTensorLocal * - mTransform.getOrientation().getMatrix().getTranspose(); +// Get the inverse local inertia tensor of the body (in body coordinates) +inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const { + return mInertiaTensorLocalInverse; } // Return the inverse of the inertia tensor in world coordinates. diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index f4762c49..a7b86b35 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -259,8 +259,17 @@ void ContactSolver::initializeForIsland(Island* island) { bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) { + mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2; - mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse(); + decimal det = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getDeterminant(); + + // If the matrix is not inversible + if (approxEqual(det, decimal(0.0))) { + mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); + } + else { + mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse(); + } } mContactConstraints[mNbContactManifolds].normal.normalize(); From 010d7876ef8cd69d0fcbca91cd99d9db64aa2030 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 21 Jan 2018 13:11:11 +0100 Subject: [PATCH 9/9] Make sure we do not recompute automatically center of mass and inertia tensor when they are set by the user --- src/body/RigidBody.cpp | 106 +++++++++++++++++++++++++---------------- src/body/RigidBody.h | 6 +++ 2 files changed, 72 insertions(+), 40 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index bad11916..5e5a67c7 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -42,7 +42,7 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde : CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), - mJointsList(nullptr) { + mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) { // Compute the inverse mass mMassInverse = decimal(1.0) / mInitMass; @@ -93,16 +93,18 @@ void RigidBody::setType(BodyType type) { mMassInverse = decimal(0.0); mInertiaTensorLocalInverse.setToZero(); mInertiaTensorInverseWorld.setToZero(); - } else { // If it is a dynamic body mMassInverse = decimal(1.0) / mInitMass; - mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse; - // Update the world inverse inertia tensor - updateInertiaTensorInverseWorld(); + if (mIsInertiaTensorSetByUser) { + mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse; + } } + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); + // Awake the body setIsSleeping(false); @@ -119,6 +121,8 @@ void RigidBody::setType(BodyType type) { } // Set the local inertia tensor of the body (in local-space coordinates) +/// If the inertia tensor is set with this method, it will not be computed +/// using the collision shapes of the body. /** * @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space * coordinates @@ -126,6 +130,7 @@ void RigidBody::setType(BodyType type) { void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { mUserInertiaTensorLocalInverse = inertiaTensorLocal.getInverse(); + mIsInertiaTensorSetByUser = true; if (mType != BodyType::DYNAMIC) return; @@ -136,10 +141,17 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { updateInertiaTensorInverseWorld(); } -// Set the inverse local inertia tensor of the body (in body coordinates) +// Set the inverse local inertia tensor of the body (in local-space coordinates) +/// If the inverse inertia tensor is set with this method, it will not be computed +/// using the collision shapes of the body. +/** + * @param inverseInertiaTensorLocal The 3x3 inverse inertia tensor matrix of the body in local-space + * coordinates + */ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTensorLocal) { mUserInertiaTensorLocalInverse = inverseInertiaTensorLocal; + mIsInertiaTensorSetByUser = true; if (mType != BodyType::DYNAMIC) return; @@ -151,6 +163,8 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens } // Set the local center of mass of the body (in local-space coordinates) +/// If you set the center of mass with the method, it will not be computed +/// automatically using collision shapes. /** * @param centerOfMassLocal The center of mass of the body in local-space * coordinates @@ -159,6 +173,8 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { if (mType != BodyType::DYNAMIC) return; + mIsCenterOfMassSetByUser = true; + const Vector3 oldCenterOfMass = mCenterOfMassWorld; mCenterOfMassLocal = centerOfMassLocal; @@ -359,9 +375,9 @@ void RigidBody::recomputeMassInformation() { mInitMass = decimal(0.0); mMassInverse = decimal(0.0); - mInertiaTensorLocalInverse.setToZero(); - mInertiaTensorInverseWorld.setToZero(); - mCenterOfMassLocal.setToZero(); + if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero(); + if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero(); + if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero(); Matrix3x3 inertiaTensorLocal; inertiaTensorLocal.setToZero(); @@ -376,7 +392,10 @@ void RigidBody::recomputeMassInformation() { // Compute the total mass of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { mInitMass += shape->getMass(); - mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); + + if (!mIsCenterOfMassSetByUser) { + mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); + } } if (mInitMass > decimal(0.0)) { @@ -389,39 +408,46 @@ void RigidBody::recomputeMassInformation() { // Compute the center of mass const Vector3 oldCenterOfMass = mCenterOfMassWorld; - mCenterOfMassLocal *= mMassInverse; - mCenterOfMassWorld = mTransform * mCenterOfMassLocal; - // Compute the total mass and inertia tensor using all the collision shapes - for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { - - // Get the inertia tensor of the collision shape in its local-space - Matrix3x3 inertiaTensor; - shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); - - // Convert the collision shape inertia tensor into the local-space of the body - const Transform& shapeTransform = shape->getLocalToBodyTransform(); - Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); - inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); - - // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape - // center into a inertia tensor w.r.t to the body origin. - Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal; - decimal offsetSquare = offset.lengthSquare(); - Matrix3x3 offsetMatrix; - offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0)); - offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0)); - offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare); - offsetMatrix[0] += offset * (-offset.x); - offsetMatrix[1] += offset * (-offset.y); - offsetMatrix[2] += offset * (-offset.z); - offsetMatrix *= shape->getMass(); - - inertiaTensorLocal += inertiaTensor + offsetMatrix; + if (!mIsCenterOfMassSetByUser) { + mCenterOfMassLocal *= mMassInverse; } - // Compute the local inverse inertia tensor - mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse(); + mCenterOfMassWorld = mTransform * mCenterOfMassLocal; + + if (!mIsInertiaTensorSetByUser) { + + // Compute the inertia tensor using all the collision shapes + for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { + + // Get the inertia tensor of the collision shape in its local-space + Matrix3x3 inertiaTensor; + shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); + + // Convert the collision shape inertia tensor into the local-space of the body + const Transform& shapeTransform = shape->getLocalToBodyTransform(); + Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); + inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); + + // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape + // center into a inertia tensor w.r.t to the body origin. + Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal; + decimal offsetSquare = offset.lengthSquare(); + Matrix3x3 offsetMatrix; + offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0)); + offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0)); + offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare); + offsetMatrix[0] += offset * (-offset.x); + offsetMatrix[1] += offset * (-offset.y); + offsetMatrix[2] += offset * (-offset.z); + offsetMatrix *= shape->getMass(); + + inertiaTensorLocal += inertiaTensor + offsetMatrix; + } + + // Compute the local inverse inertia tensor + mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse(); + } // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index eaadb59c..22c784ef 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -109,6 +109,12 @@ class RigidBody : public CollisionBody { /// First element of the linked list of joints involving this body JointListElement* mJointsList; + /// True if the center of mass is set by the user + bool mIsCenterOfMassSetByUser; + + /// True if the inertia tensor is set by the user + bool mIsInertiaTensorSetByUser; + // -------------------- Methods -------------------- // /// Remove a joint from the joints list