public override void SynchronizeMotionStates()
 {
     ///@todo: iterate over awake simulation islands!
     foreach (CollisionObject colObj in m_collisionObjects)
     {
         RigidBody body = RigidBody.Upcast(colObj);
         if (body != null && body.GetMotionState() != null)
         {
             if (body.GetActivationState() != ActivationState.ISLAND_SLEEPING)
             {
                 body.GetMotionState().SetWorldTransform(body.GetWorldTransform());
             }
         }
     }
 }
예제 #2
0
        //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);
        }
예제 #3
0
        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);
                    }
                }
            }
        }