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); } } } }
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); } } } }