public DynamicsWorld(IDispatcher dispatcher, IBroadphaseInterface broadphase, ICollisionConfiguration collisionConfiguration)
: base(dispatcher, broadphase, collisionConfiguration)
{
m_internalTickCallback = null;
m_worldUserInfo = null;
m_solverInfo = new ContactSolverInfo();
}
//InplaceSolverIslandCallback operator=(InplaceSolverIslandCallback& other)
//{
// Debug.Assert(false);
// //(void)other;
// return *this;
//}
public void Setup(ContactSolverInfo solverInfo, ObjectArray <TypedConstraint> sortedConstraints, int numConstraints, IDebugDraw debugDrawer)
{
Debug.Assert(solverInfo != null);
m_solverInfo = solverInfo;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.Resize(0);
m_manifolds.Resize(0);
m_constraints.Resize(0);
}
public virtual void SolveConstraints(ContactSolverInfo solverInfo)
{
//sorted version of all btTypedConstraint, based on islandId
m_sortedConstraints.Resize(m_constraints.Count);
int numConstraints = GetNumConstraints();
for (int i = 0; i < numConstraints; i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
if (numConstraints > 1)
{
//sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
// If this sort is removed then the constraint gets twitchy...
//m_sortedConstraints.Sort(m_islandSortPredicate);
m_sortedConstraints.QuickSort(m_islandQuickSortPredicate);
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("solveConstraints");
}
#endif
// btAssert(0);
if (m_solverIslandCallback == null)
{
m_solverIslandCallback = new InplaceSolverIslandCallback(solverInfo, m_constraintSolver, m_sortedConstraints, GetNumConstraints(), m_debugDrawer, m_dispatcher1);
}
else
{
m_solverIslandCallback.Setup(solverInfo, m_sortedConstraints, numConstraints, m_debugDrawer);
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("prepareSolve");
}
#endif
m_constraintSolver.PrepareSolve(GetCollisionWorld().GetNumCollisionObjects(), GetCollisionWorld().GetDispatcher().GetNumManifolds());
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("buildAndProcessIslands");
}
#endif
/// solve all the constraints for this island
m_islandManager.BuildAndProcessIslands(GetCollisionWorld().GetDispatcher(), GetCollisionWorld(), m_solverIslandCallback);
m_solverIslandCallback.ProcessConstraints();
m_constraintSolver.AllSolved(solverInfo, m_debugDrawer);
}
//response between two dynamic objects without friction, assuming 0 penetration depth
public static float ResolveSingleCollision(
RigidBody body1,
CollisionObject colObj2,
ref IndexedVector3 contactPositionWorld,
ref IndexedVector3 contactNormalOnB,
ContactSolverInfo solverInfo,
float distance)
{
RigidBody body2 = RigidBody.Upcast(colObj2);
IndexedVector3 normal = contactNormalOnB;
IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin;
IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin;
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2 != null?body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;
IndexedVector3 vel = vel1 - vel2;
float rel_vel = normal.Dot(ref vel);
float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution();
float restitution = combinedRestitution * -rel_vel;
float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
float velocityError = -(1.0f + restitution) * rel_vel; // * damping;
float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal);
float denom1 = body2 != null?body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f;
float relaxation = 1.0f;
float jacDiagABInv = relaxation / (denom0 + denom1);
float penetrationImpulse = positionalError * jacDiagABInv;
float velocityImpulse = velocityError * jacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse;
body1.ApplyImpulse(normal * (normalImpulse), rel_pos1);
if (body2 != null)
{
body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2);
}
return(normalImpulse);
}
public InplaceSolverIslandCallback(
ContactSolverInfo solverInfo,
IConstraintSolver solver,
ObjectArray <TypedConstraint> sortedConstraints,
int numConstraints,
IDebugDraw debugDrawer,
IDispatcher dispatcher)
{
m_solverInfo = solverInfo;
m_solver = solver;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_dispatcher = dispatcher;
m_bodies = new ObjectArray <CollisionObject>();
m_manifolds = new PersistentManifoldArray();
m_constraints = new ObjectArray <TypedConstraint>();
}
///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
public override int StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep)
{
///apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = GetDispatchInfo();
dispatchInfo.SetTimeStep(timeStep);
dispatchInfo.SetStepCount(0);
dispatchInfo.SetDebugDraw(GetDebugDrawer());
///perform collision detection
PerformDiscreteCollisionDetection();
///solve contact constraints
int numManifolds = m_dispatcher1.GetNumManifolds();
if (numManifolds != 0)
{
PersistentManifoldArray manifoldPtr = (m_dispatcher1 as CollisionDispatcher).GetInternalManifoldPointer();
ContactSolverInfo infoGlobal = new ContactSolverInfo();
infoGlobal.m_timeStep = timeStep;
m_constraintSolver.PrepareSolve(0, numManifolds);
m_constraintSolver.SolveGroup(null, 0, manifoldPtr, 0, numManifolds, null, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1);
m_constraintSolver.AllSolved(infoGlobal, m_debugDrawer);
}
///integrate transforms
IntegrateTransforms(timeStep);
UpdateAabbs();
SynchronizeMotionStates();
ClearForces();
return(1);
}
