// for two constraints on the same rigidbody (for example vehicle friction) public float GetNonDiagonal(JacobianEntry jacB, float massInvA) { float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis); float ang = Vector3.Dot(_0MinvJt, jacB._aJ); return(lin + ang); }
public override void BuildJacobian() { AppliedImpulse = 0f; Vector3 normal = new Vector3(); if (!_angularOnly) { for (int i = 0; i < 3; i++) { MathHelper.SetElement(ref normal, i, 1); _jac[i] = new JacobianEntry( MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, normal, RigidBodyA.InvInertiaDiagLocal, RigidBodyA.InverseMass, RigidBodyB.InvInertiaDiagLocal, RigidBodyB.InverseMass); MathHelper.SetElement(ref normal, i, 0); } } //calculate two perpendicular jointAxis, orthogonal to hingeAxis //these two jointAxis require equal angular velocities for both bodies //this is unused for now, it's a todo Vector3 jointAxisALocal = new Vector3(); Vector3 jointAxisBLocal = new Vector3(); MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal); Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform); Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform); Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); _jacAng[0] = new JacobianEntry(jointAxisA, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), RigidBodyA.InvInertiaDiagLocal, RigidBodyB.InvInertiaDiagLocal); _jacAng[1] = new JacobianEntry(jointAxisB, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), RigidBodyA.InvInertiaDiagLocal, RigidBodyB.InvInertiaDiagLocal); _jacAng[2] = new JacobianEntry(hingeAxisWorld, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), RigidBodyA.InvInertiaDiagLocal, RigidBodyB.InvInertiaDiagLocal); }
// 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) { Vector3 lin = _linearJointAxis * jacB._linearJointAxis; Vector3 ang0 = _0MinvJt * jacB._aJ; Vector3 ang1 = _1MinvJt * jacB._bJ; Vector3 lin0 = massInvA * lin; Vector3 lin1 = massInvB * lin; Vector3 sum = ang0 + ang1 + lin0 + lin1; return(sum.X + sum.Y + sum.Z); }
/// <summary> /// bilateral constraint between two dynamic objects /// positive distance = separation, negative distance = penetration /// </summary> /// <param name="body1"></param> /// <param name="pos1"></param> /// <param name="body2"></param> /// <param name="pos2"></param> /// <param name="distance"></param> /// <param name="normal"></param> /// <param name="impulse"></param> /// <param name="timeStep"></param> public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA, RigidBody bodyB, Vector3 posB, float distance, Vector3 normal, out float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); if (Math.Abs(normalLenSqr) >= 1.1f) { throw new BulletException(); } /*if (normalLenSqr > 1.1f) * { * impulse = 0f; * return; * }*/ Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition; Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition; //this jacobian entry could be re-used for all iterations Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform), Matrix.Transpose(bodyB.CenterOfMassTransform), rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass, bodyB.InvInertiaDiagLocal, bodyB.InverseMass); float jacDiagAB = jac.Diagonal; float jacDiagABInv = 1f / jacDiagAB; float rel_vel = jac.GetRelativeVelocity( bodyA.LinearVelocity, Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)), bodyB.LinearVelocity, Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform))); float a; a = jacDiagABInv; rel_vel = Vector3.Dot(normal, vel); float contactDamping = 0.2f; float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; }
// solve unilateral constraint (equality, direct method) public void ResolveUnilateralPairConstraint( RigidBody body1, RigidBody body2, Matrix world2A, Matrix world2B, Vector3 invInertiaADiag, float invMassA, Vector3 linvelA, Vector3 angvelA, Vector3 rel_posA1, Vector3 invInertiaBDiag, float invMassB, Vector3 linvelB, Vector3 angvelB, Vector3 rel_posA2, float depthA, Vector3 normalA, Vector3 rel_posB1, Vector3 rel_posB2, float depthB, Vector3 normalB, out float imp0, out float imp1) { imp0 = 0; imp1 = 0; float len = Math.Abs(normalA.Length()) - 1f; if (Math.Abs(len) >= float.Epsilon) { return; } BulletDebug.Assert(len < float.Epsilon); //this jacobian entry could be re-used for all iterations JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv float massTerm = 1f / (invMassA + invMassB); // calculate rhs (or error) terms float dv0 = depthA * _tau * massTerm - vel0 * _damping; float dv1 = depthB * _tau * massTerm - vel1 * _damping; float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; }
// solve unilateral constraint (equality, direct method) public void ResolveUnilateralPairConstraint( RigidBody body1, RigidBody body2, Matrix world2A, Matrix world2B, Vector3 invInertiaADiag, float invMassA, Vector3 linvelA, Vector3 angvelA, Vector3 rel_posA1, Vector3 invInertiaBDiag, float invMassB, Vector3 linvelB, Vector3 angvelB, Vector3 rel_posA2, float depthA, Vector3 normalA, Vector3 rel_posB1, Vector3 rel_posB2, float depthB, Vector3 normalB, out float imp0, out float imp1) { imp0 = 0; imp1 = 0; float len = Math.Abs(normalA.Length()) - 1f; if (Math.Abs(len) >= float.Epsilon) return; BulletDebug.Assert(len < float.Epsilon); //this jacobian entry could be re-used for all iterations JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv float massTerm = 1f / (invMassA + invMassB); // calculate rhs (or error) terms float dv0 = depthA * _tau * massTerm - vel0 * _damping; float dv1 = depthB * _tau * massTerm - vel1 * _damping; float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; }
public override void BuildJacobian() { Vector3 normal = new Vector3(); for (int i = 0; i < 3; i++) { MathHelper.SetElement(ref normal, i, 1); _jacobian[i] = new JacobianEntry( MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, normal, RigidBodyA.InvInertiaDiagLocal, RigidBodyA.InverseMass, RigidBodyB.InvInertiaDiagLocal, RigidBodyB.InverseMass ); MathHelper.SetElement(ref normal, i, 0); } }
public override void BuildJacobian() { Vector3 localNormalInA = new Vector3(0, 0, 0); Vector3 pivotInA = _frameInA.Translation; Vector3 pivotInB = _frameInB.Translation; Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; //linear part for (int i = 0; i < 3; i++) { if (IsLimited(i)) { if (i == 0) localNormalInA = new Vector3(1, 0, 0); else if (i == 1) localNormalInA = new Vector3(0, 1, 0); else localNormalInA = new Vector3(0, 0, 1); Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); // Create linear atom _jacLinear[i] = new JacobianEntry( MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, normalWorld, RigidBodyA.InvInertiaDiagLocal, RigidBodyA.InverseMass, RigidBodyB.InvInertiaDiagLocal, RigidBodyB.InverseMass); //optionally disable warmstarting _accumulatedImpulse[i] = 0f; // Apply accumulated impulse Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); } } // angular part for (int i = 0; i < 3; i++) { if (IsLimited(i + 3)) { Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); // Create angular atom _jacAng[i] = new JacobianEntry(axis, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), RigidBodyA.InvInertiaDiagLocal, RigidBodyB.InvInertiaDiagLocal); _accumulatedImpulse[i + 3] = 0f; // Apply accumulated impulse Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; RigidBodyA.ApplyTorqueImpulse(impulse_vector); RigidBodyB.ApplyTorqueImpulse(-impulse_vector); } } }
// for two constraints on the same rigidbody (for example vehicle friction) public float GetNonDiagonal(JacobianEntry jacB, float massInvA) { float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis); float ang = Vector3.Dot(_0MinvJt, jacB._aJ); return lin + ang; }
protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) { RigidBody body0 = manifold.BodyA as RigidBody; RigidBody body1 = manifold.BodyB as RigidBody; //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop { manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); int numpoints = manifold.ContactsCount; _totalContactPoints += numpoints; Vector3 color = new Vector3(0, 1, 0); for (int i = 0; i < numpoints; i++) { ManifoldPoint cp = manifold.GetContactPoint(i); if (cp.Distance <= 0) { Vector3 pos1 = cp.PositionWorldOnA; Vector3 pos2 = cp.PositionWorldOnB; Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; //this jacobian entry is re-used for all iterations JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), MatrixOperations.Transpose(body1.CenterOfMassTransform), rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, body1.InvInertiaDiagLocal, body1.InverseMass); float jacDiagAB = jac.Diagonal; ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; if (cpd != null) { //might be invalid cpd.PersistentLifeTime++; if (cpd.PersistentLifeTime != cp.LifeTime) { //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); cpd = new ConstraintPersistentData(); cpd.PersistentLifeTime = cp.LifeTime; } } else { cpd = new ConstraintPersistentData(); _totalCpd++; //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); cp.UserPersistentData = cpd; cpd.PersistentLifeTime = cp.LifeTime; //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); } if (cpd == null) throw new BulletException(); cpd.JacDiagABInv = 1f / jacDiagAB; //Dependent on Rigidbody A and B types, fetch the contact/friction response func //perhaps do a similar thing for friction/restutution combiner funcs... cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float rel_vel; rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); float combinedRestitution = cp.CombinedRestitution; cpd.Penetration = cp.Distance; cpd.Friction = cp.CombinedFriction; cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); if (cpd.Restitution < 0f) { cpd.Restitution = 0.0f; }; //restitution and penetration work in same direction so //rel_vel float penVel = -cpd.Penetration / info.TimeStep; if (cpd.Restitution > penVel) { cpd.Penetration = 0; } float relaxation = info.Damping; if ((_solverMode & SolverMode.UseWarmstarting) != 0) { cpd.AppliedImpulse *= relaxation; } else { cpd.AppliedImpulse = 0f; } //for friction cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; //re-calculate friction direction every frame, todo: check if this is really needed Vector3 fwta = cpd.FrictionWorldTangentialA; Vector3 fwtb = cpd.FrictionWorldTangentialB; MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); cpd.FrictionWorldTangentialA = fwta; cpd.FrictionWorldTangentialB = fwtb; cpd.AccumulatedTangentImpulseA = 0; cpd.AccumulatedTangentImpulseB = 0; float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); float denom = relaxation / (denom0 + denom1); cpd.JacDiagABInvTangentA = denom; denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); denom = relaxation / (denom0 + denom1); cpd.JacDiagABInvTangentB = denom; Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; { Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); } { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); } //apply previous frames impulse on both bodies body0.ApplyImpulse(totalImpulse, rel_pos1); body1.ApplyImpulse(-totalImpulse, rel_pos2); } } } }
public override void BuildJacobian() { Vector3 localNormalInA = new Vector3(0, 0, 0); Vector3 pivotInA = _frameInA.Translation; Vector3 pivotInB = _frameInB.Translation; Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; //linear part for (int i = 0; i < 3; i++) { if (IsLimited(i)) { if (i == 0) { localNormalInA = new Vector3(1, 0, 0); } else if (i == 1) { localNormalInA = new Vector3(0, 1, 0); } else { localNormalInA = new Vector3(0, 0, 1); } Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); // Create linear atom _jacLinear[i] = new JacobianEntry( MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, normalWorld, RigidBodyA.InvInertiaDiagLocal, RigidBodyA.InverseMass, RigidBodyB.InvInertiaDiagLocal, RigidBodyB.InverseMass); //optionally disable warmstarting _accumulatedImpulse[i] = 0f; // Apply accumulated impulse Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); } } // angular part for (int i = 0; i < 3; i++) { if (IsLimited(i + 3)) { Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); // Create angular atom _jacAng[i] = new JacobianEntry(axis, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), RigidBodyA.InvInertiaDiagLocal, RigidBodyB.InvInertiaDiagLocal); _accumulatedImpulse[i + 3] = 0f; // Apply accumulated impulse Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; RigidBodyA.ApplyTorqueImpulse(impulse_vector); RigidBodyB.ApplyTorqueImpulse(-impulse_vector); } } }
/// <summary> /// bilateral constraint between two dynamic objects /// positive distance = separation, negative distance = penetration /// </summary> /// <param name="body1"></param> /// <param name="pos1"></param> /// <param name="body2"></param> /// <param name="pos2"></param> /// <param name="distance"></param> /// <param name="normal"></param> /// <param name="impulse"></param> /// <param name="timeStep"></param> public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA, RigidBody bodyB, Vector3 posB, float distance, Vector3 normal, out float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); if (Math.Abs(normalLenSqr) >= 1.1f) throw new BulletException(); /*if (normalLenSqr > 1.1f) { impulse = 0f; return; }*/ Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition; Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition; //this jacobian entry could be re-used for all iterations Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform), Matrix.Transpose(bodyB.CenterOfMassTransform), rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass, bodyB.InvInertiaDiagLocal, bodyB.InverseMass); float jacDiagAB = jac.Diagonal; float jacDiagABInv = 1f / jacDiagAB; float rel_vel = jac.GetRelativeVelocity( bodyA.LinearVelocity, Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)), bodyB.LinearVelocity, Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform))); float a; a = jacDiagABInv; rel_vel = Vector3.Dot(normal, vel); float contactDamping = 0.2f; float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; }
// solving 2x2 lcp problem (inequality, direct solution ) public void ResolveBilateralPairConstraint( RigidBody body1, RigidBody body2, Matrix world2A, Matrix world2B, Vector3 invInertiaADiag, float invMassA, Vector3 linvelA, Vector3 angvelA, Vector3 rel_posA1, Vector3 invInertiaBDiag, float invMassB, Vector3 linvelB, Vector3 angvelB, Vector3 rel_posA2, float depthA, Vector3 normalA, Vector3 rel_posB1, Vector3 rel_posB2, float depthB, Vector3 normalB, out float imp0, out float imp1) { imp0 = 0f; imp1 = 0f; float len = Math.Abs(normalA.Length()) - 1f; if (Math.Abs(len) >= float.Epsilon) { return; } BulletDebug.Assert(len < float.Epsilon); JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); // calculate rhs (or error) terms float dv0 = depthA * _tau - vel0 * _damping; float dv1 = depthB * _tau - vel1 * _damping; float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; if (imp0 > 0.0f) { if (imp1 <= 0.0f) { imp1 = 0f; // now imp0>0 imp1<0 imp0 = dv0 / jacA.Diagonal; if (imp0 < 0.0f) { imp0 = 0f; } } } else { imp0 = 0f; imp1 = dv1 / jacB.Diagonal; if (imp1 <= 0.0f) { imp1 = 0f; // now imp0>0 imp1<0 imp0 = dv0 / jacA.Diagonal; if (imp0 > 0.0f) { } else { imp0 = 0f; } } } }
// solving 2x2 lcp problem (inequality, direct solution ) public void ResolveBilateralPairConstraint( RigidBody body1, RigidBody body2, Matrix world2A, Matrix world2B, Vector3 invInertiaADiag, float invMassA, Vector3 linvelA, Vector3 angvelA, Vector3 rel_posA1, Vector3 invInertiaBDiag, float invMassB, Vector3 linvelB, Vector3 angvelB, Vector3 rel_posA2, float depthA, Vector3 normalA, Vector3 rel_posB1, Vector3 rel_posB2, float depthB, Vector3 normalB, out float imp0, out float imp1) { imp0 = 0f; imp1 = 0f; float len = Math.Abs(normalA.Length()) - 1f; if (Math.Abs(len) >= float.Epsilon) return; BulletDebug.Assert(len < float.Epsilon); JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); // calculate rhs (or error) terms float dv0 = depthA * _tau - vel0 * _damping; float dv1 = depthB * _tau - vel1 * _damping; float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; if (imp0 > 0.0f) { if (imp1 <= 0.0f) { imp1 = 0f; // now imp0>0 imp1<0 imp0 = dv0 / jacA.Diagonal; if (imp0 < 0.0f) imp0 = 0f; } } else { imp0 = 0f; imp1 = dv1 / jacB.Diagonal; if (imp1 <= 0.0f) { imp1 = 0f; // now imp0>0 imp1<0 imp0 = dv0 / jacA.Diagonal; if (imp0 > 0.0f) { } else { imp0 = 0f; } } } }
// 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) { Vector3 lin = _linearJointAxis * jacB._linearJointAxis; Vector3 ang0 = _0MinvJt * jacB._aJ; Vector3 ang1 = _1MinvJt * jacB._bJ; Vector3 lin0 = massInvA * lin; Vector3 lin1 = massInvB * lin; Vector3 sum = ang0 + ang1 + lin0 + lin1; return sum.X + sum.Y + sum.Z; }
protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) { RigidBody body0 = manifold.BodyA as RigidBody; RigidBody body1 = manifold.BodyB as RigidBody; //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop { manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); int numpoints = manifold.ContactsCount; _totalContactPoints += numpoints; Vector3 color = new Vector3(0, 1, 0); for (int i = 0; i < numpoints; i++) { ManifoldPoint cp = manifold.GetContactPoint(i); if (cp.Distance <= 0) { Vector3 pos1 = cp.PositionWorldOnA; Vector3 pos2 = cp.PositionWorldOnB; Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; //this jacobian entry is re-used for all iterations JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), MatrixOperations.Transpose(body1.CenterOfMassTransform), rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, body1.InvInertiaDiagLocal, body1.InverseMass); float jacDiagAB = jac.Diagonal; ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; if (cpd != null) { //might be invalid cpd.PersistentLifeTime++; if (cpd.PersistentLifeTime != cp.LifeTime) { //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); cpd = new ConstraintPersistentData(); cpd.PersistentLifeTime = cp.LifeTime; } } else { cpd = new ConstraintPersistentData(); _totalCpd++; //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); cp.UserPersistentData = cpd; cpd.PersistentLifeTime = cp.LifeTime; //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); } if (cpd == null) { throw new BulletException(); } cpd.JacDiagABInv = 1f / jacDiagAB; //Dependent on Rigidbody A and B types, fetch the contact/friction response func //perhaps do a similar thing for friction/restutution combiner funcs... cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float rel_vel; rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); float combinedRestitution = cp.CombinedRestitution; cpd.Penetration = cp.Distance; cpd.Friction = cp.CombinedFriction; cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); if (cpd.Restitution < 0f) { cpd.Restitution = 0.0f; } ; //restitution and penetration work in same direction so //rel_vel float penVel = -cpd.Penetration / info.TimeStep; if (cpd.Restitution > penVel) { cpd.Penetration = 0; } float relaxation = info.Damping; if ((_solverMode & SolverMode.UseWarmstarting) != 0) { cpd.AppliedImpulse *= relaxation; } else { cpd.AppliedImpulse = 0f; } //for friction cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; //re-calculate friction direction every frame, todo: check if this is really needed Vector3 fwta = cpd.FrictionWorldTangentialA; Vector3 fwtb = cpd.FrictionWorldTangentialB; MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); cpd.FrictionWorldTangentialA = fwta; cpd.FrictionWorldTangentialB = fwtb; cpd.AccumulatedTangentImpulseA = 0; cpd.AccumulatedTangentImpulseB = 0; float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); float denom = relaxation / (denom0 + denom1); cpd.JacDiagABInvTangentA = denom; denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); denom = relaxation / (denom0 + denom1); cpd.JacDiagABInvTangentB = denom; Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; { Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); } { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); } //apply previous frames impulse on both bodies body0.ApplyImpulse(totalImpulse, rel_pos1); body1.ApplyImpulse(-totalImpulse, rel_pos2); } } } }