diff options
Diffstat (limited to 'extern/bullet2/BulletDynamics/Vehicle')
5 files changed, 1204 insertions, 0 deletions
diff --git a/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.cpp new file mode 100644 index 00000000000..b42c024f178 --- /dev/null +++ b/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -0,0 +1,758 @@ +/* + * 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" + + + +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]; + + 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,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()) + { + + 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; +} + diff --git a/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.h new file mode 100644 index 00000000000..5ce80f4d275 --- /dev/null +++ b/extern/bullet2/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -0,0 +1,236 @@ +/* + * 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. +*/ +#ifndef RAYCASTVEHICLE_H +#define RAYCASTVEHICLE_H + +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "btVehicleRaycaster.h" +class btDynamicsWorld; +#include "LinearMath/btAlignedObjectArray.h" +#include "btWheelInfo.h" +#include "BulletDynamics/Dynamics/btActionInterface.h" + +class btVehicleTuning; + +///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle. +class btRaycastVehicle : public btActionInterface +{ + + btAlignedObjectArray<btVector3> m_forwardWS; + btAlignedObjectArray<btVector3> m_axle; + btAlignedObjectArray<btScalar> m_forwardImpulse; + btAlignedObjectArray<btScalar> m_sideImpulse; + + ///backwards compatibility + int m_userConstraintType; + int m_userConstraintId; + +public: + class btVehicleTuning + { + public: + + btVehicleTuning() + :m_suspensionStiffness(btScalar(5.88)), + m_suspensionCompression(btScalar(0.83)), + m_suspensionDamping(btScalar(0.88)), + m_maxSuspensionTravelCm(btScalar(500.)), + m_frictionSlip(btScalar(10.5)), + m_maxSuspensionForce(btScalar(6000.)) + { + } + btScalar m_suspensionStiffness; + btScalar m_suspensionCompression; + btScalar m_suspensionDamping; + btScalar m_maxSuspensionTravelCm; + btScalar m_frictionSlip; + btScalar m_maxSuspensionForce; + + }; +private: + + btScalar m_tau; + btScalar m_damping; + btVehicleRaycaster* m_vehicleRaycaster; + btScalar m_pitchControl; + btScalar m_steeringValue; + btScalar m_currentVehicleSpeedKmHour; + + btRigidBody* m_chassisBody; + + int m_indexRightAxis; + int m_indexUpAxis; + int m_indexForwardAxis; + + void defaultInit(const btVehicleTuning& tuning); + +public: + + //constructor to create a car from an existing rigidbody + btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ); + + virtual ~btRaycastVehicle() ; + + + ///btActionInterface interface + virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step) + { + (void) collisionWorld; + updateVehicle(step); + } + + + ///btActionInterface interface + void debugDraw(btIDebugDraw* debugDrawer); + + const btTransform& getChassisWorldTransform() const; + + btScalar rayCast(btWheelInfo& wheel); + + virtual void updateVehicle(btScalar step); + + + void resetSuspension(); + + btScalar getSteeringValue(int wheel) const; + + void setSteeringValue(btScalar steering,int wheel); + + + void applyEngineForce(btScalar force, int wheel); + + const btTransform& getWheelTransformWS( int wheelIndex ) const; + + void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true ); + + void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth); + + btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel); + + inline int getNumWheels() const { + return int (m_wheelInfo.size()); + } + + btAlignedObjectArray<btWheelInfo> m_wheelInfo; + + + const btWheelInfo& getWheelInfo(int index) const; + + btWheelInfo& getWheelInfo(int index); + + void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true); + + + void setBrake(btScalar brake,int wheelIndex); + + void setPitchControl(btScalar pitch) + { + m_pitchControl = pitch; + } + + void updateSuspension(btScalar deltaTime); + + virtual void updateFriction(btScalar timeStep); + + + + inline btRigidBody* getRigidBody() + { + return m_chassisBody; + } + + const btRigidBody* getRigidBody() const + { + return m_chassisBody; + } + + inline int getRightAxis() const + { + return m_indexRightAxis; + } + inline int getUpAxis() const + { + return m_indexUpAxis; + } + + inline int getForwardAxis() const + { + return m_indexForwardAxis; + } + + + ///Worldspace forward vector + btVector3 getForwardVector() const + { + const btTransform& chassisTrans = getChassisWorldTransform(); + + btVector3 forwardW ( + chassisTrans.getBasis()[0][m_indexForwardAxis], + chassisTrans.getBasis()[1][m_indexForwardAxis], + chassisTrans.getBasis()[2][m_indexForwardAxis]); + + return forwardW; + } + + ///Velocity of vehicle (positive if velocity vector has same direction as foward vector) + btScalar getCurrentSpeedKmHour() const + { + return m_currentVehicleSpeedKmHour; + } + + virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex) + { + m_indexRightAxis = rightIndex; + m_indexUpAxis = upIndex; + m_indexForwardAxis = forwardIndex; + } + + + ///backwards compatibility + int getUserConstraintType() const + { + return m_userConstraintType ; + } + + void setUserConstraintType(int userConstraintType) + { + m_userConstraintType = userConstraintType; + }; + + void setUserConstraintId(int uid) + { + m_userConstraintId = uid; + } + + int getUserConstraintId() const + { + return m_userConstraintId; + } + +}; + +class btDefaultVehicleRaycaster : public btVehicleRaycaster +{ + btDynamicsWorld* m_dynamicsWorld; +public: + btDefaultVehicleRaycaster(btDynamicsWorld* world) + :m_dynamicsWorld(world) + { + } + + virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result); + +}; + + +#endif //RAYCASTVEHICLE_H + diff --git a/extern/bullet2/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet2/BulletDynamics/Vehicle/btVehicleRaycaster.h new file mode 100644 index 00000000000..5112ce6d420 --- /dev/null +++ b/extern/bullet2/BulletDynamics/Vehicle/btVehicleRaycaster.h @@ -0,0 +1,35 @@ +/* + * 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. +*/ +#ifndef VEHICLE_RAYCASTER_H +#define VEHICLE_RAYCASTER_H + +#include "LinearMath/btVector3.h" + +/// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting +struct btVehicleRaycaster +{ +virtual ~btVehicleRaycaster() +{ +} + struct btVehicleRaycasterResult + { + btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){}; + btVector3 m_hitPointInWorld; + btVector3 m_hitNormalInWorld; + btScalar m_distFraction; + }; + + virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0; + +}; + +#endif //VEHICLE_RAYCASTER_H + diff --git a/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.cpp b/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.cpp new file mode 100644 index 00000000000..ef93c16fffc --- /dev/null +++ b/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.cpp @@ -0,0 +1,56 @@ +/* + * 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 "btWheelInfo.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity + + +btScalar btWheelInfo::getSuspensionRestLength() const +{ + + return m_suspensionRestLength1; + +} + +void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo) +{ + (void)raycastInfo; + + + if (m_raycastInfo.m_isInContact) + + { + btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); + btVector3 chassis_velocity_at_contactPoint; + btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); + chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); + btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); + if ( project >= btScalar(-0.1)) + { + m_suspensionRelativeVelocity = btScalar(0.0); + m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); + } + else + { + btScalar inv = btScalar(-1.) / project; + m_suspensionRelativeVelocity = projVel * inv; + m_clippedInvContactDotSuspension = inv; + } + + } + + else // Not in contact : position wheel in a nice (rest length) position + { + m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); + m_suspensionRelativeVelocity = btScalar(0.0); + m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; + m_clippedInvContactDotSuspension = btScalar(1.0); + } +} diff --git a/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.h new file mode 100644 index 00000000000..b74f8c80acb --- /dev/null +++ b/extern/bullet2/BulletDynamics/Vehicle/btWheelInfo.h @@ -0,0 +1,119 @@ +/* + * 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. +*/ +#ifndef WHEEL_INFO_H +#define WHEEL_INFO_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" + +class btRigidBody; + +struct btWheelInfoConstructionInfo +{ + btVector3 m_chassisConnectionCS; + btVector3 m_wheelDirectionCS; + btVector3 m_wheelAxleCS; + btScalar m_suspensionRestLength; + btScalar m_maxSuspensionTravelCm; + btScalar m_wheelRadius; + + btScalar m_suspensionStiffness; + btScalar m_wheelsDampingCompression; + btScalar m_wheelsDampingRelaxation; + btScalar m_frictionSlip; + btScalar m_maxSuspensionForce; + bool m_bIsFrontWheel; + +}; + +/// btWheelInfo contains information per wheel about friction and suspension. +struct btWheelInfo +{ + struct RaycastInfo + { + //set by raycaster + btVector3 m_contactNormalWS;//contactnormal + btVector3 m_contactPointWS;//raycast hitpoint + btScalar m_suspensionLength; + btVector3 m_hardPointWS;//raycast starting point + btVector3 m_wheelDirectionWS; //direction in worldspace + btVector3 m_wheelAxleWS; // axle in worldspace + bool m_isInContact; + void* m_groundObject; //could be general void* ptr + }; + + RaycastInfo m_raycastInfo; + + btTransform m_worldTransform; + + btVector3 m_chassisConnectionPointCS; //const + btVector3 m_wheelDirectionCS;//const + btVector3 m_wheelAxleCS; // const or modified by steering + btScalar m_suspensionRestLength1;//const + btScalar m_maxSuspensionTravelCm; + btScalar getSuspensionRestLength() const; + btScalar m_wheelsRadius;//const + btScalar m_suspensionStiffness;//const + btScalar m_wheelsDampingCompression;//const + btScalar m_wheelsDampingRelaxation;//const + btScalar m_frictionSlip; + btScalar m_steering; + btScalar m_rotation; + btScalar m_deltaRotation; + btScalar m_rollInfluence; + btScalar m_maxSuspensionForce; + + btScalar m_engineForce; + + btScalar m_brake; + + bool m_bIsFrontWheel; + + void* m_clientInfo;//can be used to store pointer to sync transforms... + + btWheelInfo(btWheelInfoConstructionInfo& ci) + + { + + m_suspensionRestLength1 = ci.m_suspensionRestLength; + m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; + + m_wheelsRadius = ci.m_wheelRadius; + m_suspensionStiffness = ci.m_suspensionStiffness; + m_wheelsDampingCompression = ci.m_wheelsDampingCompression; + m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation; + m_chassisConnectionPointCS = ci.m_chassisConnectionCS; + m_wheelDirectionCS = ci.m_wheelDirectionCS; + m_wheelAxleCS = ci.m_wheelAxleCS; + m_frictionSlip = ci.m_frictionSlip; + m_steering = btScalar(0.); + m_engineForce = btScalar(0.); + m_rotation = btScalar(0.); + m_deltaRotation = btScalar(0.); + m_brake = btScalar(0.); + m_rollInfluence = btScalar(0.1); + m_bIsFrontWheel = ci.m_bIsFrontWheel; + m_maxSuspensionForce = ci.m_maxSuspensionForce; + + } + + void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); + + btScalar m_clippedInvContactDotSuspension; + btScalar m_suspensionRelativeVelocity; + //calculated by suspension + btScalar m_wheelsSuspensionForce; + btScalar m_skidInfo; + +}; + +#endif //WHEEL_INFO_H + |