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

Remove unecessary variables in constraints and cache inverse inertia world tensor of bodies

Browse Source
This commit is contained in:
Daniel Chappuis 2016-09-26 22:51:30 +02:00
parent 8f4979f4a2
commit c597815191
5 changed files with 97 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/body/RigidBody.cpp
View File

@ -46,6 +46,9 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde
// Compute the inverse mass // Compute the inverse mass
mMassInverse = decimal(1.0) / mInitMass; mMassInverse = decimal(1.0) / mInitMass;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
} }
// Destructor // Destructor
@ -90,11 +93,15 @@ void RigidBody::setType(BodyType type) {
mMassInverse = decimal(0.0); mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero(); mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero(); mInertiaTensorLocalInverse.setToZero();
mInertiaTensorInverseWorld.setToZero();
} }
else { // If it is a dynamic body else { // If it is a dynamic body
mMassInverse = decimal(1.0) / mInitMass; mMassInverse = decimal(1.0) / mInitMass;
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
} }
// Awake the body // Awake the body
@ -125,6 +132,9 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
// Compute the inverse local inertia tensor // Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
} }
// Set the local center of mass of the body (in local-space coordinates) // Set the local center of mass of the body (in local-space coordinates)
@ -314,6 +324,9 @@ void RigidBody::setTransform(const Transform& transform) {
// Update the linear velocity of the center of mass // Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Update the broad-phase state of the body // Update the broad-phase state of the body
updateBroadPhaseState(); updateBroadPhaseState();
} }
@ -326,6 +339,7 @@ void RigidBody::recomputeMassInformation() {
mMassInverse = decimal(0.0); mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero(); mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero(); mInertiaTensorLocalInverse.setToZero();
mInertiaTensorInverseWorld.setToZero();
mCenterOfMassLocal.setToZero(); mCenterOfMassLocal.setToZero();
// If it is STATIC or KINEMATIC body // If it is STATIC or KINEMATIC body
@ -386,6 +400,9 @@ void RigidBody::recomputeMassInformation() {
// Compute the local inverse inertia tensor // Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Update the linear velocity of the center of mass // Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
} }

23
src/body/RigidBody.h
View File

@ -83,6 +83,9 @@ class RigidBody : public CollisionBody {
/// Inverse of the inertia tensor of the body /// Inverse of the inertia tensor of the body
Matrix3x3 mInertiaTensorLocalInverse; Matrix3x3 mInertiaTensorLocalInverse;
/// Inverse of the world inertia tensor of the body
Matrix3x3 mInertiaTensorInverseWorld;
/// Inverse of the mass of the body /// Inverse of the mass of the body
decimal mMassInverse; decimal mMassInverse;
@ -112,6 +115,9 @@ class RigidBody : public CollisionBody {
/// Update the broad-phase state for this body (because it has moved for instance) /// Update the broad-phase state for this body (because it has moved for instance)
virtual void updateBroadPhaseState() const override; virtual void updateBroadPhaseState() const override;
/// Update the world inverse inertia tensor of the body
void updateInertiaTensorInverseWorld();
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -291,12 +297,19 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
*/ */
inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
// INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
// Compute and return the inertia tensor in world coordinates // Compute and return the inertia tensor in world coordinates
return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse * return mInertiaTensorInverseWorld;
mTransform.getOrientation().getMatrix().getTranspose(); }
// Update the world inverse inertia tensor of the body
/// The inertia tensor I_w in world coordinates is computed with the
/// local inverse inertia tensor I_b^-1 in body coordinates
/// by I_w = R * I_b^-1 * R^T
/// where R is the rotation matrix (and R^T its transpose) of the
/// current orientation quaternion of the body
inline void RigidBody::updateInertiaTensorInverseWorld() {
Matrix3x3 orientation = mTransform.getOrientation().getMatrix();
mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose();
} }
// Return true if the gravity needs to be applied to this rigid body // Return true if the gravity needs to be applied to this rigid body

163
src/engine/ContactSolver.cpp
View File

