Esempio n. 1
0
        public static float CalcRollingFriction(WheelContactPoint contactPoint)
        {
            float j1 = 0f;

            IndexedVector3 contactPosWorld = contactPoint.m_frictionPositionWorld;

            IndexedVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0.GetCenterOfMassPosition();
            IndexedVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1.GetCenterOfMassPosition();

            float maxImpulse = contactPoint.m_maxImpulse;

            IndexedVector3 vel1 = contactPoint.m_body0.GetVelocityInLocalPoint(ref rel_pos1);
            IndexedVector3 vel2 = contactPoint.m_body1.GetVelocityInLocalPoint(ref rel_pos2);
            IndexedVector3 vel  = vel1 - vel2;

            float vrel = IndexedVector3.Dot(contactPoint.m_frictionDirectionWorld, vel);

            // calculate j that moves us to zero relative velocity
            j1 = -vrel * contactPoint.m_jacDiagABInv;
            j1 = Math.Min(j1, maxImpulse);
            j1 = Math.Max(j1, -maxImpulse);

            return(j1);
        }
Esempio n. 2
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);
                    }
                }
            }
        }