Remove unecessary variables in constraints and cache inverse inertia world tensor of bodies
This commit is contained in:
Daniel Chappuis
parent
8f4979f4a2
commit
c597815191
|
|
@ -46,6 +46,9 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde
|
|||
|
||||
// Compute the inverse mass
|
||||
mMassInverse = decimal(1.0) / mInitMass;
|
||||
|
||||
// Update the world inverse inertia tensor
|
||||
updateInertiaTensorInverseWorld();
|
||||
}
|
||||
|
||||
// Destructor
|
||||
|
|
@ -90,11 +93,15 @@ void RigidBody::setType(BodyType type) {
|
|||
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();
|
||||
}
|
||||
|
||||
// Awake the body
|
||||
|
|
@ -125,6 +132,9 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
|
|||
|
||||
// Compute the inverse local inertia tensor
|
||||
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
|
||||
|
||||
// Update the world inverse inertia tensor
|
||||
updateInertiaTensorInverseWorld();
|
||||
}
|
||||
|
||||
// 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
|
||||
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
|
||||
|
||||
// Update the world inverse inertia tensor
|
||||
updateInertiaTensorInverseWorld();
|
||||
|
||||
// Update the broad-phase state of the body
|
||||
updateBroadPhaseState();
|
||||
}
|
||||
|
|
@ -326,6 +339,7 @@ void RigidBody::recomputeMassInformation() {
|
|||
mMassInverse = decimal(0.0);
|
||||
mInertiaTensorLocal.setToZero();
|
||||
mInertiaTensorLocalInverse.setToZero();
|
||||
mInertiaTensorInverseWorld.setToZero();
|
||||
mCenterOfMassLocal.setToZero();
|
||||
|
||||
// If it is STATIC or KINEMATIC body
|
||||
|
|
@ -386,6 +400,9 @@ void RigidBody::recomputeMassInformation() {
|
|||
// Compute the local inverse inertia tensor
|
||||
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
|
||||
|
||||
// Update the world inverse inertia tensor
|
||||
updateInertiaTensorInverseWorld();
|
||||
|
||||
// Update the linear velocity of the center of mass
|
||||
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -83,6 +83,9 @@ class RigidBody : public CollisionBody {
|
|||
/// Inverse of the inertia tensor of the body
|
||||
Matrix3x3 mInertiaTensorLocalInverse;
|
||||
|
||||
/// Inverse of the world inertia tensor of the body
|
||||
Matrix3x3 mInertiaTensorInverseWorld;
|
||||
|
||||
/// Inverse of the mass of the body
|
||||
decimal mMassInverse;
|
||||
|
||||
|
|
@ -112,6 +115,9 @@ class RigidBody : public CollisionBody {
|
|||
/// Update the broad-phase state for this body (because it has moved for instance)
|
||||
virtual void updateBroadPhaseState() const override;
|
||||
|
||||
/// Update the world inverse inertia tensor of the body
|
||||
void updateInertiaTensorInverseWorld();
|
||||
|
||||
public :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
|
@ -291,12 +297,19 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() 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
|
||||
return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse *
|
||||
mTransform.getOrientation().getMatrix().getTranspose();
|
||||
return mInertiaTensorInverseWorld;
|
||||
}
|
||||
|
||||
// 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
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ using namespace std;
|
|||
// Constants initialization
|
||||
const decimal ContactSolver::BETA = 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
|
||||
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
|
||||
uint nbPenetrationConstraints = nbContactManifolds * MAX_CONTACT_POINTS_IN_MANIFOLD;
|
||||
mPenetrationConstraints = static_cast<PenetrationConstraint*>(mSingleFrameAllocator.allocate(sizeof(PenetrationConstraint) * nbPenetrationConstraints));
|
||||
//mPenetrationConstraints = new PenetrationConstraint[nbPenetrationConstraints];
|
||||
assert(mPenetrationConstraints != nullptr);
|
||||
//mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4];
|
||||
|
||||
mFrictionConstraints = static_cast<FrictionConstraint*>(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds));
|
||||
//mFrictionConstraints = new FrictionConstraint[nbContactManifolds];
|
||||
assert(mFrictionConstraints != nullptr);
|
||||
//mFrictionConstraints = new FrictionConstraint[mNbContactManifolds];
|
||||
|
||||
// For each island of the world
|
||||
for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
|
||||
|
|
@ -146,23 +142,17 @@ void ContactSolver::initializeForIsland(Island* island) {
|
|||
// contact manifold
|
||||
mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 = body1->mMassInverse;
|
||||
mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 = body2->mMassInverse;
|
||||
//internalManifold.nbContacts = externalManifold->getNbContactPoints();
|
||||
decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
|
||||
mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
|
||||
mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false;
|
||||
//internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
|
||||
//internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
|
||||
|
||||
bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
|
||||
bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
|
||||
|
||||
// If we solve the friction constraints at the center of the contact manifold
|
||||
//if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 = Vector3::zero();
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 = Vector3::zero();
|
||||
mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero();
|
||||
//}
|
||||
Vector3 frictionPointBody1;
|
||||
Vector3 frictionPointBody2;
|
||||
mFrictionConstraints[mNbFrictionConstraints].normal.setToZero();
|
||||
|
||||
// Compute the inverse K matrix for the rolling resistance constraint
|
||||
mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.setToZero();
|
||||
|
|
@ -186,7 +176,6 @@ void ContactSolver::initializeForIsland(Island* island) {
|
|||
mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 = I2;
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 = body1->mMassInverse;
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse;
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor = restitutionFactor;
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints;
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact;
|
||||
|
||||
|
|
@ -196,8 +185,6 @@ void ContactSolver::initializeForIsland(Island* island) {
|
|||
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].r1 = p1 - x1;
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].r2 = p2 - x2;
|
||||
|
||||
//mPenetrationConstraints[penConstIndex].externalContact = externalContact;
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal();
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth();
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact();
|
||||
|
|
@ -205,18 +192,10 @@ void ContactSolver::initializeForIsland(Island* island) {
|
|||
mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact;
|
||||
|
||||
externalContact->setIsRestingContact(true);
|
||||
//mPenetrationConstraints[penConstIndex].oldFrictionVector1 = externalContact->getFrictionVector1();
|
||||
//mPenetrationConstraints[penConstIndex].oldFrictionVector2 = externalContact->getFrictionVector2();
|
||||
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
|
||||
//if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 += p1;
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 += p2;
|
||||
//}
|
||||
frictionPointBody1 += p1;
|
||||
frictionPointBody2 += p2;
|
||||
|
||||
// Compute the velocity difference
|
||||
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;
|
||||
decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[mNbPenetrationConstraints].normal);
|
||||
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
|
||||
|
|
@ -251,77 +230,70 @@ void ContactSolver::initializeForIsland(Island* island) {
|
|||
// Initialize the split impulses to zero
|
||||
mPenetrationConstraints[mNbPenetrationConstraints].penetrationSplitImpulse = 0.0;
|
||||
|
||||
// If we solve the friction constraints at the center of the contact manifold
|
||||
//if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||
mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal;
|
||||
//}
|
||||
mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal;
|
||||
|
||||
mNbPenetrationConstraints++;
|
||||
nbContacts++;
|
||||
}
|
||||
|
||||
// If we solve the friction constraints at the center of the contact manifold
|
||||
//if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||
frictionPointBody1 /= nbContacts;
|
||||
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();
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 /= nbContacts;
|
||||
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
|
||||
if (mIsWarmStartingActive) {
|
||||
|
||||
// If warm starting is active
|
||||
if (mIsWarmStartingActive) {
|
||||
// Initialize the accumulated impulses with the previous step accumulated impulses
|
||||
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
|
||||
mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = externalManifold->getFrictionImpulse1();
|
||||
mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = externalManifold->getFrictionImpulse2();
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
|
||||
}
|
||||
else {
|
||||
// Initialize the accumulated impulses to zero
|
||||
mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = 0.0;
|
||||
mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = 0.0;
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = 0.0;
|
||||
mFrictionConstraints[mNbFrictionConstraints].rollingResistanceImpulse = Vector3(0, 0, 0);
|
||||
}
|
||||
|
||||
// Initialize the accumulated impulses to zero
|
||||
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();
|
||||
|
||||
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) -
|
||||
v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction);
|
||||
// Compute the friction vectors
|
||||
computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]);
|
||||
|
||||
// Compute the friction vectors
|
||||
computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]);
|
||||
|
||||
// Compute the inverse mass matrix K for the friction constraints at the center of the contact manifold
|
||||
mFrictionConstraints[mNbFrictionConstraints].r1CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
|
||||
mFrictionConstraints[mNbFrictionConstraints].r1CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
|
||||
mFrictionConstraints[mNbFrictionConstraints].r2CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
|
||||
mFrictionConstraints[mNbFrictionConstraints].r2CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
|
||||
decimal friction1Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
|
||||
((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionVector1) +
|
||||
((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
|
||||
decimal friction2Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
|
||||
((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionVector2) +
|
||||
((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
|
||||
decimal frictionTwistMass = mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 *
|
||||
mFrictionConstraints[mNbFrictionConstraints].normal) +
|
||||
mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 *
|
||||
mFrictionConstraints[mNbFrictionConstraints].normal);
|
||||
friction1Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction1Mass = decimal(1.0)/friction1Mass
|
||||
: decimal(0.0);
|
||||
friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass
|
||||
: decimal(0.0);
|
||||
frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) /
|
||||
frictionTwistMass :
|
||||
decimal(0.0);
|
||||
// Compute the inverse mass matrix K for the friction constraints at the center of the contact manifold
|
||||
mFrictionConstraints[mNbFrictionConstraints].r1CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
|
||||
mFrictionConstraints[mNbFrictionConstraints].r1CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
|
||||
mFrictionConstraints[mNbFrictionConstraints].r2CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
|
||||
mFrictionConstraints[mNbFrictionConstraints].r2CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
|
||||
decimal friction1Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
|
||||
((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionVector1) +
|
||||
((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
|
||||
decimal friction2Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
|
||||
((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionVector2) +
|
||||
((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
|
||||
mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
|
||||
decimal frictionTwistMass = mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 *
|
||||
mFrictionConstraints[mNbFrictionConstraints].normal) +
|
||||
mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 *
|
||||
mFrictionConstraints[mNbFrictionConstraints].normal);
|
||||
friction1Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction1Mass = decimal(1.0)/friction1Mass
|
||||
: decimal(0.0);
|
||||
friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass
|
||||
: decimal(0.0);
|
||||
frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) /
|
||||
frictionTwistMass :
|
||||
decimal(0.0);
|
||||
|
||||
mNbFrictionConstraints++;
|
||||
}
|
||||
|
|
@ -463,9 +435,6 @@ void 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
|
||||
|
||||
decimal deltaLambda;
|
||||
|
|
@ -550,9 +519,6 @@ void ContactSolver::solvePenetrationConstraints() {
|
|||
// Solve the friction constraints
|
||||
void ContactSolver::solveFrictionConstraints() {
|
||||
|
||||
// TODO : Check that the FrictionConstraint struct only contains variables that are
|
||||
// used in this method, nothing more
|
||||
|
||||
PROFILE("ContactSolver::solveFrictionConstraints()");
|
||||
|
||||
for (uint i=0; i<mNbFrictionConstraints; i++) {
|
||||
|
|
@ -698,17 +664,6 @@ void ContactSolver::storeImpulses() {
|
|||
mFrictionConstraints[i].contactManifold->setFrictionVector1(mFrictionConstraints[i].frictionVector1);
|
||||
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
|
||||
|
|
|
|||
|
|
@ -115,8 +115,6 @@ class ContactSolver {
|
|||
|
||||
struct PenetrationConstraint {
|
||||
|
||||
// TODO : Pack bools into a single value
|
||||
|
||||
/// Index of body 1 in the constraint solver
|
||||
uint indexBody1;
|
||||
|
||||
|
|
@ -144,9 +142,6 @@ class ContactSolver {
|
|||
/// Velocity restitution bias
|
||||
decimal restitutionBias;
|
||||
|
||||
/// Mix of the restitution factor for two bodies
|
||||
decimal restitutionFactor;
|
||||
|
||||
/// Accumulated normal impulse
|
||||
decimal penetrationImpulse;
|
||||
|
||||
|
|
@ -180,8 +175,6 @@ class ContactSolver {
|
|||
|
||||
struct FrictionConstraint {
|
||||
|
||||
// TODO : Pack bools into a single value
|
||||
|
||||
/// Index of body 1 in the constraint solver
|
||||
uint indexBody1;
|
||||
|
||||
|
|
@ -194,12 +187,6 @@ class ContactSolver {
|
|||
/// R2 vector for the friction constraints
|
||||
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
|
||||
Vector3 normal;
|
||||
|
||||
|
|
|
|||
|
|
@ -212,6 +212,9 @@ void DynamicsWorld::updateBodiesState() {
|
|||
// Update the transform of the body (using the new center of mass and new orientation)
|
||||
bodies[b]->updateTransformWithCenterOfMass();
|
||||
|
||||
// Update the world inverse inertia tensor of the body
|
||||
bodies[b]->updateInertiaTensorInverseWorld();
|
||||
|
||||
// Update the broad-phase state of the body
|
||||
bodies[b]->updateBroadPhaseState();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user