diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Vehicle')
5 files changed, 0 insertions, 1126 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp deleted file mode 100644 index fc70d8e637..0000000000 --- a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ /dev/null @@ -1,708 +0,0 @@ -/* - * 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; - basis2[0][m_indexRightAxis] = -right[0]; - basis2[1][m_indexRightAxis] = -right[1]; - basis2[2][m_indexRightAxis] = -right[2]; - - basis2[0][m_indexUpAxis] = up[0]; - basis2[1][m_indexUpAxis] = up[1]; - basis2[2][m_indexUpAxis] = up[2]; - - basis2[0][m_indexForwardAxis] = fwd[0]; - basis2[1][m_indexForwardAxis] = fwd[1]; - basis2[2][m_indexForwardAxis] = fwd[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, int numWheelsOnGround); -btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround) -{ - 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 / btScalar(numWheelsOnGround); - 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); - btAssert(numWheelsOnGround > 0); - rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround); - } - } - - //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; -} diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h b/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h deleted file mode 100644 index 99d6894e56..0000000000 --- a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h +++ /dev/null @@ -1,218 +0,0 @@ -/* - * 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 BT_RAYCASTVEHICLE_H -#define BT_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: - 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 //BT_RAYCASTVEHICLE_H diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btVehicleRaycaster.h b/thirdparty/bullet/BulletDynamics/Vehicle/btVehicleRaycaster.h deleted file mode 100644 index 2c44ce546c..0000000000 --- a/thirdparty/bullet/BulletDynamics/Vehicle/btVehicleRaycaster.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://bulletphysics.org - * - * 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 BT_VEHICLE_RAYCASTER_H -#define BT_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 //BT_VEHICLE_RAYCASTER_H diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.cpp b/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.cpp deleted file mode 100644 index d5c12f223b..0000000000 --- a/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/* - * 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/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.h b/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.h deleted file mode 100644 index af88b8ff83..0000000000 --- a/thirdparty/bullet/BulletDynamics/Vehicle/btWheelInfo.h +++ /dev/null @@ -1,116 +0,0 @@ -/* - * 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 BT_WHEEL_INFO_H -#define BT_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() {} - - 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 //BT_WHEEL_INFO_H |