//velocity + friction
//response between two dynamic objects with friction
public virtual float ResolveSingleCollisionCombinedCacheFriendly(
SolverBody bodyA,
SolverBody bodyB,
SolverConstraint contactConstraint,
ContactSolverInfo solverInfo)
{
float normalImpulse = 0;
if (contactConstraint.Penetration < 0)
{
return(0);
}
float relVel;
float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
relVel = velADotn - velBDotn;
float positionalError = contactConstraint.Penetration;
float velocityError = contactConstraint.Restitution - relVel; // * damping;
float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;
normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = contactConstraint.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;
float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
float velocitySum = oldVelocityImpulse + velocityImpulse;
contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;
normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;
if (bodyA.InvMass != 0)
{
bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
contactConstraint.AngularComponentA, normalImpulse);
}
if (bodyB.InvMass != 0)
{
bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
contactConstraint.AngularComponentB, -normalImpulse);
}
return(normalImpulse);
}
public virtual float ResolveSingleFrictionCacheFriendly(
SolverBody bodyA,
SolverBody bodyB,
SolverConstraint contactConstraint,
ContactSolverInfo solverInfo,
float appliedNormalImpulse)
{
float combinedFriction = contactConstraint.Friction;
float limit = appliedNormalImpulse * combinedFriction;
if (appliedNormalImpulse > 0)
//friction
{
float j1;
{
float relVel;
float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
relVel = velADotn - velBDotn;
// calculate j that moves us to zero relative velocity
j1 = -relVel * contactConstraint.JacDiagABInv;
float oldTangentImpulse = contactConstraint.AppliedImpulse;
contactConstraint.AppliedImpulse = oldTangentImpulse + j1;
float test = contactConstraint.AppliedImpulse;
MathHelper.SetMin(ref test, limit);
MathHelper.SetMax(ref test, -limit);
contactConstraint.AppliedImpulse = test;
j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
}
if (bodyA.InvMass != 0)
{
bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
}
if (bodyB.InvMass != 0)
{
bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
}
}
return(0);
}
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;
}
public virtual float ResolveSingleFrictionCacheFriendly(
SolverBody bodyA,
SolverBody bodyB,
SolverConstraint contactConstraint,
ContactSolverInfo solverInfo,
float appliedNormalImpulse)
{
float combinedFriction = contactConstraint.Friction;
float limit = appliedNormalImpulse * combinedFriction;
if (appliedNormalImpulse > 0)
//friction
{
float j1;
{
float relVel;
float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
relVel = velADotn - velBDotn;
// calculate j that moves us to zero relative velocity
j1 = -relVel * contactConstraint.JacDiagABInv;
float oldTangentImpulse = contactConstraint.AppliedImpulse;
contactConstraint.AppliedImpulse = oldTangentImpulse + j1;
float test = contactConstraint.AppliedImpulse;
MathHelper.SetMin(ref test, limit);
MathHelper.SetMax(ref test, -limit);
contactConstraint.AppliedImpulse = test;
j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
}
if (bodyA.InvMass != 0)
{
bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
}
if (bodyB.InvMass != 0)
{
bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
}
}
return 0;
}
//velocity + friction
//response between two dynamic objects with friction
public virtual float ResolveSingleCollisionCombinedCacheFriendly(
SolverBody bodyA,
SolverBody bodyB,
SolverConstraint contactConstraint,
ContactSolverInfo solverInfo)
{
float normalImpulse = 0;
if (contactConstraint.Penetration < 0)
return 0;
float relVel;
float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity);
float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity)
+ Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity);
relVel = velADotn - velBDotn;
float positionalError = contactConstraint.Penetration;
float velocityError = contactConstraint.Restitution - relVel;// * damping;
float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;
normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = contactConstraint.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;
float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
float velocitySum = oldVelocityImpulse + velocityImpulse;
contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;
normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;
if (bodyA.InvMass != 0)
{
bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
contactConstraint.AngularComponentA, normalImpulse);
}
if (bodyB.InvMass != 0)
{
bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
contactConstraint.AngularComponentB, -normalImpulse);
}
return normalImpulse;
}
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);
}