// for two constraints on the same rigidbody (for example vehicle friction) public float GetNonDiagonal(JacobianEntry jacB, float massInvA) { JacobianEntry jacA = this; float lin = massInvA * IndexedVector3.Dot(jacA.m_linearJointAxis, jacB.m_linearJointAxis); float ang = IndexedVector3.Dot(jacA.m_0MinvJt, jacB.m_aJ); return(lin + ang); }
///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1, RigidBody body2, ref IndexedVector3 pos2, float distance, ref IndexedVector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0f; return; } IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2); IndexedVector3 vel = vel1 - vel2; IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose(); IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose(); JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal, body1.GetInvInertiaDiagLocal(), body1.GetInvMass(), body2.GetInvInertiaDiagLocal(), body2.GetInvMass()); float jacDiagAB = jac.GetDiagonal(); float jacDiagABInv = 1f / jacDiagAB; float rel_vel = jac.GetRelativeVelocity( body1.GetLinearVelocity(), body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(), body2.GetLinearVelocity(), body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity()); float a = jacDiagABInv; rel_vel = normal.Dot(ref vel); //todo: move this into proper structure float contactDamping = 0.2f; if (ONLY_USE_LINEAR_MASS) { float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass()); impulse = -contactDamping * rel_vel * massTerm; } else { float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; } }
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB) { JacobianEntry jacA = this; IndexedVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; IndexedVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; IndexedVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; IndexedVector3 lin0 = massInvA * lin; IndexedVector3 lin1 = massInvB * lin; IndexedVector3 sum = ang0 + ang1 + lin0 + lin1; return(sum.X + sum.Y + sum.Z); }
// // solve unilateral raint (equality, direct method) // public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref IndexedBasisMatrix world2A, ref IndexedBasisMatrix world2B, ref IndexedVector3 invInertiaADiag, float invMassA, ref IndexedVector3 linvelA, ref IndexedVector3 angvelA, ref IndexedVector3 rel_posA1, ref IndexedVector3 invInertiaBDiag, float invMassB, ref IndexedVector3 linvelB, ref IndexedVector3 angvelB, ref IndexedVector3 rel_posA2, float depthA, ref IndexedVector3 normalA, ref IndexedVector3 rel_posB1, ref IndexedVector3 rel_posB2, float depthB, ref IndexedVector3 normalB, out float imp0, out float imp1) { //(void)linvelA; //(void)linvelB; //(void)angvelB; //(void)angvelA; imp0 = 0f; imp1 = 0f; float len = Math.Abs(normalA.Length()) - 1f; if (Math.Abs(len) >= MathUtil.SIMD_EPSILON) { return; } Debug.Assert(len < MathUtil.SIMD_EPSILON); //this jacobian entry could be re-used for all iterations JacobianEntry jacA = new JacobianEntry(ref world2A, ref world2B, ref rel_posA1, ref rel_posA2, ref normalA, ref invInertiaADiag, invMassA, ref invInertiaBDiag, invMassB); JacobianEntry jacB = new JacobianEntry(ref world2A, ref world2B, ref rel_posB1, ref rel_posB2, ref normalB, ref invInertiaADiag, invMassA, ref invInertiaBDiag, invMassB); // float vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); // float vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); float vel0 = IndexedVector3.Dot(normalA, (body0.GetVelocityInLocalPoint(ref rel_posA1) - body1.GetVelocityInLocalPoint(ref rel_posA1))); float vel1 = IndexedVector3.Dot(normalB, (body0.GetVelocityInLocalPoint(ref rel_posB1) - body1.GetVelocityInLocalPoint(ref rel_posB1))); // float penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv float massTerm = 1f / (invMassA + invMassB); // calculate rhs (or error) terms float dv0 = depthA * m_tau * massTerm - vel0 * m_damping; float dv1 = depthB * m_tau * massTerm - vel1 * m_damping; // dC/dv * dv = -C // jacobian * impulse = -error // //impulse = jacobianInverse * -error // inverting 2x2 symmetric system (offdiagonal are equal!) // float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); float invDet = 1f / (jacA.GetDiagonal() * jacB.GetDiagonal() - nonDiag * nonDiag); //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; imp0 = dv0 * jacA.GetDiagonal() * invDet + dv1 * -nonDiag * invDet; imp1 = dv1 * jacB.GetDiagonal() * invDet + dv0 * -nonDiag * invDet; //[a b] [d -c] //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) //[jA nD] * [imp0] = [dv0] //[nD jB] [imp1] [dv1] }