protected virtual void CalculateSimulationIslands()
{
BulletGlobals.StartProfile("calculateSimulationIslands");
GetSimulationIslandManager().UpdateActivationState(GetCollisionWorld(), GetCollisionWorld().GetDispatcher());
{
int length = m_constraints.Count;
for (int i = 0; i < length; ++i)
{
TypedConstraint constraint = m_constraints[i];
RigidBody colObj0 = constraint.GetRigidBodyA();
RigidBody colObj1 = constraint.GetRigidBodyB();
if (((colObj0 != null) && (!colObj0.IsStaticOrKinematicObject())) &&
((colObj1 != null) && (!colObj1.IsStaticOrKinematicObject())))
{
if (colObj0.IsActive() || colObj1.IsActive())
{
GetSimulationIslandManager().GetUnionFind().Unite((colObj0).GetIslandTag(),
(colObj1).GetIslandTag());
}
}
}
}
//Store the island id in each body
GetSimulationIslandManager().StoreIslandActivationState(GetCollisionWorld());
BulletGlobals.StopProfile();
}
public override void SynchronizeMotionStates()
{
if (m_synchronizeAllMotionStates)
{
//iterate over all collision objects
int length = m_collisionObjects.Count;
for (int i = 0; i < length; ++i)
{
RigidBody body = RigidBody.Upcast(m_collisionObjects[i]);
if (body != null)
{
SynchronizeSingleMotionState(body);
}
}
}
else
{
//iterate over all active rigid bodies
int length = m_nonStaticRigidBodies.Count;
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body.IsActive())
{
SynchronizeSingleMotionState(body);
}
}
}
}
///apply gravity, call this once per timestep
public virtual void ApplyGravity()
{
int length = m_nonStaticRigidBodies.Count;
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body != null && body.IsActive())
{
body.ApplyGravity();
}
}
}
public override void SetGravity(ref IndexedVector3 gravity)
{
m_gravity = gravity;
int length = m_nonStaticRigidBodies.Count;
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body.IsActive() && ((body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY) == 0))
{
body.SetGravity(ref gravity);
}
}
}
protected void IntegrateTransforms(float timeStep)
{
IndexedMatrix predictedTrans;
foreach (CollisionObject colObj in m_collisionObjects)
{
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
if (body.IsActive() && (!body.IsStaticObject()))
{
body.PredictIntegratedTransform(timeStep, out predictedTrans);
body.ProceedToTransform(ref predictedTrans);
}
}
}
}
public override void UpdateAabbs()
{
//IndexedMatrix predictedTrans = IndexedMatrix.Identity;
foreach (CollisionObject colObj in m_collisionObjects)
{
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
if (body.IsActive() && (!body.IsStaticObject()))
{
IndexedVector3 minAabb;
IndexedVector3 maxAabb;
colObj.GetCollisionShape().GetAabb(colObj.GetWorldTransform(), out minAabb, out maxAabb);
IBroadphaseInterface bp = GetBroadphase();
bp.SetAabb(body.GetBroadphaseHandle(), ref minAabb, ref maxAabb, m_dispatcher1);
}
}
}
}
protected void PredictUnconstraintMotion(float timeStep)
{
foreach (CollisionObject colObj in m_collisionObjects)
{
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
if (!body.IsStaticObject())
{
if (body.IsActive())
{
body.ApplyGravity();
body.IntegrateVelocities(timeStep);
body.ApplyDamping(timeStep);
IndexedMatrix temp = body.GetInterpolationWorldTransform();
body.PredictIntegratedTransform(timeStep, out temp);
body.SetInterpolationWorldTransform(ref temp);
}
}
}
}
}
protected virtual void IntegrateTransforms(float timeStep)
{
BulletGlobals.StartProfile("integrateTransforms");
IndexedMatrix predictedTrans;
int length = m_nonStaticRigidBodies.Count;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("IntegrateTransforms [{0}]", length);
}
#endif
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body != null)
{
body.SetHitFraction(1f);
if (body.IsActive() && (!body.IsStaticOrKinematicObject()))
{
body.PredictIntegratedTransform(timeStep, out predictedTrans);
float squareMotion = (predictedTrans._origin - body.GetWorldTransform()._origin).LengthSquared();
//if (body.GetCcdSquareMotionThreshold() != 0 && body.GetCcdSquareMotionThreshold() < squareMotion)
if (GetDispatchInfo().m_useContinuous&& body.GetCcdSquareMotionThreshold() != 0.0f && body.GetCcdSquareMotionThreshold() < squareMotion)
{
BulletGlobals.StartProfile("CCD motion clamping");
if (body.GetCollisionShape().IsConvex())
{
gNumClampedCcdMotions++;
using (ClosestNotMeConvexResultCallback sweepResults = BulletGlobals.ClosestNotMeConvexResultCallbackPool.Get())
{
sweepResults.Initialize(body, body.GetWorldTransform()._origin, predictedTrans._origin, GetBroadphase().GetOverlappingPairCache(), GetDispatcher());
//btConvexShape* convexShape = static_cast<btConvexShape*>(body.GetCollisionShape());
SphereShape tmpSphere = BulletGlobals.SphereShapePool.Get();
tmpSphere.Initialize(body.GetCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body.GetCollisionShape());
sweepResults.m_allowedPenetration = GetDispatchInfo().GetAllowedCcdPenetration();
sweepResults.m_collisionFilterGroup = body.GetBroadphaseProxy().m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body.GetBroadphaseProxy().m_collisionFilterMask;
IndexedMatrix modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans._basis = body.GetWorldTransform()._basis;
modifiedPredictedTrans._origin = predictedTrans._origin;
ConvexSweepTest(tmpSphere, body.GetWorldTransform(), modifiedPredictedTrans, sweepResults, 0f);
if (sweepResults.HasHit() && (sweepResults.m_closestHitFraction < 1.0f))
{
//printf("clamped integration to hit fraction = %f\n",fraction);
body.SetHitFraction(sweepResults.m_closestHitFraction);
body.PredictIntegratedTransform(timeStep * body.GetHitFraction(), out predictedTrans);
body.SetHitFraction(0.0f);
body.ProceedToTransform(ref predictedTrans);
#if false
btVector3 linVel = body.getLinearVelocity();
float maxSpeed = body.getCcdMotionThreshold() / getSolverInfo().m_timeStep;
float maxSpeedSqr = maxSpeed * maxSpeed;
if (linVel.LengthSquared() > maxSpeedSqr)
{
linVel.normalize();
linVel *= maxSpeed;
body.setLinearVelocity(linVel);
float ms2 = body.getLinearVelocity().LengthSquared();
body.predictIntegratedTransform(timeStep, predictedTrans);
float sm2 = (predictedTrans._origin - body.GetWorldTransform()._origin).LengthSquared();
float smt = body.getCcdSquareMotionThreshold();
printf("sm2=%f\n", sm2);
}
#else
//response between two dynamic objects without friction, assuming 0 penetration depth
float appliedImpulse = 0.0f;
float depth = 0.0f;
appliedImpulse = ContactConstraint.ResolveSingleCollision(body, sweepResults.m_hitCollisionObject, ref sweepResults.m_hitPointWorld, ref sweepResults.m_hitNormalWorld, GetSolverInfo(), depth);
#endif
continue;
}
BulletGlobals.SphereShapePool.Free(tmpSphere);
}
}
BulletGlobals.StopProfile();
}
body.ProceedToTransform(ref predictedTrans);
}
}
}
}