Exemplo n.º 1
0
        public void UpdateWheelTransformsWS(WheelInfo wheel, bool interpolatedTransform)
        {
            wheel.m_raycastInfo.m_isInContact = false;

            IndexedMatrix chassisTrans = GetChassisWorldTransform();

            if (interpolatedTransform && (GetRigidBody().GetMotionState() != null))
            {
                GetRigidBody().GetMotionState().GetWorldTransform(out chassisTrans);
            }

            wheel.m_raycastInfo.m_hardPointWS      = chassisTrans * wheel.m_chassisConnectionPointCS;
            wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans._basis * wheel.m_wheelDirectionCS;
            wheel.m_raycastInfo.m_wheelAxleWS      = chassisTrans._basis * wheel.m_wheelAxleCS;
        }
Exemplo n.º 2
0
        public float RayCast(WheelInfo wheel)
        {
            UpdateWheelTransformsWS(wheel, false);

            float depth = -1;

            float raylen = wheel.GetSuspensionRestLength() + wheel.m_wheelsRadius;

            IndexedVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
            IndexedVector3 source    = wheel.m_raycastInfo.m_hardPointWS;

            wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
            IndexedVector3 target = wheel.m_raycastInfo.m_contactPointWS;

            float param = 0f;

            VehicleRaycasterResult rayResults = new VehicleRaycasterResult();

            Debug.Assert(m_vehicleRaycaster != null);

            Object object1 = m_vehicleRaycaster.CastRay(ref source, ref target, ref rayResults);

            //if (object1 != null && (object1 as RigidBody).m_debugBodyId != 1)
            //{
            //    int ibreak = 0;
            //}

            //{
            //    IndexedVector3 from1 = new IndexedVector3(0.7957098f, -9.13606f, 1.794605f);
            //    IndexedVector3 to1 = new IndexedVector3(0.886791f, -10.23207f, 1.815941f);
            //    VehicleRaycasterResult results1  = new VehicleRaycasterResult();
            //    Object o2 = m_vehicleRaycaster.castRay(ref from1, ref to1, ref results1);

            //    IndexedVector3 from2 = new IndexedVector3(0.7957281f, -9.136093f, 1.794625f);
            //    IndexedVector3 to2 = new IndexedVector3(0.8867911f, -10.23211f, 1.815956f);
            //    VehicleRaycasterResult results2 = new VehicleRaycasterResult();
            //    Object o3 = m_vehicleRaycaster.castRay(ref from2, ref to2, ref results2);

            //    if (Math.Abs(results1.m_distFraction - results2.m_distFraction) > 0.1f)
            //    {
            //        int ibreak = 0;
            //    }
            //}



            wheel.m_raycastInfo.m_groundObject = null;

            if (object1 != null)
            {
                param = rayResults.m_distFraction;
                depth = raylen * rayResults.m_distFraction;
                wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
                wheel.m_raycastInfo.m_isInContact     = true;

                wheel.m_raycastInfo.m_groundObject = s_fixedObject;///@todo for driving on dynamic/movable objects!;
                //wheel.m_raycastInfo.m_groundObject = object;

                float hitDistance = param * raylen;
                wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;



                //clamp on max suspension travel

                float minSuspensionLength = wheel.GetSuspensionRestLength() - wheel.m_maxSuspensionTravelCm * 0.01f;
                float maxSuspensionLength = wheel.GetSuspensionRestLength() + wheel.m_maxSuspensionTravelCm * 0.01f;
                if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
                {
                    wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
                }
                if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
                {
                    wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
                }

                //if (Math.Abs(wheel.m_raycastInfo.m_suspensionLength - wheel.m_raycastInfo.m_suspensionLengthBak) > 0.1f)
                //{
                //    int ibreak = 0;
                //}

                wheel.m_raycastInfo.m_suspensionLengthBak = wheel.m_raycastInfo.m_suspensionLength;

                wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;

                float denominator = IndexedVector3.Dot(wheel.m_raycastInfo.m_contactNormalWS, wheel.m_raycastInfo.m_wheelDirectionWS);

                IndexedVector3 chassis_velocity_at_contactPoint;
                IndexedVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody().GetCenterOfMassPosition();

                chassis_velocity_at_contactPoint = GetRigidBody().GetVelocityInLocalPoint(ref relpos);

                float projVel = IndexedVector3.Dot(wheel.m_raycastInfo.m_contactNormalWS, chassis_velocity_at_contactPoint);

                //if (projVel > 1f)
                //{
                //    int ibreak = 0;
                //}

                if (denominator >= -0.1f)
                {
                    wheel.m_suspensionRelativeVelocity     = 0f;
                    wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f;
                }
                else
                {
                    float inv = -1f / denominator;
                    wheel.m_suspensionRelativeVelocity     = projVel * inv;
                    wheel.m_clippedInvContactDotSuspension = inv;
                }
            }
            else
            {
                //put wheel info as in rest position
                wheel.m_raycastInfo.m_suspensionLength = wheel.GetSuspensionRestLength();
                wheel.m_suspensionRelativeVelocity     = 0.0f;
                wheel.m_raycastInfo.m_contactNormalWS  = -wheel.m_raycastInfo.m_wheelDirectionWS;
                wheel.m_clippedInvContactDotSuspension = 1.0f;
            }

            return(depth);
        }
