Ejemplo n.º 1
0
        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);
                    }
                }
            }
        }