///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; } }
public void UpdateWheel(RigidBody chassis, ref WheelRaycastInfo raycastInfo) { if (m_raycastInfo.m_isInContact) { float project= Vector3.Dot(m_raycastInfo.m_contactNormalWS,m_raycastInfo.m_wheelDirectionWS ); Vector3 chassis_velocity_at_contactPoint; Vector3 relpos = m_raycastInfo.m_contactPointWS - chassis.GetCenterOfMassPosition(); chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint( ref relpos ); float projVel = Vector3.Dot(m_raycastInfo.m_contactNormalWS,chassis_velocity_at_contactPoint ); if ( project >= -0.1f) { m_suspensionRelativeVelocity = 0f; m_clippedInvContactDotSuspension = 1.0f / 0.1f; } else { float inv = -1f / project; m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; } } else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = this.GetSuspensionRestLength(); m_suspensionRelativeVelocity = 0f; m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; m_clippedInvContactDotSuspension = 1f; } }
// // 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] }