summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp')
-rw-r--r--thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp772
1 files changed, 772 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp
new file mode 100644
index 0000000000..a7b1688469
--- /dev/null
+++ b/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp
@@ -0,0 +1,772 @@
+/*
+ * 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 "btRaycastVehicle.h"
+
+#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
+#include "LinearMath/btQuaternion.h"
+#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
+#include "btVehicleRaycaster.h"
+#include "btWheelInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+
+#define ROLLING_INFLUENCE_FIX
+
+
+btRigidBody& btActionInterface::getFixedBody()
+{
+ static btRigidBody s_fixed(0, 0,0);
+ s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+ return s_fixed;
+}
+
+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;
+ ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
+
+ 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 = &getFixedBody();///@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;
+
+ if (suspensionForce > wheel.m_maxSuspensionForce)
+ {
+ suspensionForce = wheel.m_maxSuspensionForce;
+ }
+ 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];
+
+#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
+ btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
+ rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
+#else
+ rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
+#endif
+ 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,1,1);
+ if (getWheelInfo(v).m_raycastInfo.m_isInContact)
+ {
+ wheelColor.setValue(0,0,1);
+ } else
+ {
+ wheelColor.setValue(1,0,1);
+ }
+
+ 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())
+ {
+
+ const 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 (void*)body;
+ }
+ }
+ return 0;
+}
+
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-05 14:48:41 +0000