///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration public static void ResolveSingleBilateral(RigidBody body1, ref Vector3 pos1, RigidBody body2, ref Vector3 pos2, float distance, ref Vector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(System.Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0f; return; } Vector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); Vector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations Vector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); Vector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2); Vector3 vel = vel1 - vel2; Matrix m1 = MathUtil.TransposeBasis(body1.GetCenterOfMassTransform()); Matrix m2 = MathUtil.TransposeBasis(body2.GetCenterOfMassTransform()); 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(),Vector3.TransformNormal(body1.GetAngularVelocity(),m1), body2.GetLinearVelocity(),Vector3.TransformNormal(body2.GetAngularVelocity(),m2)); float a = jacDiagABInv; rel_vel = Vector3.Dot(normal,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; } }
protected virtual void BuildAngularJacobian(ref JacobianEntry jacAngular, ref Vector3 jointAxisW) { jacAngular = new JacobianEntry(jointAxisW, MathUtil.TransposeBasis(m_rbA.GetCenterOfMassTransform()), MathUtil.TransposeBasis(m_rbB.GetCenterOfMassTransform()), m_rbA.GetInvInertiaDiagLocal(), m_rbB.GetInvInertiaDiagLocal()); }
protected virtual void BuildLinearJacobian( ref JacobianEntry jacLinear, ref Vector3 normalWorld, ref Vector3 pivotAInW, ref Vector3 pivotBInW) { jacLinear = new JacobianEntry( MathUtil.TransposeBasis(m_rbA.GetCenterOfMassTransform()), MathUtil.TransposeBasis(m_rbB.GetCenterOfMassTransform()), pivotAInW - m_rbA.GetCenterOfMassPosition(), pivotBInW - m_rbB.GetCenterOfMassPosition(), normalWorld, m_rbA.GetInvInertiaDiagLocal(), m_rbA.GetInvMass(), m_rbB.GetInvInertiaDiagLocal(), m_rbB.GetInvMass()); }
// // solve unilateral raint (equality, direct method) // public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref Matrix world2A, ref Matrix world2B, ref Vector3 invInertiaADiag, float invMassA, ref Vector3 linvelA, ref Vector3 angvelA, ref Vector3 rel_posA1, ref Vector3 invInertiaBDiag, float invMassB, ref Vector3 linvelB, ref Vector3 angvelB, ref Vector3 rel_posA2, float depthA, ref Vector3 normalA, ref Vector3 rel_posB1, ref Vector3 rel_posB2, float depthB, ref Vector3 normalB, ref float imp0, ref float imp1) { //(void)linvelA; //(void)linvelB; //(void)angvelB; //(void)angvelA; imp0 = 0f; imp1 = 0f; float len = System.Math.Abs(normalA.Length()) - 1f; if (System.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 = Vector3.Dot(normalA,(body0.GetVelocityInLocalPoint(ref rel_posA1)-body1.GetVelocityInLocalPoint(ref rel_posA1))); float vel1 = Vector3.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] }
// 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; Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; Vector3 lin0 = massInvA * lin ; Vector3 lin1 = massInvB * lin; Vector3 sum = ang0+ang1+lin0+lin1; return sum.X+sum.Y+sum.Z; }
// for two constraints on the same rigidbody (for example vehicle friction) public float GetNonDiagonal(JacobianEntry jacB, float massInvA) { JacobianEntry jacA = this; float lin = massInvA * Vector3.Dot(jacA.m_linearJointAxis,jacB.m_linearJointAxis); float ang = Vector3.Dot(jacA.m_0MinvJt,jacB.m_aJ); return lin + ang; }