Exemplo n.º 3
0
        public virtual void UpdateFriction(float timeStep)
        {
            //calculate the impulse, so that the wheels don't move sidewards
            int numWheel = GetNumWheels();

            if (numWheel == 0)
            {
                return;
            }

            //m_forwardWS.resize(numWheel);
            //m_axle.resize(numWheel);
            //m_forwardImpulse.resize(numWheel);
            //m_sideImpulse.resize(numWheel);

            int numWheelsOnGround = 0;


            //collapse all those loops into one!
            for (int i = 0; i < numWheel; i++)
            {
                WheelInfo wheelInfo    = m_wheelInfo[i];
                RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
                if (groundObject != null)
                {
                    numWheelsOnGround++;
                }
                m_sideImpulse[i]    = 0f;
                m_forwardImpulse[i] = 0f;
            }

            //f (numWheelsOnGround != 4)
            //
            //   int ibreak = 0;
            //

            {
                //foreach(WheelInfo wheelInfo in m_wheelInfo)
                for (int i = 0; i < numWheel; ++i)
                {
                    WheelInfo wheelInfo    = m_wheelInfo[i];
                    RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;

                    if (groundObject != null)
                    {
                        IndexedMatrix wheelTrans = GetWheelTransformWS(i);

                        IndexedBasisMatrix wheelBasis0 = wheelTrans._basis;
                        m_axle[i] = new IndexedVector3(
                            wheelBasis0._el0[m_indexRightAxis],
                            wheelBasis0._el1[m_indexRightAxis],
                            wheelBasis0._el2[m_indexRightAxis]);

                        IndexedVector3 surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
                        float          proj         = IndexedVector3.Dot(m_axle[i], surfNormalWS);
                        m_axle[i] -= surfNormalWS * proj;
                        m_axle[i].Normalize();

                        m_forwardWS[i] = IndexedVector3.Cross(surfNormalWS, m_axle[i]);
                        m_forwardWS[i].Normalize();

                        IndexedVector3 tempAxle    = m_axle[i];
                        float          tempImpulse = m_sideImpulse[i];
                        ContactConstraint.ResolveSingleBilateral(m_chassisBody, ref wheelInfo.m_raycastInfo.m_contactPointWS,
                                                                 groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS,
                                                                 0f, ref tempAxle, ref tempImpulse, timeStep);
                        m_sideImpulse[i] = (tempImpulse * sideFrictionStiffness2);
                    }
                }
            }

            float sideFactor = 1f;
            float fwdFactor  = 0.5f;

            bool sliding = false;

            {
                for (int wheel = 0; wheel < numWheel; wheel++)
                {
                    WheelInfo wheelInfo    = m_wheelInfo[wheel];
                    RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;

                    float rollingFriction = 0f;

                    if (groundObject != null)
                    {
                        if (wheelInfo.m_engineForce != 0.0f)
                        {
                            rollingFriction = wheelInfo.m_engineForce * timeStep;
                        }
                        else
                        {
                            float             defaultRollingFrictionImpulse = 0f;
                            float             maxImpulse = (wheelInfo.m_brake != 0f) ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
                            IndexedVector3    tempWheel  = m_forwardWS[wheel];
                            WheelContactPoint contactPt  = new WheelContactPoint(m_chassisBody, groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS, ref tempWheel, maxImpulse);
                            m_forwardWS[wheel] = tempWheel;
                            rollingFriction    = CalcRollingFriction(contactPt);
                        }
                    }

                    //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)

                    m_forwardImpulse[wheel]       = 0f;
                    m_wheelInfo[wheel].m_skidInfo = 1f;

                    if (groundObject != null)
                    {
                        m_wheelInfo[wheel].m_skidInfo = 1f;

                        float maximp     = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
                        float maximpSide = maximp;

                        float maximpSquared = maximp * maximpSide;
                        m_forwardImpulse[wheel] = rollingFriction;            //wheelInfo.m_engineForce* timeStep;

                        float x = (m_forwardImpulse[wheel]) * fwdFactor;
                        float y = (m_sideImpulse[wheel]) * sideFactor;

                        float impulseSquared = (x * x + y * y);

                        if (impulseSquared > maximpSquared)
                        {
                            sliding = true;

                            float factor = (float)(maximp / Math.Sqrt(impulseSquared));

                            m_wheelInfo[wheel].m_skidInfo *= factor;
                        }
                    }
                }
            }

            if (sliding)
            {
                for (int wheel = 0; wheel < numWheel; wheel++)
                {
                    if (m_sideImpulse[wheel] != 0f)
                    {
                        if (m_wheelInfo[wheel].m_skidInfo < 1f)
                        {
                            m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
                            m_sideImpulse[wheel]    *= m_wheelInfo[wheel].m_skidInfo;
                        }
                    }
                }
            }

            // apply the impulses
            {
                for (int wheel = 0; wheel < numWheel; wheel++)
                {
                    WheelInfo wheelInfo = m_wheelInfo[wheel];

                    IndexedVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
                                             m_chassisBody.GetCenterOfMassPosition();


                    //if (m_forwardImpulse[wheel] > 5f || m_sideImpulse[wheel] > 5f)
                    //{
                    //    int ibreak = 0;
                    //}

                    if (m_forwardImpulse[wheel] != 0f)
                    {
                        m_chassisBody.ApplyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
                    }
                    if (m_sideImpulse[wheel] != 0f)
                    {
                        RigidBody groundObject = m_wheelInfo[wheel].m_raycastInfo.m_groundObject as RigidBody;

                        IndexedVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
                                                  groundObject.GetCenterOfMassPosition();

                        IndexedVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];

#if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
                        IndexedVector3 vChassisWorldUp = GetRigidBody().GetCenterOfMassTransform()._basis.GetColumn(m_indexUpAxis);
                        rel_pos -= vChassisWorldUp * (IndexedVector3.Dot(vChassisWorldUp, rel_pos) * (1.0f - wheelInfo.m_rollInfluence));
#else
                        rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif


                        m_chassisBody.ApplyImpulse(ref sideImp, ref rel_pos);

                        //apply friction impulse on the ground
                        IndexedVector3 temp = -sideImp;
                        groundObject.ApplyImpulse(ref temp, ref rel_pos2);
                    }
                }
            }
        }
