public void ProcessIsland(List <CollisionObject> bodies, List <PersistentManifold> manifolds, int numManifolds, int islandID)
{
//also add all non-contact constraints/joints for this island
List <TypedConstraint> startConstraint = new List <TypedConstraint>();
int numCurConstraints = 0;
int startIndex = 0;
int i;
//find the first constraint for this island
for (i = 0; i < _sortedConstraints.Count; i++)
{
if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
{
//startConstraint = &m_sortedConstraints[i];
startIndex = i;
break;
}
}
//count the number of constraints in this island
for (; i < _sortedConstraints.Count; i++)
{
if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
{
numCurConstraints++;
}
}
for (i = startIndex; i < startIndex + numCurConstraints; i++)
{
startConstraint.Add(_sortedConstraints[i]);
}
_solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer);
}
protected void CalculateSimulationIslands()
{
SimulationIslandManager.UpdateActivationState(this, Dispatcher);
for (int i = 0; i < _constraints.Count; i++)
{
TypedConstraint constraint = _constraints[i];
RigidBody colObj0 = constraint.RigidBodyA;
RigidBody colObj1 = constraint.RigidBodyB;
if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) &&
((colObj1 != null) && (colObj1.MergesSimulationIslands)))
{
if (colObj0.IsActive || colObj1.IsActive)
{
SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag,
(colObj1).IslandTag);
}
}
}
//Store the island id in each body
SimulationIslandManager.StoreIslandActivationState(this);
}
internal static int GetConstraintIslandId(TypedConstraint lhs)
{
int islandId;
CollisionObject colObjA = lhs.RigidBodyA;
CollisionObject colObjB = lhs.RigidBodyB;
islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag;
return islandId;
}
internal static int GetConstraintIslandId(TypedConstraint lhs)
{
int islandId;
CollisionObject colObjA = lhs.RigidBodyA;
CollisionObject colObjB = lhs.RigidBodyB;
islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag;
return(islandId);
}
public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right)
{
int rightIslandID, leftIslandID;
rightIslandID = GetConstraintIslandId(right);
leftIslandID = GetConstraintIslandId(left);
if (leftIslandID < rightIslandID)
return -1;
else
return 1;
return 0;
}
public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right)
{
int rightIslandID, leftIslandID;
rightIslandID = GetConstraintIslandId(right);
leftIslandID = GetConstraintIslandId(left);
if (leftIslandID < rightIslandID)
{
return(-1);
}
else
{
return(1);
}
return(0);
}
public virtual void RemoveConstraint(TypedConstraint constraint) { }
public virtual void AddConstraint(TypedConstraint constraint) { }
public override void RemoveConstraint(TypedConstraint constraint)
{
_constraints.Remove(constraint);
}
public override void AddConstraint(TypedConstraint constraint)
{
_constraints.Add(constraint);
}
public virtual void RemoveConstraint(TypedConstraint constraint)
{
}
public virtual void AddConstraint(TypedConstraint constraint)
{
}
/// <summary>
/// Called when the game has determined that game logic needs to be processed.
/// </summary>
/// <param name="gameTime">Time passed since the last call to this function.</param>
protected override void Update(GameTime gameTime)
{
MouseState mouseState = Mouse.GetState();
Vector3 rayTo = getRayTo(mouseState.X, mouseState.Y);
if (mouseState.LeftButton == ButtonState.Pressed && _prevMouseState.LeftButton == ButtonState.Released)
{
shootBox(rayTo);
}
if (mouseState.MiddleButton == ButtonState.Pressed && _prevMouseState.MiddleButton == ButtonState.Released)
{
if (_world != null)
{
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(_camera.Position, rayTo);
_world.RayTest(_camera.Position, rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
if (body != null)
{
//other exclusions?
if (!(body.IsStaticObject || body.IsKinematicObject))
{
_pickedBody = body;
_pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
Vector3 localPivot = Vector3.Transform(pickPos, XnaDevRu.BulletX.MathHelper.InvertMatrix(body.CenterOfMassTransform));
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
_world.AddConstraint(p2p);
_pickConstraint = p2p;
//save mouse position for dragging
_oldPickingPos = rayTo;
Vector3 eyePos = new Vector3(_camera.Position.X, _camera.Position.Y, _camera.Position.Z);
_oldPickingDist = (eyePos - pickPos).Length();
//very weak constraint for picking
p2p.Settings.Tau = 1.1f;
}
}
}
}
}
else if (mouseState.MiddleButton == ButtonState.Released && _prevMouseState.MiddleButton == ButtonState.Pressed)
{
if (_pickConstraint != null && _world != null)
{
_world.RemoveConstraint(_pickConstraint);
_pickConstraint = null;
_pickedBody.ForceActivationState(ActivationState.Active);
_pickedBody.DeactivationTime = 0f;
_pickedBody = null;
}
}
if (_pickConstraint != null)
{
//move the constraint pivot
Point2PointConstraint p2p = _pickConstraint as Point2PointConstraint;
if (p2p != null)
{
//keep it at the same picking distance
Vector3 dir = rayTo - _camera.Position;
dir.Normalize();
dir *= _oldPickingDist;
Vector3 newPos = _camera.Position + dir;
p2p.PivotInB = newPos;
}
}
_prevMouseState = mouseState;
if (Keyboard.GetState().IsKeyDown(Keys.Space))
{
//world.stepSimulation(1.0f/60.0f,0);
int numObjects = _world.CollisionObjectsCount;
for (int i = 0; i < numObjects; i++)
{
CollisionObject colObj = _world.CollisionObjects[i];
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
if (body.MotionState != null)
{
DefaultMotionState myMotionState = (DefaultMotionState)body.MotionState;
myMotionState.GraphicsWorldTransform = myMotionState.StartWorldTransform;
colObj.WorldTransform = myMotionState.GraphicsWorldTransform;
colObj.InterpolationWorldTransform = myMotionState.StartWorldTransform;
colObj.Activate();
}
//removed cached contact points
_world.Broadphase.CleanProxyFromPairs(colObj.Broadphase);
if (body != null && !body.IsStaticObject)
{
RigidBody.Upcast(colObj).LinearVelocity = new Vector3(0, 0, 0);
RigidBody.Upcast(colObj).AngularVelocity = new Vector3(0, 0, 0);
}
}
}
}
else if (Keyboard.GetState().IsKeyDown(Keys.Escape) || GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
{
Exit();
}
else
{
//world.stepSimulation(1.0f / 60.0f, 1);
}
base.Update(gameTime);
}
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 override void RemoveConstraint(TypedConstraint constraint)
{
_constraints.Remove(constraint);
}
public override void AddConstraint(TypedConstraint constraint)
{
_constraints.Add(constraint);
}
