protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
float maxImpulse = 0;
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
if (iter == 0)
{
if (debugDraw != null)
{
debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
}
}
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
float impulse = cpd.ContactSolverFunc(
bodyA, bodyB,
cp,
info);
if (maxImpulse < impulse)
{
maxImpulse = impulse;
}
}
return(maxImpulse);
}
public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep)
{
//apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = new DispatcherInfo();
dispatchInfo.TimeStep = timeStep;
dispatchInfo.StepCount = 0;
dispatchInfo.DebugDraw = DebugDrawer;
//perform collision detection
PerformDiscreteCollisionDetection();
//solve contact constraints
int numManifolds = Dispatcher.ManifoldCount;
if (numManifolds != 0)
{
List <PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds;
//int numManifolds = m_dispatcher1.GetNumManifolds();
ContactSolverInfo infoGlobal = new ContactSolverInfo();
infoGlobal.TimeStep = timeStep;
_constraintSolver.SolveGroup(new List <CollisionObject>(), manifolds, manifolds.Count, new List <TypedConstraint>(), infoGlobal, _debugDrawer);
}
//integrate transforms
IntegrateTransforms(timeStep);
UpdateAabbs();
SynchronizeMotionStates();
}
public static float ResolveSingleFrictionEmpty(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
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 InplaceSolverIslandCallback(
ContactSolverInfo solverInfo,
IConstraintSolver solver,
List <TypedConstraint> sortedConstraints,
IDebugDraw debugDrawer)
{
_solverInfo = solverInfo;
_solver = solver;
_sortedConstraints = sortedConstraints;
_debugDrawer = debugDrawer;
}
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);
}
//protected void SolveNonContactConstraints(ContactSolverInfo solverInfo)
//{
// //constraint preparation: building jacobians
// for (int i = 0; i < _constraints.Count; i++)
// {
// TypedConstraint constraint = _constraints[i];
// constraint.BuildJacobian();
// }
// //solve the regular non-contact constraints (point 2 point, hinge, generic d6)
// for (int g = 0; g < solverInfo.IterationsCount; g++)
// {
// for (int i = 0; i < _constraints.Count; i++)
// {
// TypedConstraint constraint = _constraints[i];
// constraint.SolveConstraint(solverInfo.TimeStep);
// }
// }
//}
//protected void SolveContactConstraints(ContactSolverInfo solverInfo)
//{
// InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer);
// // solve all the contact points and contact friction
// _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback);
//}
protected void SolveConstraints(ContactSolverInfo solverInfo)
{
//sorted version of all btTypedConstraint, based on islandId
List <TypedConstraint> sortedConstraints = new List <TypedConstraint>(ConstraintsCount);
for (int i = 0; i < ConstraintsCount; i++)
{
sortedConstraints.Add(_constraints[i]);
}
sortedConstraints.Sort(new Comparison <TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate));
List <TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List <TypedConstraint>();
InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer);
// solve all the constraints for this island
_islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback);
}
//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;
}
protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
cpd.FrictionSolverFunc(
bodyA, bodyB,
cp,
info);
}
return 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);
}
}
}
}
protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
cpd.FrictionSolverFunc(
bodyA, bodyB,
cp,
info);
}
return(0);
}
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)
{
return(Solve(bodyA, bodyB, cp, info, iter, null));
}
public static float ResolveSingleFrictionOriginal(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
{
throw new BulletException();
}
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//if (contactPoint.m_appliedImpulse>0.f)
//friction
{
//apply friction in the 2 tangential directions
{
// 1st tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentA;
float total = cpd.AccumulatedTangentImpulseA + j;
if (limit < total)
{
total = limit;
}
if (total < -limit)
{
total = -limit;
}
j = total - cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
}
{
// 2nd tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentB;
float total = cpd.AccumulatedTangentImpulseB + j;
if (limit < total)
{
total = limit;
}
if (total < -limit)
{
total = -limit;
}
j = total - cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
}
}
return(cpd.AppliedImpulse);
}
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
float maxImpulse = 0;
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
if (iter == 0)
if(debugDraw != null)
debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
float impulse = cpd.ContactSolverFunc(
bodyA, bodyB,
cp,
info);
if (maxImpulse < impulse)
maxImpulse = impulse;
}
return maxImpulse;
}
public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB,
ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
{
Vector3 pos1 = contactPoint.PositionWorldOnA;
Vector3 pos2 = contactPoint.PositionWorldOnB;
Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//friction
if (cpd.AppliedImpulse > 0)
{
//apply friction in the 2 tangential directions
// 1st tangent
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float j1, j2;
{
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * cpd.JacDiagABInvTangentA;
float oldTangentImpulse = cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1;
float atia = cpd.AccumulatedTangentImpulseA;
MathHelper.SetMin(ref atia, limit);
MathHelper.SetMax(ref atia, -limit);
cpd.AccumulatedTangentImpulseA = atia;
j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse;
}
{
// 2nd tangent
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
j2 = -vrel * cpd.JacDiagABInvTangentB;
float oldTangentImpulse = cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2;
float atib = cpd.AccumulatedTangentImpulseB;
MathHelper.SetMin(ref atib, limit);
MathHelper.SetMax(ref atib, -limit);
cpd.AccumulatedTangentImpulseB = atib;
j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse;
}
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1);
bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1);
bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2);
}
}
return cpd.AppliedImpulse;
}
/// <summary>
/// contact constraint resolution:
/// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
/// positive distance = separation, negative distance = penetration
/// </summary>
/// <param name="body1"></param>
/// <param name="body2"></param>
/// <param name="contactPoint"></param>
/// <param name="info"></param>
/// <returns></returns>
public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB,
ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
{
Vector3 pos1 = contactPoint.PositionWorldOnA;
Vector3 pos2 = contactPoint.PositionWorldOnB;
// printf("distance=%f\n",distance);
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
//printf("dist=%f\n",distance);
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
{
throw new BulletException();
}
float distance = cpd.Penetration; //contactPoint.getDistance();
//distance = 0.f;
float positionalError = Kcor * -distance;
//jacDiagABInv;
float velocityError = cpd.Restitution - rel_vel; // * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0f > sum ? 0f : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
/*body1.applyImpulse(normal * (normalImpulse), rel_pos1);
* body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/
return(normalImpulse);
}
/// <summary>
/// contact constraint resolution:
/// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
/// positive distance = separation, negative distance = penetration
/// </summary>
/// <param name="body1"></param>
/// <param name="body2"></param>
/// <param name="contactPoint"></param>
/// <param name="info"></param>
/// <returns></returns>
public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB,
ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
{
Vector3 pos1 = contactPoint.PositionWorldOnA;
Vector3 pos2 = contactPoint.PositionWorldOnB;
// printf("distance=%f\n",distance);
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
//printf("dist=%f\n",distance);
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float distance = cpd.Penetration;//contactPoint.getDistance();
//distance = 0.f;
float positionalError = Kcor * -distance;
//jacDiagABInv;
float velocityError = cpd.Restitution - rel_vel;// * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0f > sum ? 0f : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
/*body1.applyImpulse(normal * (normalImpulse), rel_pos1);
body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/
return normalImpulse;
}
public virtual float SolveGroup(List <CollisionObject> bodies, List <PersistentManifold> manifolds, int numManifolds, List <TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
{
if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None)
{
return(SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer));
}
ContactSolverInfo info = infoGlobal;
int totalPoints = 0;
int numiter = infoGlobal.IterationsCount;
for (int j = 0; j < manifolds.Count; j++)
{
PersistentManifold manifold = manifolds[j];
PrepareConstraints(manifold, info);
for (int p = 0; p < manifolds[j].ContactsCount; p++)
{
_order[totalPoints].ManifoldIndex = j;
_order[totalPoints].PointIndex = p;
totalPoints++;
}
}
for (int j = 0; j < constraints.Count; j++)
{
constraints[j].BuildJacobian();
}
//should traverse the contacts random order...
int iteration;
for (iteration = 0; iteration < numiter; iteration++)
{
int j;
if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
{
if ((iteration & 7) == 0)
{
for (j = 0; j < totalPoints; ++j)
{
OrderIndex tmp = _order[j];
int swapi = RandInt2(j + 1);
_order[j] = _order[swapi];
_order[swapi] = tmp;
}
}
}
for (j = 0; j < constraints.Count; j++)
{
constraints[j].SolveConstraint(info.TimeStep);
}
for (j = 0; j < totalPoints; j++)
{
PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB,
manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
}
for (j = 0; j < totalPoints; j++)
{
PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
SolveFriction((RigidBody)manifold.BodyA,
(RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
}
}
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 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;
}
protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
float maxImpulse = 0;
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
if (iter == 0)
{
if (debugDraw != null)
{
debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
}
}
float impulse = ContactConstraint.ResolveSingleCollisionCombined(
bodyA, bodyB,
cp,
info);
if (maxImpulse < impulse)
{
maxImpulse = impulse;
}
}
return(maxImpulse);
}
public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
{
if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None)
{
return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer);
}
ContactSolverInfo info = infoGlobal;
int totalPoints = 0;
int numiter = infoGlobal.IterationsCount;
for (int j = 0; j < manifolds.Count; j++)
{
PersistentManifold manifold = manifolds[j];
PrepareConstraints(manifold, info);
for (int p = 0; p < manifolds[j].ContactsCount; p++)
{
_order[totalPoints].ManifoldIndex = j;
_order[totalPoints].PointIndex = p;
totalPoints++;
}
}
for (int j = 0; j < constraints.Count; j++)
{
constraints[j].BuildJacobian();
}
//should traverse the contacts random order...
int iteration;
for (iteration = 0; iteration < numiter; iteration++)
{
int j;
if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
{
if ((iteration & 7) == 0)
{
for (j = 0; j < totalPoints; ++j)
{
OrderIndex tmp = _order[j];
int swapi = RandInt2(j + 1);
_order[j] = _order[swapi];
_order[swapi] = tmp;
}
}
}
for (j = 0; j < constraints.Count; j++)
{
constraints[j].SolveConstraint(info.TimeStep);
}
for (j = 0; j < totalPoints; j++)
{
PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB,
manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
}
for (j = 0; j < totalPoints; j++)
{
PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
SolveFriction((RigidBody)manifold.BodyA,
(RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
}
}
return 0;
}
//protected void SolveNonContactConstraints(ContactSolverInfo solverInfo)
//{
// //constraint preparation: building jacobians
// for (int i = 0; i < _constraints.Count; i++)
// {
// TypedConstraint constraint = _constraints[i];
// constraint.BuildJacobian();
// }
// //solve the regular non-contact constraints (point 2 point, hinge, generic d6)
// for (int g = 0; g < solverInfo.IterationsCount; g++)
// {
// for (int i = 0; i < _constraints.Count; i++)
// {
// TypedConstraint constraint = _constraints[i];
// constraint.SolveConstraint(solverInfo.TimeStep);
// }
// }
//}
//protected void SolveContactConstraints(ContactSolverInfo solverInfo)
//{
// InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer);
// // solve all the contact points and contact friction
// _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback);
//}
protected void SolveConstraints(ContactSolverInfo solverInfo)
{
//sorted version of all btTypedConstraint, based on islandId
List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount);
for (int i = 0; i < ConstraintsCount; i++)
{
sortedConstraints.Add(_constraints[i]);
}
sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate));
List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>();
InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer);
// solve all the constraints for this island
_islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback);
}
public static float ResolveSingleFrictionOriginal(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//if (contactPoint.m_appliedImpulse>0.f)
//friction
{
//apply friction in the 2 tangential directions
{
// 1st tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentA;
float total = cpd.AccumulatedTangentImpulseA + j;
if (limit < total)
total = limit;
if (total < -limit)
total = -limit;
j = total - cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
}
{
// 2nd tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentB;
float total = cpd.AccumulatedTangentImpulseB + j;
if (limit < total)
total = limit;
if (total < -limit)
total = -limit;
j = total - cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
}
}
return cpd.AppliedImpulse;
}
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 static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB,
ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
{
Vector3 pos1 = contactPoint.PositionWorldOnA;
Vector3 pos2 = contactPoint.PositionWorldOnB;
Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
{
throw new BulletException();
}
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//friction
if (cpd.AppliedImpulse > 0)
{
//apply friction in the 2 tangential directions
// 1st tangent
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float j1, j2;
{
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * cpd.JacDiagABInvTangentA;
float oldTangentImpulse = cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1;
float atia = cpd.AccumulatedTangentImpulseA;
MathHelper.SetMin(ref atia, limit);
MathHelper.SetMax(ref atia, -limit);
cpd.AccumulatedTangentImpulseA = atia;
j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse;
}
{
// 2nd tangent
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
j2 = -vrel * cpd.JacDiagABInvTangentB;
float oldTangentImpulse = cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2;
float atib = cpd.AccumulatedTangentImpulseB;
MathHelper.SetMin(ref atib, limit);
MathHelper.SetMax(ref atib, -limit);
cpd.AccumulatedTangentImpulseB = atib;
j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse;
}
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1);
bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1);
bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2);
}
}
return(cpd.AppliedImpulse);
}
//velocity + friction
//response between two dynamic objects with friction
public static float ResolveSingleCollisionCombined(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float relVel;
relVel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float distance = cpd.Penetration;
float positionalError = Kcor * -distance;
float velocityError = cpd.Restitution - relVel;// * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0 > sum ? 0 : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
{
//friction
Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel3 = vel12 - vel22;
relVel = Vector3.Dot(normal, vel3);
Vector3 latVel = vel3 - normal * relVel;
float lat_rel_vel = latVel.Length();
float combinedFriction = cpd.Friction;
if (cpd.AppliedImpulse > 0)
if (lat_rel_vel > float.Epsilon)
{
latVel /= lat_rel_vel;
Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
float friction_impulse = lat_rel_vel /
(bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
float normal_impulse = cpd.AppliedImpulse * combinedFriction;
MathHelper.SetMin(ref friction_impulse, normal_impulse);
MathHelper.SetMin(ref friction_impulse, -normal_impulse);
bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
}
}
return normalImpulse;
}
//velocity + friction
//response between two dynamic objects with friction
public static float ResolveSingleCollisionCombined(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float relVel;
relVel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
{
throw new BulletException();
}
float distance = cpd.Penetration;
float positionalError = Kcor * -distance;
float velocityError = cpd.Restitution - relVel; // * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0 > sum ? 0 : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
{
//friction
Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel3 = vel12 - vel22;
relVel = Vector3.Dot(normal, vel3);
Vector3 latVel = vel3 - normal * relVel;
float lat_rel_vel = latVel.Length();
float combinedFriction = cpd.Friction;
if (cpd.AppliedImpulse > 0)
{
if (lat_rel_vel > float.Epsilon)
{
latVel /= lat_rel_vel;
Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
float friction_impulse = lat_rel_vel /
(bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
float normal_impulse = cpd.AppliedImpulse * combinedFriction;
MathHelper.SetMin(ref friction_impulse, normal_impulse);
MathHelper.SetMin(ref friction_impulse, -normal_impulse);
bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
}
}
}
return(normalImpulse);
}
public static float ResolveSingleFrictionEmpty(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
return 0;
}
public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep)
{
//apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = new DispatcherInfo();
dispatchInfo.TimeStep = timeStep;
dispatchInfo.StepCount = 0;
dispatchInfo.DebugDraw = DebugDrawer;
//perform collision detection
PerformDiscreteCollisionDetection();
//solve contact constraints
int numManifolds = Dispatcher.ManifoldCount;
if (numManifolds != 0)
{
List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds;
//int numManifolds = m_dispatcher1.GetNumManifolds();
ContactSolverInfo infoGlobal = new ContactSolverInfo();
infoGlobal.TimeStep = timeStep;
_constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer);
}
//integrate transforms
IntegrateTransforms(timeStep);
UpdateAabbs();
SynchronizeMotionStates();
}
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)
{
return Solve(bodyA, bodyB, cp, info, iter, null);
}
public InplaceSolverIslandCallback(
ContactSolverInfo solverInfo,
IConstraintSolver solver,
List<TypedConstraint> sortedConstraints,
IDebugDraw debugDrawer)
{
_solverInfo = solverInfo;
_solver = solver;
_sortedConstraints = sortedConstraints;
_debugDrawer = debugDrawer;
}
protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
float maxImpulse = 0;
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
if (iter == 0)
if (debugDraw != null)
debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
float impulse = ContactConstraint.ResolveSingleCollisionCombined(
bodyA, bodyB,
cp,
info);
if (maxImpulse < impulse)
maxImpulse = impulse;
}
return maxImpulse;
}
