mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'optimization' into sat

Browse Source
This commit is contained in:
Daniel Chappuis 2018-01-21 13:11:57 +01:00
commit 624de80453
7 changed files with 262 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
.travis.yml
View File

@ -1,20 +1,42 @@
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
- os: osx
osx_image: xcode8
env:
- MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9"
# Linux / Clang
- 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
- os: osx
osx_image: xcode8
before_install:
- eval "${MATRIX_EVAL}"
branches:
only:
- master

42
documentation/UserManual/ReactPhysics3D-UserManual.tex
View File

@ -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}

128
src/body/RigidBody.cpp
View File

@ -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;
@ -91,19 +91,20 @@ 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();
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
if (mIsInertiaTensorSetByUser) {
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
}
}
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Awake the body
setIsSleeping(false);
@ -120,24 +121,50 @@ 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
*/
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
mUserInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
mIsInertiaTensorSetByUser = true;
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 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;
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
}
// 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
@ -146,6 +173,8 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
if (mType != BodyType::DYNAMIC) return;
mIsCenterOfMassSetByUser = true;
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
mCenterOfMassLocal = centerOfMassLocal;
@ -346,12 +375,13 @@ void RigidBody::recomputeMassInformation() {
mInitMass = decimal(0.0);
mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
mInertiaTensorInverseWorld.setToZero();
mCenterOfMassLocal.setToZero();
if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero();
if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero();
if (!mIsCenterOfMassSetByUser) 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;
@ -362,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)) {
@ -375,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();
mInertiaTensorLocal += inertiaTensor + offsetMatrix;
if (!mIsCenterOfMassSetByUser) {
mCenterOfMassLocal *= mMassInverse;
}
// Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.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();

55
src/body/RigidBody.h
View File

@ -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;
@ -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
@ -163,24 +169,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 +279,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.

52
src/constraint/FixedJoint.cpp
View File

@ -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();
}
// Initialize before solving the constraint
@ -104,10 +111,9 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the bias "b" for the 3 rotation constraints
mBiasRotation.setToZero();
if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::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();
}
@ -295,10 +301,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

51
src/constraint/SliderJoint.cpp
View File

@ -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() *
@ -157,9 +164,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the bias "b" of the rotation constraint
mBRotation.setToZero();
if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::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();
}
@ -539,10 +544,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

11
src/engine/ContactSolver.cpp
View File

@ -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();