@ -36,7 +36,7 @@ using namespace std;
// Constants initialization // Constants initialization
const decimal ContactSolver::BETA = decimal(0.2); const decimal ContactSolver::BETA = decimal(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP= decimal(0.01); const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor // Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex, ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
@ -74,14 +74,10 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
// TODO : Count exactly the number of constraints to allocate here // TODO : Count exactly the number of constraints to allocate here
uint nbPenetrationConstraints = nbContactManifolds * MAX_CONTACT_POINTS_IN_MANIFOLD; uint nbPenetrationConstraints = nbContactManifolds * MAX_CONTACT_POINTS_IN_MANIFOLD;
mPenetrationConstraints = static_cast<PenetrationConstraint*>(mSingleFrameAllocator.allocate(sizeof(PenetrationConstraint) * nbPenetrationConstraints)); mPenetrationConstraints = static_cast<PenetrationConstraint*>(mSingleFrameAllocator.allocate(sizeof(PenetrationConstraint) * nbPenetrationConstraints));
//mPenetrationConstraints = new PenetrationConstraint[nbPenetrationConstraints];
assert(mPenetrationConstraints != nullptr); assert(mPenetrationConstraints != nullptr);
//mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4];
mFrictionConstraints = static_cast<FrictionConstraint*>(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds)); mFrictionConstraints = static_cast<FrictionConstraint*>(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds));
//mFrictionConstraints = new FrictionConstraint[nbContactManifolds];
assert(mFrictionConstraints != nullptr); assert(mFrictionConstraints != nullptr);
//mFrictionConstraints = new FrictionConstraint[mNbContactManifolds];
// For each island of the world // For each island of the world
for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) { for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
@ -146,23 +142,17 @@ void ContactSolver::initializeForIsland(Island* island) {
// contact manifold // contact manifold
mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 = body1->mMassInverse; mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 = body1->mMassInverse;
mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 = body2->mMassInverse; mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 = body2->mMassInverse;
//internalManifold.nbContacts = externalManifold->getNbContactPoints();
decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false; mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false;
//internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
//internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
// If we solve the friction constraints at the center of the contact manifold Vector3 frictionPointBody1;
//if (mIsSolveFrictionAtContactManifoldCenterActive) { Vector3 frictionPointBody2;
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 = Vector3::zero(); mFrictionConstraints[mNbFrictionConstraints].normal.setToZero();
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 = Vector3::zero();
mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero();
//}
// Compute the inverse K matrix for the rolling resistance constraint // Compute the inverse K matrix for the rolling resistance constraint
mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.setToZero(); mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.setToZero();
@ -186,7 +176,6 @@ void ContactSolver::initializeForIsland(Island* island) {
mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 = I2; mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 = I2;
mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 = body1->mMassInverse; mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 = body1->mMassInverse;
mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse; mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse;
mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor = restitutionFactor;
mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints; mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints;
mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact; mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact;
@ -196,8 +185,6 @@ void ContactSolver::initializeForIsland(Island* island) {
mPenetrationConstraints[mNbPenetrationConstraints].r1 = p1 - x1; mPenetrationConstraints[mNbPenetrationConstraints].r1 = p1 - x1;
mPenetrationConstraints[mNbPenetrationConstraints].r2 = p2 - x2; mPenetrationConstraints[mNbPenetrationConstraints].r2 = p2 - x2;
//mPenetrationConstraints[penConstIndex].externalContact = externalContact;
mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal(); mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal();
mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth(); mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth();
mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact(); mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact();
@ -205,18 +192,10 @@ void ContactSolver::initializeForIsland(Island* island) {
mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact; mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact;
externalContact->setIsRestingContact(true); externalContact->setIsRestingContact(true);
//mPenetrationConstraints[penConstIndex].oldFrictionVector1 = externalContact->getFrictionVector1();
//mPenetrationConstraints[penConstIndex].oldFrictionVector2 = externalContact->getFrictionVector2();
mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = 0.0; mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = 0.0;
//mPenetrationConstraints[penConstIndex].friction1Impulse = 0.0;
//mPenetrationConstraints[penConstIndex].friction2Impulse = 0.0;
//mPenetrationConstraints[penConstIndex].rollingResistanceImpulse = Vector3::zero();
// If we solve the friction constraints at the center of the contact manifold frictionPointBody1 += p1;
//if (mIsSolveFrictionAtContactManifoldCenterActive) { frictionPointBody2 += p2;
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 += p1;
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 += p2;
//}
// Compute the velocity difference // Compute the velocity difference
Vector3 deltaV = v2 + w2.cross(mPenetrationConstraints[mNbPenetrationConstraints].r2) - v1 - w1.cross(mPenetrationConstraints[mNbPenetrationConstraints].r1); Vector3 deltaV = v2 + w2.cross(mPenetrationConstraints[mNbPenetrationConstraints].r2) - v1 - w1.cross(mPenetrationConstraints[mNbPenetrationConstraints].r1);
@ -238,7 +217,7 @@ void ContactSolver::initializeForIsland(Island* island) {
mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = 0.0; mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = 0.0;
decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[mNbPenetrationConstraints].normal); decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[mNbPenetrationConstraints].normal);
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor * deltaVDotN; mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = restitutionFactor * deltaVDotN;
} }
// If the warm starting of the contact solver is active // If the warm starting of the contact solver is active
@ -251,77 +230,70 @@ void ContactSolver::initializeForIsland(Island* island) {
// Initialize the split impulses to zero // Initialize the split impulses to zero
mPenetrationConstraints[mNbPenetrationConstraints].penetrationSplitImpulse = 0.0; mPenetrationConstraints[mNbPenetrationConstraints].penetrationSplitImpulse = 0.0;
// If we solve the friction constraints at the center of the contact manifold mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal;
//if (mIsSolveFrictionAtContactManifoldCenterActive) {
mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal;
//}
mNbPenetrationConstraints++; mNbPenetrationConstraints++;
nbContacts++; nbContacts++;
} }
// If we solve the friction constraints at the center of the contact manifold frictionPointBody1 /= nbContacts;
//if (mIsSolveFrictionAtContactManifoldCenterActive) { frictionPointBody2 /= nbContacts;
mFrictionConstraints[mNbFrictionConstraints].r1Friction = frictionPointBody1 - x1;
mFrictionConstraints[mNbFrictionConstraints].r2Friction = frictionPointBody2 - x2;
mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector1 = externalManifold->getFrictionVector1();
mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector2 = externalManifold->getFrictionVector2();
//mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero(); // If warm starting is active
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 /= nbContacts; if (mIsWarmStartingActive) {
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 /= nbContacts;
mFrictionConstraints[mNbFrictionConstraints].r1Friction = mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 - x1;
mFrictionConstraints[mNbFrictionConstraints].r2Friction = mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 - x2;
mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector1 = externalManifold->getFrictionVector1();
mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector2 = externalManifold->getFrictionVector2();
// If warm starting is active // Initialize the accumulated impulses with the previous step accumulated impulses
if (mIsWarmStartingActive) { mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = externalManifold->getFrictionImpulse1();
mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = externalManifold->getFrictionImpulse2();
mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
}
else {
// Initialize the accumulated impulses with the previous step accumulated impulses // Initialize the accumulated impulses to zero
mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = externalManifold->getFrictionImpulse1(); mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = 0.0;
mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = externalManifold->getFrictionImpulse2(); mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = 0.0;
mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = 0.0;
} mFrictionConstraints[mNbFrictionConstraints].rollingResistanceImpulse = Vector3(0, 0, 0);
else { }
// Initialize the accumulated impulses to zero mFrictionConstraints[mNbFrictionConstraints].normal.normalize();
mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = 0.0;
mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = 0.0;
mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = 0.0;
mFrictionConstraints[mNbFrictionConstraints].rollingResistanceImpulse = Vector3(0, 0, 0);
}
mFrictionConstraints[mNbFrictionConstraints].normal.normalize(); Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) -
v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction);
Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) - // Compute the friction vectors
v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction); computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]);
// Compute the friction vectors // Compute the inverse mass matrix K for the friction constraints at the center of the contact manifold
computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]); mFrictionConstraints[mNbFrictionConstraints].r1CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
mFrictionConstraints[mNbFrictionConstraints].r1CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
// Compute the inverse mass matrix K for the friction constraints at the center of the contact manifold mFrictionConstraints[mNbFrictionConstraints].r2CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
mFrictionConstraints[mNbFrictionConstraints].r1CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1); mFrictionConstraints[mNbFrictionConstraints].r2CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
mFrictionConstraints[mNbFrictionConstraints].r1CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2); decimal friction1Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
mFrictionConstraints[mNbFrictionConstraints].r2CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1); ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
mFrictionConstraints[mNbFrictionConstraints].r2CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2); mFrictionConstraints[mNbFrictionConstraints].frictionVector1) +
decimal friction1Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 + ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot( mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
mFrictionConstraints[mNbFrictionConstraints].frictionVector1) + decimal friction2Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot( ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
mFrictionConstraints[mNbFrictionConstraints].frictionVector1); mFrictionConstraints[mNbFrictionConstraints].frictionVector2) +
decimal friction2Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 + ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot( mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
mFrictionConstraints[mNbFrictionConstraints].frictionVector2) + decimal frictionTwistMass = mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 *
((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot( mFrictionConstraints[mNbFrictionConstraints].normal) +
mFrictionConstraints[mNbFrictionConstraints].frictionVector2); mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 *
decimal frictionTwistMass = mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 * mFrictionConstraints[mNbFrictionConstraints].normal);
mFrictionConstraints[mNbFrictionConstraints].normal) + friction1Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction1Mass = decimal(1.0)/friction1Mass
mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 * : decimal(0.0);
mFrictionConstraints[mNbFrictionConstraints].normal); friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass
friction1Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction1Mass = decimal(1.0)/friction1Mass : decimal(0.0);
: decimal(0.0); frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) /
friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass frictionTwistMass :
: decimal(0.0); decimal(0.0);
frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) /
frictionTwistMass :
decimal(0.0);
mNbFrictionConstraints++; mNbFrictionConstraints++;
} }
@ -463,9 +435,6 @@ void ContactSolver::solvePenetrationConstraints() {
PROFILE("ContactSolver::solvePenetrationConstraints()"); PROFILE("ContactSolver::solvePenetrationConstraints()");
// TODO : Check that the PenetrationConstraint struct only contains variables that are
// used in this method, nothing more
// TODO : Maybe solve split impulses and normal impulses separately // TODO : Maybe solve split impulses and normal impulses separately
decimal deltaLambda; decimal deltaLambda;
@ -550,9 +519,6 @@ void ContactSolver::solvePenetrationConstraints() {
// Solve the friction constraints // Solve the friction constraints
void ContactSolver::solveFrictionConstraints() { void ContactSolver::solveFrictionConstraints() {
// TODO : Check that the FrictionConstraint struct only contains variables that are
// used in this method, nothing more
PROFILE("ContactSolver::solveFrictionConstraints()"); PROFILE("ContactSolver::solveFrictionConstraints()");
for (uint i=0; i<mNbFrictionConstraints; i++) { for (uint i=0; i<mNbFrictionConstraints; i++) {
@ -698,17 +664,6 @@ void ContactSolver::storeImpulses() {
mFrictionConstraints[i].contactManifold->setFrictionVector1(mFrictionConstraints[i].frictionVector1); mFrictionConstraints[i].contactManifold->setFrictionVector1(mFrictionConstraints[i].frictionVector1);
mFrictionConstraints[i].contactManifold->setFrictionVector2(mFrictionConstraints[i].frictionVector2); mFrictionConstraints[i].contactManifold->setFrictionVector2(mFrictionConstraints[i].frictionVector2);
} }
/*
if (mPenetrationConstraints != nullptr) {
// TODO : Delete this
delete[] mPenetrationConstraints;
}
if (mFrictionConstraints != nullptr) {
delete[] mFrictionConstraints;
}
*/
} }
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane

13
src/engine/ContactSolver.h
View File

@ -115,8 +115,6 @@ class ContactSolver {
struct PenetrationConstraint { struct PenetrationConstraint {
// TODO : Pack bools into a single value
/// Index of body 1 in the constraint solver /// Index of body 1 in the constraint solver
uint indexBody1; uint indexBody1;
@ -144,9 +142,6 @@ class ContactSolver {
/// Velocity restitution bias /// Velocity restitution bias
decimal restitutionBias; decimal restitutionBias;
/// Mix of the restitution factor for two bodies
decimal restitutionFactor;
/// Accumulated normal impulse /// Accumulated normal impulse
decimal penetrationImpulse; decimal penetrationImpulse;
@ -180,8 +175,6 @@ class ContactSolver {
struct FrictionConstraint { struct FrictionConstraint {
// TODO : Pack bools into a single value
/// Index of body 1 in the constraint solver /// Index of body 1 in the constraint solver
uint indexBody1; uint indexBody1;
@ -194,12 +187,6 @@ class ContactSolver {
/// R2 vector for the friction constraints /// R2 vector for the friction constraints
Vector3 r2Friction; Vector3 r2Friction;
/// Point on body 1 where to apply the friction constraints
Vector3 frictionPointBody1;
/// Point on body 2 where to apply the friction constraints
Vector3 frictionPointBody2;
/// Average normal vector of the contact manifold /// Average normal vector of the contact manifold
Vector3 normal; Vector3 normal;

3
src/engine/DynamicsWorld.cpp
View File

@ -212,6 +212,9 @@ void DynamicsWorld::updateBodiesState() {
// Update the transform of the body (using the new center of mass and new orientation) // Update the transform of the body (using the new center of mass and new orientation)
bodies[b]->updateTransformWithCenterOfMass(); bodies[b]->updateTransformWithCenterOfMass();
// Update the world inverse inertia tensor of the body
bodies[b]->updateInertiaTensorInverseWorld();
// Update the broad-phase state of the body // Update the broad-phase state of the body
bodies[b]->updateBroadPhaseState(); bodies[b]->updateBroadPhaseState();
} }