| /* |
| * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ |
| * |
| * Permission to use, copy, modify, distribute and sell this software |
| * and its documentation for any purpose is hereby granted without fee, |
| * provided that the above copyright notice appear in all copies. |
| * Erwin Coumans makes no representations about the suitability |
| * of this software for any purpose. |
| * It is provided "as is" without express or implied warranty. |
| */ |
| |
| #include "LinearMath/btVector3.h" |
| #include "BulletDynamics/Vehicle/btRaycastVehicle.h" |
| |
| #include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" |
| #include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" |
| #include "LinearMath/btQuaternion.h" |
| #include "BulletDynamics/Dynamics/btDynamicsWorld.h" |
| #include "BulletDynamics/Vehicle/btVehicleRaycaster.h" |
| #include "BulletDynamics/Vehicle/btWheelInfo.h" |
| #include "LinearMath/btMinMax.h" |
| #include "LinearMath/btIDebugDraw.h" |
| #include "BulletDynamics/ConstraintSolver/btContactConstraint.h" |
| |
| static btRigidBody s_fixedObject( 0,0,0); |
| |
| btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) |
| :m_vehicleRaycaster(raycaster), |
| m_pitchControl(btScalar(0.)) |
| { |
| m_chassisBody = chassis; |
| m_indexRightAxis = 0; |
| m_indexUpAxis = 2; |
| m_indexForwardAxis = 1; |
| defaultInit(tuning); |
| } |
| |
| |
| void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning) |
| { |
| (void)tuning; |
| m_currentVehicleSpeedKmHour = btScalar(0.); |
| m_steeringValue = btScalar(0.); |
| |
| } |
| |
| |
| |
| btRaycastVehicle::~btRaycastVehicle() |
| { |
| } |
| |
| |
| // |
| // basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed |
| // |
| btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel) |
| { |
| |
| btWheelInfoConstructionInfo ci; |
| |
| ci.m_chassisConnectionCS = connectionPointCS; |
| ci.m_wheelDirectionCS = wheelDirectionCS0; |
| ci.m_wheelAxleCS = wheelAxleCS; |
| ci.m_suspensionRestLength = suspensionRestLength; |
| ci.m_wheelRadius = wheelRadius; |
| ci.m_suspensionStiffness = tuning.m_suspensionStiffness; |
| ci.m_wheelsDampingCompression = tuning.m_suspensionCompression; |
| ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping; |
| ci.m_frictionSlip = tuning.m_frictionSlip; |
| ci.m_bIsFrontWheel = isFrontWheel; |
| ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm; |
| |
| m_wheelInfo.push_back( btWheelInfo(ci)); |
| |
| btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1]; |
| |
| updateWheelTransformsWS( wheel , false ); |
| updateWheelTransform(getNumWheels()-1,false); |
| return wheel; |
| } |
| |
| |
| |
| |
| const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const |
| { |
| btAssert(wheelIndex < getNumWheels()); |
| const btWheelInfo& wheel = m_wheelInfo[wheelIndex]; |
| return wheel.m_worldTransform; |
| |
| } |
| |
| void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform) |
| { |
| |
| btWheelInfo& wheel = m_wheelInfo[ wheelIndex ]; |
| updateWheelTransformsWS(wheel,interpolatedTransform); |
| btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; |
| const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS; |
| btVector3 fwd = up.cross(right); |
| fwd = fwd.normalize(); |
| // up = right.cross(fwd); |
| // up.normalize(); |
| |
| //rotate around steering over de wheelAxleWS |
| btScalar steering = wheel.m_steering; |
| |
| btQuaternion steeringOrn(up,steering);//wheel.m_steering); |
| btMatrix3x3 steeringMat(steeringOrn); |
| |
| btQuaternion rotatingOrn(right,-wheel.m_rotation); |
| btMatrix3x3 rotatingMat(rotatingOrn); |
| |
| btMatrix3x3 basis2( |
| right[0],fwd[0],up[0], |
| right[1],fwd[1],up[1], |
| right[2],fwd[2],up[2] |
| ); |
| |
| wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2); |
| wheel.m_worldTransform.setOrigin( |
| wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength |
| ); |
| } |
| |
| void btRaycastVehicle::resetSuspension() |
| { |
| |
| int i; |
| for (i=0;i<m_wheelInfo.size(); i++) |
| { |
| btWheelInfo& wheel = m_wheelInfo[i]; |
| wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); |
| wheel.m_suspensionRelativeVelocity = btScalar(0.0); |
| |
| wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; |
| //wheel_info.setContactFriction(btScalar(0.0)); |
| wheel.m_clippedInvContactDotSuspension = btScalar(1.0); |
| } |
| } |
| |
| void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform) |
| { |
| wheel.m_raycastInfo.m_isInContact = false; |
| |
| btTransform chassisTrans = getChassisWorldTransform(); |
| if (interpolatedTransform && (getRigidBody()->getMotionState())) |
| { |
| getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); |
| } |
| |
| wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); |
| wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; |
| wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; |
| } |
| |
| btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) |
| { |
| updateWheelTransformsWS( wheel,false); |
| |
| |
| btScalar depth = -1; |
| |
| btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius; |
| |
| btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); |
| const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; |
| wheel.m_raycastInfo.m_contactPointWS = source + rayvector; |
| const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; |
| |
| btScalar param = btScalar(0.); |
| |
| btVehicleRaycaster::btVehicleRaycasterResult rayResults; |
| |
| btAssert(m_vehicleRaycaster); |
| |
| void* object = m_vehicleRaycaster->castRay(source,target,rayResults); |
| |
| wheel.m_raycastInfo.m_groundObject = 0; |
| |
| if (object) |
| { |
| 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; |
| |
| |
| btScalar hitDistance = param*raylen; |
| wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; |
| //clamp on max suspension travel |
| |
| btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01); |
| btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01); |
| 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; |
| } |
| |
| wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; |
| |
| btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); |
| |
| btVector3 chassis_velocity_at_contactPoint; |
| btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); |
| |
| chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); |
| |
| btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); |
| |
| if ( denominator >= btScalar(-0.1)) |
| { |
| wheel.m_suspensionRelativeVelocity = btScalar(0.0); |
| wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); |
| } |
| else |
| { |
| btScalar inv = btScalar(-1.) / 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 = btScalar(0.0); |
| wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; |
| wheel.m_clippedInvContactDotSuspension = btScalar(1.0); |
| } |
| |
| return depth; |
| } |
| |
| |
| const btTransform& btRaycastVehicle::getChassisWorldTransform() const |
| { |
| /*if (getRigidBody()->getMotionState()) |
| { |
| btTransform chassisWorldTrans; |
| getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans); |
| return chassisWorldTrans; |
| } |
| */ |
| |
| |
| return getRigidBody()->getCenterOfMassTransform(); |
| } |
| |
| |
| void btRaycastVehicle::updateVehicle( btScalar step ) |
| { |
| { |
| for (int i=0;i<getNumWheels();i++) |
| { |
| updateWheelTransform(i,false); |
| } |
| } |
| |
| |
| m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length(); |
| |
| const btTransform& chassisTrans = getChassisWorldTransform(); |
| |
| btVector3 forwardW ( |
| chassisTrans.getBasis()[0][m_indexForwardAxis], |
| chassisTrans.getBasis()[1][m_indexForwardAxis], |
| chassisTrans.getBasis()[2][m_indexForwardAxis]); |
| |
| if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.)) |
| { |
| m_currentVehicleSpeedKmHour *= btScalar(-1.); |
| } |
| |
| // |
| // simulate suspension |
| // |
| |
| int i=0; |
| for (i=0;i<m_wheelInfo.size();i++) |
| { |
| btScalar depth; |
| depth = rayCast( m_wheelInfo[i]); |
| } |
| |
| updateSuspension(step); |
| |
| |
| for (i=0;i<m_wheelInfo.size();i++) |
| { |
| //apply suspension force |
| btWheelInfo& wheel = m_wheelInfo[i]; |
| |
| btScalar suspensionForce = wheel.m_wheelsSuspensionForce; |
| |
| btScalar gMaxSuspensionForce = btScalar(6000.); |
| if (suspensionForce > gMaxSuspensionForce) |
| { |
| suspensionForce = gMaxSuspensionForce; |
| } |
| btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; |
| btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition(); |
| |
| getRigidBody()->applyImpulse(impulse, relpos); |
| |
| } |
| |
| |
| |
| updateFriction( step); |
| |
| |
| for (i=0;i<m_wheelInfo.size();i++) |
| { |
| btWheelInfo& wheel = m_wheelInfo[i]; |
| btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition(); |
| btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos ); |
| |
| if (wheel.m_raycastInfo.m_isInContact) |
| { |
| const btTransform& chassisWorldTransform = getChassisWorldTransform(); |
| |
| btVector3 fwd ( |
| chassisWorldTransform.getBasis()[0][m_indexForwardAxis], |
| chassisWorldTransform.getBasis()[1][m_indexForwardAxis], |
| chassisWorldTransform.getBasis()[2][m_indexForwardAxis]); |
| |
| btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); |
| fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; |
| |
| btScalar proj2 = fwd.dot(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 *= btScalar(0.99);//damping of rotation when not in contact |
| |
| } |
| |
| |
| |
| } |
| |
| |
| void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel) |
| { |
| btAssert(wheel>=0 && wheel < getNumWheels()); |
| |
| btWheelInfo& wheelInfo = getWheelInfo(wheel); |
| wheelInfo.m_steering = steering; |
| } |
| |
| |
| |
| btScalar btRaycastVehicle::getSteeringValue(int wheel) const |
| { |
| return getWheelInfo(wheel).m_steering; |
| } |
| |
| |
| void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) |
| { |
| btAssert(wheel>=0 && wheel < getNumWheels()); |
| btWheelInfo& wheelInfo = getWheelInfo(wheel); |
| wheelInfo.m_engineForce = force; |
| } |
| |
| |
| const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const |
| { |
| btAssert((index >= 0) && (index < getNumWheels())); |
| |
| return m_wheelInfo[index]; |
| } |
| |
| btWheelInfo& btRaycastVehicle::getWheelInfo(int index) |
| { |
| btAssert((index >= 0) && (index < getNumWheels())); |
| |
| return m_wheelInfo[index]; |
| } |
| |
| void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex) |
| { |
| btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels())); |
| getWheelInfo(wheelIndex).m_brake = brake; |
| } |
| |
| |
| void btRaycastVehicle::updateSuspension(btScalar deltaTime) |
| { |
| (void)deltaTime; |
| |
| btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass(); |
| |
| for (int w_it=0; w_it<getNumWheels(); w_it++) |
| { |
| btWheelInfo &wheel_info = m_wheelInfo[w_it]; |
| |
| if ( wheel_info.m_raycastInfo.m_isInContact ) |
| { |
| btScalar force; |
| // Spring |
| { |
| btScalar susp_length = wheel_info.getSuspensionRestLength(); |
| btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength; |
| |
| btScalar length_diff = (susp_length - current_length); |
| |
| force = wheel_info.m_suspensionStiffness |
| * length_diff * wheel_info.m_clippedInvContactDotSuspension; |
| } |
| |
| // Damper |
| { |
| btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity; |
| { |
| btScalar susp_damping; |
| if ( projected_rel_vel < btScalar(0.0) ) |
| { |
| susp_damping = wheel_info.m_wheelsDampingCompression; |
| } |
| else |
| { |
| susp_damping = wheel_info.m_wheelsDampingRelaxation; |
| } |
| force -= susp_damping * projected_rel_vel; |
| } |
| } |
| |
| // RESULT |
| wheel_info.m_wheelsSuspensionForce = force * chassisMass; |
| if (wheel_info.m_wheelsSuspensionForce < btScalar(0.)) |
| { |
| wheel_info.m_wheelsSuspensionForce = btScalar(0.); |
| } |
| } |
| else |
| { |
| wheel_info.m_wheelsSuspensionForce = btScalar(0.0); |
| } |
| } |
| |
| } |
| |
| |
| struct btWheelContactPoint |
| { |
| btRigidBody* m_body0; |
| btRigidBody* m_body1; |
| btVector3 m_frictionPositionWorld; |
| btVector3 m_frictionDirectionWorld; |
| btScalar m_jacDiagABInv; |
| btScalar m_maxImpulse; |
| |
| |
| btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse) |
| :m_body0(body0), |
| m_body1(body1), |
| m_frictionPositionWorld(frictionPosWorld), |
| m_frictionDirectionWorld(frictionDirectionWorld), |
| m_maxImpulse(maxImpulse) |
| { |
| btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); |
| btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); |
| btScalar relaxation = 1.f; |
| m_jacDiagABInv = relaxation/(denom0+denom1); |
| } |
| |
| |
| |
| }; |
| |
| btScalar calcRollingFriction(btWheelContactPoint& contactPoint); |
| btScalar calcRollingFriction(btWheelContactPoint& contactPoint) |
| { |
| |
| btScalar j1=0.f; |
| |
| const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld; |
| |
| btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); |
| btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition(); |
| |
| btScalar maxImpulse = contactPoint.m_maxImpulse; |
| |
| btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1); |
| btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2); |
| btVector3 vel = vel1 - vel2; |
| |
| btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel); |
| |
| // calculate j that moves us to zero relative velocity |
| j1 = -vrel * contactPoint.m_jacDiagABInv; |
| btSetMin(j1, maxImpulse); |
| btSetMax(j1, -maxImpulse); |
| |
| return j1; |
| } |
| |
| |
| |
| |
| btScalar sideFrictionStiffness2 = btScalar(1.0); |
| void btRaycastVehicle::updateFriction(btScalar timeStep) |
| { |
| |
| //calculate the impulse, so that the wheels don't move sidewards |
| int numWheel = getNumWheels(); |
| if (!numWheel) |
| 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<getNumWheels();i++) |
| { |
| btWheelInfo& wheelInfo = m_wheelInfo[i]; |
| class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; |
| if (groundObject) |
| numWheelsOnGround++; |
| m_sideImpulse[i] = btScalar(0.); |
| m_forwardImpulse[i] = btScalar(0.); |
| |
| } |
| |
| { |
| |
| for (int i=0;i<getNumWheels();i++) |
| { |
| |
| btWheelInfo& wheelInfo = m_wheelInfo[i]; |
| |
| class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; |
| |
| if (groundObject) |
| { |
| |
| const btTransform& wheelTrans = getWheelTransformWS( i ); |
| |
| btMatrix3x3 wheelBasis0 = wheelTrans.getBasis(); |
| m_axle[i] = btVector3( |
| wheelBasis0[0][m_indexRightAxis], |
| wheelBasis0[1][m_indexRightAxis], |
| wheelBasis0[2][m_indexRightAxis]); |
| |
| const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; |
| btScalar proj = m_axle[i].dot(surfNormalWS); |
| m_axle[i] -= surfNormalWS * proj; |
| m_axle[i] = m_axle[i].normalize(); |
| |
| m_forwardWS[i] = surfNormalWS.cross(m_axle[i]); |
| m_forwardWS[i].normalize(); |
| |
| |
| resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS, |
| *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, |
| btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep); |
| |
| m_sideImpulse[i] *= sideFrictionStiffness2; |
| |
| } |
| |
| |
| } |
| } |
| |
| btScalar sideFactor = btScalar(1.); |
| btScalar fwdFactor = 0.5; |
| |
| bool sliding = false; |
| { |
| for (int wheel =0;wheel <getNumWheels();wheel++) |
| { |
| btWheelInfo& wheelInfo = m_wheelInfo[wheel]; |
| class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; |
| |
| btScalar rollingFriction = 0.f; |
| |
| if (groundObject) |
| { |
| if (wheelInfo.m_engineForce != 0.f) |
| { |
| rollingFriction = wheelInfo.m_engineForce* timeStep; |
| } else |
| { |
| btScalar defaultRollingFrictionImpulse = 0.f; |
| btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; |
| btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse); |
| rollingFriction = calcRollingFriction(contactPt); |
| } |
| } |
| |
| //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) |
| |
| |
| |
| |
| m_forwardImpulse[wheel] = btScalar(0.); |
| m_wheelInfo[wheel].m_skidInfo= btScalar(1.); |
| |
| if (groundObject) |
| { |
| m_wheelInfo[wheel].m_skidInfo= btScalar(1.); |
| |
| btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip; |
| btScalar maximpSide = maximp; |
| |
| btScalar maximpSquared = maximp * maximpSide; |
| |
| |
| m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep; |
| |
| btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor; |
| btScalar y = (m_sideImpulse[wheel] ) * sideFactor; |
| |
| btScalar impulseSquared = (x*x + y*y); |
| |
| if (impulseSquared > maximpSquared) |
| { |
| sliding = true; |
| |
| btScalar factor = maximp / btSqrt(impulseSquared); |
| |
| m_wheelInfo[wheel].m_skidInfo *= factor; |
| } |
| } |
| |
| } |
| } |
| |
| |
| |
| |
| if (sliding) |
| { |
| for (int wheel = 0;wheel < getNumWheels(); wheel++) |
| { |
| if (m_sideImpulse[wheel] != btScalar(0.)) |
| { |
| if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.)) |
| { |
| m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; |
| m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; |
| } |
| } |
| } |
| } |
| |
| // apply the impulses |
| { |
| for (int wheel = 0;wheel<getNumWheels() ; wheel++) |
| { |
| btWheelInfo& wheelInfo = m_wheelInfo[wheel]; |
| |
| btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - |
| m_chassisBody->getCenterOfMassPosition(); |
| |
| if (m_forwardImpulse[wheel] != btScalar(0.)) |
| { |
| m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos); |
| } |
| if (m_sideImpulse[wheel] != btScalar(0.)) |
| { |
| class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; |
| |
| btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - |
| groundObject->getCenterOfMassPosition(); |
| |
| |
| btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; |
| |
| rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence; |
| m_chassisBody->applyImpulse(sideImp,rel_pos); |
| |
| //apply friction impulse on the ground |
| groundObject->applyImpulse(-sideImp,rel_pos2); |
| } |
| } |
| } |
| |
| |
| } |
| |
| |
| |
| void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer) |
| { |
| |
| for (int v=0;v<this->getNumWheels();v++) |
| { |
| btVector3 wheelColor(0,255,255); |
| if (getWheelInfo(v).m_raycastInfo.m_isInContact) |
| { |
| wheelColor.setValue(0,0,255); |
| } else |
| { |
| wheelColor.setValue(255,0,255); |
| } |
| |
| btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin(); |
| |
| btVector3 axle = btVector3( |
| getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()], |
| getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()], |
| getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]); |
| |
| //debug wheels (cylinders) |
| debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); |
| debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); |
| |
| } |
| } |
| |
| |
| void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) |
| { |
| // RayResultCallback& resultCallback; |
| |
| btCollisionWorld::ClosestRayResultCallback rayCallback(from,to); |
| |
| m_dynamicsWorld->rayTest(from, to, rayCallback); |
| |
| if (rayCallback.hasHit()) |
| { |
| |
| btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject); |
| if (body && body->hasContactResponse()) |
| { |
| result.m_hitPointInWorld = rayCallback.m_hitPointWorld; |
| result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld; |
| result.m_hitNormalInWorld.normalize(); |
| result.m_distFraction = rayCallback.m_closestHitFraction; |
| return body; |
| } |
| } |
| return 0; |
| } |
| |