public 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 virtual void PredictUnconstraintMotion(float timeStep)
{
BulletGlobals.StartProfile("predictUnconstraintMotion");
int length = m_nonStaticRigidBodies.Count;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("PredictUnconstraintMotion [{0}][{1}]", length, timeStep);
}
#endif
//for (int i = 0; i < length;i++)
for (int i = 0; i < m_nonStaticRigidBodies.Count; i++)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (!body.IsStaticOrKinematicObject())
{
body.IntegrateVelocities(timeStep);
//dampingF
body.ApplyDamping(timeStep);
IndexedMatrix temp;
body.PredictIntegratedTransform(timeStep, out temp);
body.SetInterpolationWorldTransform(ref temp);
}
}
BulletGlobals.StopProfile();
}
public override void AddRigidBody(RigidBody body)
{
if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY))
{
body.SetGravity(ref m_gravity);
}
if (body.GetCollisionShape() != null)
{
if (!body.IsStaticObject())
{
m_nonStaticRigidBodies.Add(body);
}
else
{
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
bool isDynamic = !(body.IsStaticObject() || body.IsKinematicObject());
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : (CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter);
AddCollisionObject(body, collisionFilterGroup, collisionFilterMask);
}
}
public void UpdateActivationState(float timeStep)
{
BulletGlobals.StartProfile("updateActivationState");
int length = m_nonStaticRigidBodies.Count;
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body != null)
{
body.UpdateDeactivation(timeStep);
if (body.WantsSleeping())
{
if (body.IsStaticOrKinematicObject())
{
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
else
{
if (body.GetActivationState() == ActivationState.ACTIVE_TAG)
{
body.SetActivationState(ActivationState.WANTS_DEACTIVATION);
}
if (body.GetActivationState() == ActivationState.ISLAND_SLEEPING)
{
IndexedVector3 zero = IndexedVector3.Zero;
body.SetAngularVelocity(ref zero);
body.SetLinearVelocity(ref zero);
}
}
}
else
{
if (body.GetActivationState() != ActivationState.DISABLE_DEACTIVATION)
{
body.SetActivationState(ActivationState.ACTIVE_TAG);
}
}
}
}
BulletGlobals.StopProfile();
}
///this can be useful to synchronize a single rigid body . graphics object
public void SynchronizeSingleMotionState(RigidBody body)
{
Debug.Assert(body != null);
if (body.GetMotionState() != null && !body.IsStaticOrKinematicObject())
{
//we need to call the update at least once, even for sleeping objects
//otherwise the 'graphics' transform never updates properly
///@todo: add 'dirty' flag
//if (body.getActivationState() != ISLAND_SLEEPING)
{
IndexedMatrix interpolatedTransform;
TransformUtil.IntegrateTransform(body.GetInterpolationWorldTransform(),
body.SetInterpolationLinearVelocity(), body.GetInterpolationAngularVelocity(),
m_localTime * body.GetHitFraction(), out interpolatedTransform);
body.GetMotionState().SetWorldTransform(ref interpolatedTransform);
}
}
}
public override void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY))
{
body.SetGravity(ref m_gravity);
}
if (body.GetCollisionShape() != null)
{
if (!body.IsStaticObject())
{
if (!m_nonStaticRigidBodies.Contains(body))
{
m_nonStaticRigidBodies.Add(body);
}
}
else
{
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
AddCollisionObject(body, group, mask);
}
}
public 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);
}
}
}
}
