// 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 float ComputeAngle(int axis)
{
float angle = 0;
switch (axis)
{
case 0:
{
Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform);
Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
float s = Vector3.Dot(v1, w2);
float c = Vector3.Dot(v1, v2);
angle = (float)Math.Atan2(s, c);
break;
}
case 1:
{
Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform);
Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
float s = Vector3.Dot(w1, u2);
float c = Vector3.Dot(w1, w2);
angle = (float)Math.Atan2(s, c);
break;
}
case 2:
{
Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform);
Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
float s = Vector3.Dot(u1, v2);
float c = Vector3.Dot(u1, u2);
angle = (float)Math.Atan2(s, c);
break;
}
default: BulletDebug.Assert(false); break;
}
return(angle);
}
public override void UpdateAabbs()
{
Vector3 colorvec = new Vector3(1, 0, 0);
for (int i = 0; i < CollisionObjects.Count; i++)
{
CollisionObject colObj = CollisionObjects[i];
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
// if (body->IsActive() && (!body->IsStatic()))
{
Vector3 minAabb, maxAabb;
colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb);
OverlappingPairCache bp = BroadphasePairCache;
//moving objects should be moderately sized, probably something wrong if not
if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f))
{
bp.SetAabb(body.Broadphase, minAabb, maxAabb);
}
else
{
//something went wrong, investigate
//this assert is unwanted in 3D modelers (danger of loosing work)
BulletDebug.Assert(false);
body.ActivationState = ActivationState.DisableSimulation;
if (_reportMe)
{
_reportMe = false;
Console.WriteLine("Overflow in AABB, object removed from simulation \n");
Console.WriteLine("If you can reproduce this, please email [email protected]\n");
Console.WriteLine("Please include above information, your Platform, version of OS.\n");
Console.WriteLine("Thanks.\n");
}
}
if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0)
{
DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec);
}
}
}
}
}
// 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;
}
}
}
}
public virtual float SolveGroupCacheFriendly(List <CollisionObject> bodies, List <PersistentManifold> manifolds, int numManifolds, List <TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
{
if (constraints.Count + numManifolds == 0)
{
return(0);
}
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold manifold = manifolds[i];
RigidBody rbA = (RigidBody)manifold.BodyA;
RigidBody rbB = (RigidBody)manifold.BodyB;
manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform);
}
int minReservation = manifolds.Count * 2;
_tmpSolverBodyPool = new List <SolverBody>(minReservation);
for (int i = 0; i < bodies.Count; i++)
{
RigidBody rb = RigidBody.Upcast(bodies[i]);
if (rb != null && rb.IslandTag >= 0)
{
BulletDebug.Assert(rb.CompanionID < 0);
int solverBodyId = _tmpSolverBodyPool.Count;
SolverBody solverBody;
InitSolverBody(out solverBody, rb);
_tmpSolverBodyPool.Add(solverBody);
rb.CompanionID = solverBodyId;
}
}
_tmpSolverConstraintPool = new List <SolverConstraint>(minReservation);
_tmpSolverFrictionConstraintPool = new List <SolverConstraint>(minReservation);
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold manifold = manifolds[i];
RigidBody rb0 = (RigidBody)manifold.BodyA;
RigidBody rb1 = (RigidBody)manifold.BodyB;
int solverBodyIdA = -1;
int solverBodyIdB = -1;
//if (i == 89)
// System.Diagnostics.Debugger.Break();
if (manifold.ContactsCount != 0)
{
if (rb0.IslandTag >= 0)
{
solverBodyIdA = rb0.CompanionID;
}
else
{
//create a static body
solverBodyIdA = _tmpSolverBodyPool.Count;
SolverBody solverBody;
InitSolverBody(out solverBody, rb0);
_tmpSolverBodyPool.Add(solverBody);
}
if (rb1.IslandTag >= 0)
{
solverBodyIdB = rb1.CompanionID;
}
else
{
//create a static body
solverBodyIdB = _tmpSolverBodyPool.Count;
SolverBody solverBody;
InitSolverBody(out solverBody, rb1);
_tmpSolverBodyPool.Add(solverBody);
}
}
if (solverBodyIdB == -1 || solverBodyIdA == -1)
{
System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i));
}
for (int j = 0; j < manifold.ContactsCount; j++)
{
ManifoldPoint cp = manifold.GetContactPoint(j);
int frictionIndex = _tmpSolverConstraintPool.Count;
if (cp.Distance <= 0)
{
Vector3 pos1 = cp.PositionWorldOnA;
Vector3 pos2 = cp.PositionWorldOnB;
Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition;
float relaxation = 1;
{
SolverConstraint solverConstraint = new SolverConstraint();
_tmpSolverConstraintPool.Add(solverConstraint);
solverConstraint.SolverBodyIdA = solverBodyIdA;
solverConstraint.SolverBodyIdB = solverBodyIdB;
solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact;
//can be optimized, the cross products are already calculated
float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB);
float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB);
float denom = relaxation / (denom0 + denom1);
solverConstraint.JacDiagABInv = denom;
solverConstraint.ContactNormal = cp.NormalWorldOnB;
solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
solverConstraint.Penetration = cp.Distance; //btScalar(infoGlobal.m_numIterations);
solverConstraint.Friction = cp.CombinedFriction;
float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution);
if (rest <= 0)
{
rest = 0;
}
float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep;
if (rest > penVel)
{
rest = 0;
}
solverConstraint.Restitution = rest;
solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep);
solverConstraint.AppliedImpulse = 0f;
solverConstraint.AppliedVelocityImpulse = 0f;
#warning Check to see if we need Vector3.Transform
Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld);
Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld);
}
//create 2 '1d axis' constraints for 2 tangential friction directions
//re-calculate friction direction every frame, todo: check if this is really needed
Vector3 frictionTangential0a = new Vector3(),
frictionTangential1b = new Vector3();
MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b);
{
SolverConstraint solverConstraint = new SolverConstraint();
_tmpSolverFrictionConstraintPool.Add(solverConstraint);
solverConstraint.ContactNormal = frictionTangential0a;
solverConstraint.SolverBodyIdA = solverBodyIdA;
solverConstraint.SolverBodyIdB = solverBodyIdB;
solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
solverConstraint.FrictionIndex = frictionIndex;
solverConstraint.Friction = cp.CombinedFriction;
solverConstraint.AppliedImpulse = 0;
solverConstraint.AppliedVelocityImpulse = 0;
float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
float denom = relaxation / (denom0 + denom1);
solverConstraint.JacDiagABInv = denom;
{
Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
solverConstraint.RelPosACrossNormal = ftorqueAxis0;
solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
solverConstraint.RelPosBCrossNormal = ftorqueAxis0;
solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld);
}
}
{
SolverConstraint solverConstraint = new SolverConstraint();
_tmpSolverFrictionConstraintPool.Add(solverConstraint);
solverConstraint.ContactNormal = frictionTangential1b;
solverConstraint.SolverBodyIdA = solverBodyIdA;
solverConstraint.SolverBodyIdB = solverBodyIdB;
solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
solverConstraint.FrictionIndex = frictionIndex;
solverConstraint.Friction = cp.CombinedFriction;
solverConstraint.AppliedImpulse = 0;
solverConstraint.AppliedVelocityImpulse = 0;
float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
float denom = relaxation / (denom0 + denom1);
solverConstraint.JacDiagABInv = denom;
{
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
solverConstraint.RelPosACrossNormal = ftorqueAxis1;
solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld);
}
{
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
solverConstraint.RelPosBCrossNormal = ftorqueAxis1;
solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld);
}
}
}
}
}
ContactSolverInfo info = infoGlobal;
{
for (int j = 0; j < constraints.Count; j++)
{
TypedConstraint constraint = constraints[j];
constraint.BuildJacobian();
}
}
int numConstraintPool = _tmpSolverConstraintPool.Count;
int numFrictionPool = _tmpSolverFrictionConstraintPool.Count;
//todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
List <int> gOrderTmpConstraintPool = new List <int>(numConstraintPool);
List <int> gOrderFrictionConstraintPool = new List <int>(numFrictionPool);
{
for (int i = 0; i < numConstraintPool; i++)
{
gOrderTmpConstraintPool.Add(i);
}
for (int i = 0; i < numFrictionPool; i++)
{
gOrderFrictionConstraintPool.Add(i);
}
}
//should traverse the contacts random order...
int iteration;
{
for (iteration = 0; iteration < info.IterationsCount; iteration++)
{
int j;
if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
{
if ((iteration & 7) == 0)
{
for (j = 0; j < numConstraintPool; ++j)
{
int tmp = gOrderTmpConstraintPool[j];
int swapi = RandInt2(j + 1);
gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];
gOrderTmpConstraintPool[swapi] = tmp;
}
for (j = 0; j < numFrictionPool; ++j)
{
int tmp = gOrderFrictionConstraintPool[j];
int swapi = RandInt2(j + 1);
gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];
gOrderFrictionConstraintPool[swapi] = tmp;
}
}
}
for (j = 0; j < constraints.Count; j++)
{
TypedConstraint constraint = constraints[j];
//todo: use solver bodies, so we don't need to copy from/to btRigidBody
if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
{
_tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity();
}
if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
{
_tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity();
}
constraint.SolveConstraint(info.TimeStep);
if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
{
_tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity();
}
if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
{
_tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity();
}
}
{
int numPoolConstraints = _tmpSolverConstraintPool.Count;
for (j = 0; j < numPoolConstraints; j++)
{
SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
_tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info);
}
}
{
int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count;
for (j = 0; j < numFrictionPoolConstraints; j++)
{
SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse;
ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
_tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse);
}
}
}
}
for (int i = 0; i < _tmpSolverBodyPool.Count; i++)
{
_tmpSolverBodyPool[i].WriteBackVelocity();
}
_tmpSolverBodyPool.Clear();
_tmpSolverConstraintPool.Clear();
_tmpSolverFrictionConstraintPool.Clear();
return(0);
}
