// 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;
}
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);
}
}
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;
}
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);
}
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);
}
}
}
}