Exemplo n.º 4
0
        public virtual void UpdateVehicle(float step)
        {
            int numWheels = GetNumWheels();

            {
                for (int i = 0; i < numWheels; i++)
                {
                    UpdateWheelTransform(i, false);
                }
            }

            m_currentVehicleSpeedKmHour = 3.6f * GetRigidBody().GetLinearVelocity().Length();

            IndexedMatrix chassisTrans = GetChassisWorldTransform();

            IndexedVector3 forwardW = new IndexedVector3(
                chassisTrans._basis[0, m_indexForwardAxis],
                chassisTrans._basis[1, m_indexForwardAxis],
                chassisTrans._basis[2, m_indexForwardAxis]);


            if (IndexedVector3.Dot(forwardW, GetRigidBody().GetLinearVelocity()) < 0f)
            {
                m_currentVehicleSpeedKmHour *= -1f;
            }

            //
            // simulate suspension
            //
            float[] depthData = new float[numWheels];
            float   depth     = 0f;

            for (int i = 0; i < numWheels; i++)
            {
                depth        = RayCast(m_wheelInfo[i]);
                depthData[i] = depth;

                if (m_wheelInfo[i].m_raycastInfo.m_isInContact == false)
                {
                    //int ibreak = 0;
                    depth        = RayCast(m_wheelInfo[i]);
                    depthData[i] = depth;
                }
            }

            UpdateSuspension(step);


            for (int i = 0; i < numWheels; ++i)
            {
                WheelInfo wheel = m_wheelInfo[i];
                //apply suspension force
                float suspensionForce = wheel.m_wheelsSuspensionForce;

                if (suspensionForce > wheel.m_maxSuspensionForce)
                {
                    suspensionForce = wheel.m_maxSuspensionForce;
                }

                IndexedVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
                IndexedVector3 relpos  = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody().GetCenterOfMassPosition();
                //if (impulse.Y < 30 || impulse.Y > 40)
                //{
                //    int ibreak = 0;
                //}
                //impulse = new IndexedVector3(0f, 1f, 0f);
                GetRigidBody().ApplyImpulse(ref impulse, ref relpos);
            }

            UpdateFriction(step);

            for (int i = 0; i < numWheels; ++i)
            {
                WheelInfo      wheel  = m_wheelInfo[i];
                IndexedVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - GetRigidBody().GetCenterOfMassPosition();
                IndexedVector3 vel    = GetRigidBody().GetVelocityInLocalPoint(ref relpos);

                if (wheel.m_raycastInfo.m_isInContact)
                {
                    IndexedMatrix chassisWorldTransform = GetChassisWorldTransform();

                    IndexedVector3 fwd = new IndexedVector3(
                        chassisWorldTransform._basis[0, m_indexForwardAxis],
                        chassisWorldTransform._basis[1, m_indexForwardAxis],
                        chassisWorldTransform._basis[2, m_indexForwardAxis]);

                    float proj = IndexedVector3.Dot(fwd, wheel.m_raycastInfo.m_contactNormalWS);
                    fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;

                    float proj2 = IndexedVector3.Dot(fwd, vel);

                    wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
                    wheel.m_rotation     += wheel.m_deltaRotation;
                }
                else
                {
                    wheel.m_rotation += wheel.m_deltaRotation;
                }

                wheel.m_deltaRotation *= 0.99f;        //damping of rotation when not in contact
            }
        }