Optimizations in contact solver
This commit is contained in:
Daniel Chappuis
parent
9d761291d6
commit
cf42e9f04c
|
|
@ -163,22 +163,48 @@ void ContactSolver::initializeForIsland(Island* island) {
|
|||
new (mContactPoints + mNbContactPoints) ContactPointSolver();
|
||||
mContactPoints[mNbContactPoints].externalContact = externalContact;
|
||||
mContactPoints[mNbContactPoints].normal = externalContact->getNormal();
|
||||
mContactPoints[mNbContactPoints].r1 = p1 - x1;
|
||||
mContactPoints[mNbContactPoints].r2 = p2 - x2;
|
||||
mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x;
|
||||
mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y;
|
||||
mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z;
|
||||
mContactPoints[mNbContactPoints].r2.x = p2.x - x2.x;
|
||||
mContactPoints[mNbContactPoints].r2.y = p2.y - x2.y;
|
||||
mContactPoints[mNbContactPoints].r2.z = p2.z - x2.z;
|
||||
mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth();
|
||||
mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact();
|
||||
externalContact->setIsRestingContact(true);
|
||||
mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse();
|
||||
mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
|
||||
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1;
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2;
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody1.x += p1.x;
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody1.y += p1.y;
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody1.z += p1.z;
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody2.x += p2.x;
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody2.y += p2.y;
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody2.z += p2.z;
|
||||
|
||||
// Compute the velocity difference
|
||||
Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1);
|
||||
//deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1);
|
||||
Vector3 deltaV(v2.x + w2.y * mContactPoints[mNbContactPoints].r2.z - w2.z * mContactPoints[mNbContactPoints].r2.y
|
||||
- v1.x - w1.y * mContactPoints[mNbContactPoints].r1.z - w1.z * mContactPoints[mNbContactPoints].r1.y,
|
||||
v2.y + w2.z * mContactPoints[mNbContactPoints].r2.x - w2.x * mContactPoints[mNbContactPoints].r2.z
|
||||
- v1.y - w1.z * mContactPoints[mNbContactPoints].r1.x - w1.x * mContactPoints[mNbContactPoints].r1.z,
|
||||
v2.z + w2.x * mContactPoints[mNbContactPoints].r2.y - w2.y * mContactPoints[mNbContactPoints].r2.x
|
||||
- v1.z - w1.x * mContactPoints[mNbContactPoints].r1.y - w1.y * mContactPoints[mNbContactPoints].r1.x);
|
||||
|
||||
Vector3 r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
|
||||
Vector3 r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
|
||||
// r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
|
||||
Vector3 r1CrossN(mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.z -
|
||||
mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.y,
|
||||
mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.x -
|
||||
mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.z,
|
||||
mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.y -
|
||||
mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.x);
|
||||
// r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
|
||||
Vector3 r2CrossN(mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.z -
|
||||
mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.y,
|
||||
mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.x -
|
||||
mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.z,
|
||||
mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.y -
|
||||
mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.x);
|
||||
|
||||
mContactPoints[mNbContactPoints].i1TimesR1CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * r1CrossN;
|
||||
mContactPoints[mNbContactPoints].i2TimesR2CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * r2CrossN;
|
||||
|
|
@ -194,13 +220,18 @@ void ContactSolver::initializeForIsland(Island* island) {
|
|||
// at the beginning of the contact. Note that if it is a resting contact (normal
|
||||
// velocity bellow a given threshold), we do not add a restitution velocity bias
|
||||
mContactPoints[mNbContactPoints].restitutionBias = 0.0;
|
||||
decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
|
||||
// deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
|
||||
decimal deltaVDotN = deltaV.x * mContactPoints[mNbContactPoints].normal.x +
|
||||
deltaV.y * mContactPoints[mNbContactPoints].normal.y +
|
||||
deltaV.z * mContactPoints[mNbContactPoints].normal.z;
|
||||
const decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
|
||||
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
|
||||
mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN;
|
||||
}
|
||||
|
||||
mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal;
|
||||
mContactConstraints[mNbContactManifolds].normal.x += mContactPoints[mNbContactPoints].normal.x;
|
||||
mContactConstraints[mNbContactManifolds].normal.y += mContactPoints[mNbContactPoints].normal.y;
|
||||
mContactConstraints[mNbContactManifolds].normal.z += mContactPoints[mNbContactPoints].normal.z;
|
||||
|
||||
mNbContactPoints++;
|
||||
|
||||
|
|
@ -209,8 +240,12 @@ void ContactSolver::initializeForIsland(Island* island) {
|
|||
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
|
||||
mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
|
||||
mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1;
|
||||
mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2;
|
||||
mContactConstraints[mNbContactManifolds].r1Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody1.x - x1.x;
|
||||
mContactConstraints[mNbContactManifolds].r1Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody1.y - x1.y;
|
||||
mContactConstraints[mNbContactManifolds].r1Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody1.z - x1.z;
|
||||
mContactConstraints[mNbContactManifolds].r2Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody2.x - x2.x;
|
||||
mContactConstraints[mNbContactManifolds].r2Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody2.y - x2.y;
|
||||
mContactConstraints[mNbContactManifolds].r2Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody2.z - x2.z;
|
||||
mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1();
|
||||
mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2();
|
||||
|
||||
|
|
@ -230,8 +265,20 @@ void ContactSolver::initializeForIsland(Island* island) {
|
|||
|
||||
mContactConstraints[mNbContactManifolds].normal.normalize();
|
||||
|
||||
Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) -
|
||||
v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction);
|
||||
// deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) -
|
||||
// v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction);
|
||||
Vector3 deltaVFrictionPoint(v2.x + w2.y * mContactConstraints[mNbContactManifolds].r2Friction.z -
|
||||
w2.z * mContactConstraints[mNbContactManifolds].r2Friction.y -
|
||||
v1.x - w1.y * mContactConstraints[mNbContactManifolds].r1Friction.z -
|
||||
w1.z * mContactConstraints[mNbContactManifolds].r1Friction.y,
|
||||
v2.y + w2.z * mContactConstraints[mNbContactManifolds].r2Friction.x -
|
||||
w2.x * mContactConstraints[mNbContactManifolds].r2Friction.z -
|
||||
v1.y - w1.z * mContactConstraints[mNbContactManifolds].r1Friction.x -
|
||||
w1.x * mContactConstraints[mNbContactManifolds].r1Friction.z,
|
||||
v2.z + w2.x * mContactConstraints[mNbContactManifolds].r2Friction.y -
|
||||
w2.y * mContactConstraints[mNbContactManifolds].r2Friction.x -
|
||||
v1.z - w1.x * mContactConstraints[mNbContactManifolds].r1Friction.y -
|
||||
w1.y * mContactConstraints[mNbContactManifolds].r1Friction.x);
|
||||
|
||||
// Compute the friction vectors
|
||||
computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]);
|
||||
|
|
@ -289,13 +336,25 @@ void ContactSolver::warmStart() {
|
|||
// --------- Penetration --------- //
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * impulsePenetration;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse,
|
||||
mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse,
|
||||
mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse);
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
|
||||
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
|
||||
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
}
|
||||
else { // If it is a new contact point
|
||||
|
||||
|
|
@ -312,20 +371,27 @@ void ContactSolver::warmStart() {
|
|||
|
||||
// Project the old friction impulses (with old friction vectors) into the new friction
|
||||
// vectors to get the new friction impulses
|
||||
Vector3 oldFrictionImpulse = mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1 +
|
||||
mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2;
|
||||
Vector3 oldFrictionImpulse(mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.x +
|
||||
mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.x,
|
||||
mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.y +
|
||||
mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.y,
|
||||
mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.z +
|
||||
mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.z);
|
||||
mContactConstraints[c].friction1Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector1);
|
||||
mContactConstraints[c].friction2Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector2);
|
||||
|
||||
// ------ First friction constraint at the center of the contact manifold ------ //
|
||||
|
||||
// Compute the impulse P = J^T * lambda
|
||||
Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 *
|
||||
mContactConstraints[c].friction1Impulse;
|
||||
Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 *
|
||||
mContactConstraints[c].friction1Impulse;
|
||||
Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 *
|
||||
mContactConstraints[c].friction1Impulse;
|
||||
Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * mContactConstraints[c].friction1Impulse,
|
||||
-mContactConstraints[c].r1CrossT1.y * mContactConstraints[c].friction1Impulse,
|
||||
-mContactConstraints[c].r1CrossT1.z * mContactConstraints[c].friction1Impulse);
|
||||
Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * mContactConstraints[c].friction1Impulse,
|
||||
mContactConstraints[c].frictionVector1.y * mContactConstraints[c].friction1Impulse,
|
||||
mContactConstraints[c].frictionVector1.z * mContactConstraints[c].friction1Impulse);
|
||||
Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * mContactConstraints[c].friction1Impulse,
|
||||
mContactConstraints[c].r2CrossT1.y * mContactConstraints[c].friction1Impulse,
|
||||
mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse);
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
|
||||
|
|
@ -338,23 +404,40 @@ void ContactSolver::warmStart() {
|
|||
// ------ Second friction constraint at the center of the contact manifold ----- //
|
||||
|
||||
// Compute the impulse P = J^T * lambda
|
||||
angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * mContactConstraints[c].friction2Impulse;
|
||||
linearImpulseBody2 = mContactConstraints[c].frictionVector2 * mContactConstraints[c].friction2Impulse;
|
||||
angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse;
|
||||
angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * mContactConstraints[c].friction2Impulse;
|
||||
angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * mContactConstraints[c].friction2Impulse;
|
||||
angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * mContactConstraints[c].friction2Impulse;
|
||||
linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * mContactConstraints[c].friction2Impulse;
|
||||
linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * mContactConstraints[c].friction2Impulse;
|
||||
linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * mContactConstraints[c].friction2Impulse;
|
||||
angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * mContactConstraints[c].friction2Impulse;
|
||||
angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * mContactConstraints[c].friction2Impulse;
|
||||
angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
|
||||
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
|
||||
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
|
||||
// ------ Twist friction constraint at the center of the contact manifold ------ //
|
||||
|
||||
// Compute the impulse P = J^T * lambda
|
||||
angularImpulseBody1 = -mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
|
||||
angularImpulseBody2 = mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
|
||||
angularImpulseBody1.x = -mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse;
|
||||
angularImpulseBody1.y = -mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse;
|
||||
angularImpulseBody1.z = -mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
|
||||
|
||||
angularImpulseBody2.x = mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse;
|
||||
angularImpulseBody2.y = mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse;
|
||||
angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
|
|
@ -409,8 +492,15 @@ void ContactSolver::solve() {
|
|||
// --------- Penetration --------- //
|
||||
|
||||
// Compute J*v
|
||||
Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
|
||||
decimal deltaVDotN = deltaV.dot(mContactPoints[contactPointIndex].normal);
|
||||
//Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
|
||||
Vector3 deltaV(v2.x + w2.y * mContactPoints[contactPointIndex].r2.z - w2.z * mContactPoints[contactPointIndex].r2.y - v1.x -
|
||||
w1.y * mContactPoints[contactPointIndex].r1.z + w1.z * mContactPoints[contactPointIndex].r1.y,
|
||||
v2.y + w2.z * mContactPoints[contactPointIndex].r2.x - w2.x * mContactPoints[contactPointIndex].r2.z - v1.y -
|
||||
w1.z * mContactPoints[contactPointIndex].r1.x + w1.x * mContactPoints[contactPointIndex].r1.z,
|
||||
v2.z + w2.x * mContactPoints[contactPointIndex].r2.y - w2.y * mContactPoints[contactPointIndex].r2.x - v1.z -
|
||||
w1.x * mContactPoints[contactPointIndex].r1.y + w1.y * mContactPoints[contactPointIndex].r1.x);
|
||||
decimal deltaVDotN = deltaV.x * mContactPoints[contactPointIndex].normal.x + deltaV.y * mContactPoints[contactPointIndex].normal.y +
|
||||
deltaV.z * mContactPoints[contactPointIndex].normal.z;
|
||||
decimal Jv = deltaVDotN;
|
||||
|
||||
// Compute the bias "b" of the constraint
|
||||
|
|
@ -433,15 +523,27 @@ void ContactSolver::solve() {
|
|||
deltaLambda, decimal(0.0));
|
||||
deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp;
|
||||
|
||||
Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda;
|
||||
Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambda,
|
||||
mContactPoints[contactPointIndex].normal.y * deltaLambda,
|
||||
mContactPoints[contactPointIndex].normal.z * deltaLambda);
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambda;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
|
||||
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambda;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
|
||||
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
|
||||
|
||||
sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
|
||||
|
||||
|
|
@ -453,9 +555,17 @@ void ContactSolver::solve() {
|
|||
const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1];
|
||||
const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2];
|
||||
const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2];
|
||||
Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) -
|
||||
v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
|
||||
decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal);
|
||||
|
||||
//Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
|
||||
Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x -
|
||||
w1Split.y * mContactPoints[contactPointIndex].r1.z + w1Split.z * mContactPoints[contactPointIndex].r1.y,
|
||||
v2Split.y + w2Split.z * mContactPoints[contactPointIndex].r2.x - w2Split.x * mContactPoints[contactPointIndex].r2.z - v1Split.y -
|
||||
w1Split.z * mContactPoints[contactPointIndex].r1.x + w1Split.x * mContactPoints[contactPointIndex].r1.z,
|
||||
v2Split.z + w2Split.x * mContactPoints[contactPointIndex].r2.y - w2Split.y * mContactPoints[contactPointIndex].r2.x - v1Split.z -
|
||||
w1Split.x * mContactPoints[contactPointIndex].r1.y + w1Split.y * mContactPoints[contactPointIndex].r1.x);
|
||||
decimal JvSplit = deltaVSplit.x * mContactPoints[contactPointIndex].normal.x +
|
||||
deltaVSplit.y * mContactPoints[contactPointIndex].normal.y +
|
||||
deltaVSplit.z * mContactPoints[contactPointIndex].normal.z;
|
||||
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
|
||||
mContactPoints[contactPointIndex].inversePenetrationMass;
|
||||
decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse;
|
||||
|
|
@ -464,15 +574,27 @@ void ContactSolver::solve() {
|
|||
deltaLambdaSplit, decimal(0.0));
|
||||
deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit;
|
||||
|
||||
Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit;
|
||||
Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambdaSplit,
|
||||
mContactPoints[contactPointIndex].normal.y * deltaLambdaSplit,
|
||||
mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit);
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mSplitLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse;
|
||||
mSplitAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambdaSplit;
|
||||
mSplitLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
|
||||
mSplitLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
|
||||
mSplitLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
|
||||
|
||||
mSplitAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
|
||||
mSplitAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
|
||||
mSplitAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
|
||||
mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambdaSplit;
|
||||
mSplitLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
|
||||
mSplitLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
|
||||
mSplitLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
|
||||
|
||||
mSplitAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
|
||||
mSplitAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
|
||||
mSplitAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
|
||||
}
|
||||
|
||||
contactPointIndex++;
|
||||
|
|
@ -481,9 +603,16 @@ void ContactSolver::solve() {
|
|||
// ------ First friction constraint at the center of the contact manifol ------ //
|
||||
|
||||
// Compute J*v
|
||||
Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction)
|
||||
- v1 - w1.cross(mContactConstraints[c].r1Friction);
|
||||
decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1);
|
||||
// deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
|
||||
Vector3 deltaV(v2.x + w2.y * mContactConstraints[c].r2Friction.z - w2.z * mContactConstraints[c].r2Friction.y - v1.x -
|
||||
w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y,
|
||||
v2.y + w2.z * mContactConstraints[c].r2Friction.x - w2.x * mContactConstraints[c].r2Friction.z - v1.y -
|
||||
w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z,
|
||||
v2.z + w2.x * mContactConstraints[c].r2Friction.y - w2.y * mContactConstraints[c].r2Friction.x - v1.z -
|
||||
w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x);
|
||||
decimal Jv = deltaV.x * mContactConstraints[c].frictionVector1.x +
|
||||
deltaV.y * mContactConstraints[c].frictionVector1.y +
|
||||
deltaV.z * mContactConstraints[c].frictionVector1.z;
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass;
|
||||
|
|
@ -495,23 +624,42 @@ void ContactSolver::solve() {
|
|||
deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda;
|
||||
Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda;
|
||||
Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda;
|
||||
Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * deltaLambda,
|
||||
-mContactConstraints[c].r1CrossT1.y * deltaLambda,
|
||||
-mContactConstraints[c].r1CrossT1.z * deltaLambda);
|
||||
Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * deltaLambda,
|
||||
mContactConstraints[c].frictionVector1.y * deltaLambda,
|
||||
mContactConstraints[c].frictionVector1.z * deltaLambda);
|
||||
Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * deltaLambda,
|
||||
mContactConstraints[c].r2CrossT1.y * deltaLambda,
|
||||
mContactConstraints[c].r2CrossT1.z * deltaLambda);
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
|
||||
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
|
||||
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
|
||||
// ------ Second friction constraint at the center of the contact manifol ----- //
|
||||
|
||||
// Compute J*v
|
||||
deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
|
||||
Jv = deltaV.dot(mContactConstraints[c].frictionVector2);
|
||||
//deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
|
||||
deltaV.x = v2.x + w2.y * mContactConstraints[c].r2Friction.z - v2.z * mContactConstraints[c].r2Friction.y - v1.x -
|
||||
w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y;
|
||||
deltaV.y = v2.y + w2.z * mContactConstraints[c].r2Friction.x - v2.x * mContactConstraints[c].r2Friction.z - v1.y -
|
||||
w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z;
|
||||
deltaV.z = v2.z + w2.x * mContactConstraints[c].r2Friction.y - v2.y * mContactConstraints[c].r2Friction.x - v1.z -
|
||||
w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x;
|
||||
Jv = deltaV.x * mContactConstraints[c].frictionVector2.x + deltaV.y * mContactConstraints[c].frictionVector2.y +
|
||||
deltaV.z * mContactConstraints[c].frictionVector2.z;
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass;
|
||||
|
|
@ -523,23 +671,36 @@ void ContactSolver::solve() {
|
|||
deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda;
|
||||
linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda;
|
||||
angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda;
|
||||
angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * deltaLambda;
|
||||
angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * deltaLambda;
|
||||
angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * deltaLambda;
|
||||
|
||||
linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * deltaLambda;
|
||||
linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * deltaLambda;
|
||||
linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * deltaLambda;
|
||||
|
||||
angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * deltaLambda;
|
||||
angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * deltaLambda;
|
||||
angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
|
||||
mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
|
||||
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
|
||||
// ------ Twist friction constraint at the center of the contact manifol ------ //
|
||||
|
||||
// Compute J*v
|
||||
deltaV = w2 - w1;
|
||||
Jv = deltaV.dot(mContactConstraints[c].normal);
|
||||
Jv = deltaV.x * mContactConstraints[c].normal.x + deltaV.y * mContactConstraints[c].normal.y +
|
||||
deltaV.z * mContactConstraints[c].normal.z;
|
||||
|
||||
deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass);
|
||||
frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
|
||||
|
|
@ -550,7 +711,9 @@ void ContactSolver::solve() {
|
|||
deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda;
|
||||
angularImpulseBody2.x = mContactConstraints[c].normal.x * deltaLambda;
|
||||
angularImpulseBody2.y = mContactConstraints[c].normal.y * deltaLambda;
|
||||
angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
|
||||
|
|
@ -619,8 +782,11 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
|
|||
assert(contact.normal.length() > decimal(0.0));
|
||||
|
||||
// Compute the velocity difference vector in the tangential plane
|
||||
Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
|
||||
Vector3 tangentVelocity = deltaVelocity - normalVelocity;
|
||||
Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x,
|
||||
deltaVelocity.y * contact.normal.y * contact.normal.y,
|
||||
deltaVelocity.z * contact.normal.z * contact.normal.z);
|
||||
Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y,
|
||||
deltaVelocity.z - normalVelocity.z);
|
||||
|
||||
// If the velocty difference in the tangential plane is not zero
|
||||
decimal lengthTangenVelocity = tangentVelocity.length();
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user