From 334ce42650298d79a01bd7ba69884e9861541526 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Tue, 29 Aug 2006 23:55:32 +0000 Subject: [PATCH] added basic RaycastVehicle support, and CcdPhysicsEnvironment::getAppliedImpulse(int constraintId), this value is useful as treshold to break constraints. --- BulletDynamics/CMakeLists.txt | 2 + .../ConstraintSolver/HingeConstraint.cpp | 6 +- .../Point2PointConstraint.cpp | 4 +- .../ConstraintSolver/TypedConstraint.cpp | 9 +- .../ConstraintSolver/TypedConstraint.h | 16 + BulletDynamics/Jamfile | 15 +- BulletDynamics/Vehicle/RaycastVehicle.cpp | 605 ++++++++++++++++++ BulletDynamics/Vehicle/RaycastVehicle.h | 160 +++++ BulletDynamics/Vehicle/VehicleRaycaster.h | 32 + BulletDynamics/Vehicle/WheelInfo.cpp | 55 ++ BulletDynamics/Vehicle/WheelInfo.h | 116 ++++ Demos/CollisionInterfaceDemo/Jamfile | 6 +- .../CcdPhysics/CcdPhysicsEnvironment.cpp | 19 +- .../CcdPhysics/CcdPhysicsEnvironment.h | 5 +- .../Common/PHY_IPhysicsEnvironment.h | 2 + 15 files changed, 1033 insertions(+), 19 deletions(-) create mode 100644 BulletDynamics/Vehicle/RaycastVehicle.cpp create mode 100644 BulletDynamics/Vehicle/RaycastVehicle.h create mode 100644 BulletDynamics/Vehicle/VehicleRaycaster.h create mode 100644 BulletDynamics/Vehicle/WheelInfo.cpp create mode 100644 BulletDynamics/Vehicle/WheelInfo.h diff --git a/BulletDynamics/CMakeLists.txt b/BulletDynamics/CMakeLists.txt index 46d2011e6..cb1f74bb6 100644 --- a/BulletDynamics/CMakeLists.txt +++ b/BulletDynamics/CMakeLists.txt @@ -14,4 +14,6 @@ ADD_LIBRARY(LibBulletDynamics Dynamics/BU_Joint.cpp Dynamics/ContactJoint.cpp Dynamics/RigidBody.cpp + Vehicle/RaycastVehicle.cpp + Vehicle/WheelInfo.cpp ) \ No newline at end of file diff --git a/BulletDynamics/ConstraintSolver/HingeConstraint.cpp b/BulletDynamics/ConstraintSolver/HingeConstraint.cpp index c1762c91d..fb7f1f47c 100644 --- a/BulletDynamics/ConstraintSolver/HingeConstraint.cpp +++ b/BulletDynamics/ConstraintSolver/HingeConstraint.cpp @@ -28,7 +28,7 @@ HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector SimdVector3& axisInA,SimdVector3& axisInB) :TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), m_axisInA(axisInA), -m_axisInB(axisInB), +m_axisInB(-axisInB), m_angularOnly(false) { @@ -47,6 +47,8 @@ m_angularOnly(false) void HingeConstraint::BuildJacobian() { + m_appliedImpulse = 0.f; + SimdVector3 normal(0,0,0); if (!m_angularOnly) @@ -216,7 +218,7 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep) //positional error (zeroth order error) SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal SimdScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; - + m_appliedImpulse += impulse; SimdVector3 impulse_vector = normal * impulse; m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); diff --git a/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp b/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp index 64b470f43..b3e25e0fb 100644 --- a/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp +++ b/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp @@ -40,6 +40,8 @@ Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,const SimdVector3& p void Point2PointConstraint::BuildJacobian() { + m_appliedImpulse = 0.f; + SimdVector3 normal(0,0,0); for (int i=0;i<3;i++) @@ -98,7 +100,7 @@ void Point2PointConstraint::SolveConstraint(SimdScalar timeStep) SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal SimdScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; - + m_appliedImpulse+=impulse; SimdVector3 impulse_vector = normal * impulse; m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); diff --git a/BulletDynamics/ConstraintSolver/TypedConstraint.cpp b/BulletDynamics/ConstraintSolver/TypedConstraint.cpp index d1605b7a0..61ea8ace7 100644 --- a/BulletDynamics/ConstraintSolver/TypedConstraint.cpp +++ b/BulletDynamics/ConstraintSolver/TypedConstraint.cpp @@ -24,7 +24,8 @@ TypedConstraint::TypedConstraint() : m_userConstraintType(-1), m_userConstraintId(-1), m_rbA(s_fixed), -m_rbB(s_fixed) +m_rbB(s_fixed), +m_appliedImpulse(0.f) { s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f)); } @@ -32,7 +33,8 @@ TypedConstraint::TypedConstraint(RigidBody& rbA) : m_userConstraintType(-1), m_userConstraintId(-1), m_rbA(rbA), -m_rbB(s_fixed) +m_rbB(s_fixed), +m_appliedImpulse(0.f) { s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f)); @@ -43,7 +45,8 @@ TypedConstraint::TypedConstraint(RigidBody& rbA,RigidBody& rbB) : m_userConstraintType(-1), m_userConstraintId(-1), m_rbA(rbA), -m_rbB(rbB) +m_rbB(rbB), +m_appliedImpulse(0.f) { s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f)); diff --git a/BulletDynamics/ConstraintSolver/TypedConstraint.h b/BulletDynamics/ConstraintSolver/TypedConstraint.h index 50a5ab20f..bc90814de 100644 --- a/BulletDynamics/ConstraintSolver/TypedConstraint.h +++ b/BulletDynamics/ConstraintSolver/TypedConstraint.h @@ -24,10 +24,13 @@ class TypedConstraint { int m_userConstraintType; int m_userConstraintId; + protected: RigidBody& m_rbA; RigidBody& m_rbB; + float m_appliedImpulse; + public: @@ -50,6 +53,15 @@ public: return m_rbB; } + RigidBody& GetRigidBodyA() + { + return m_rbA; + } + RigidBody& GetRigidBodyB() + { + return m_rbB; + } + int GetUserConstraintType() const { return m_userConstraintType ; @@ -69,6 +81,10 @@ public: { return m_userConstraintId; } + float GetAppliedImpulse() + { + return m_appliedImpulse; + } }; #endif //TYPED_CONSTRAINT_H diff --git a/BulletDynamics/Jamfile b/BulletDynamics/Jamfile index b7df2012a..493bebfcf 100644 --- a/BulletDynamics/Jamfile +++ b/BulletDynamics/Jamfile @@ -1,7 +1,8 @@ -SubDir TOP BulletDynamics ; - -Library bulletdynamics : - [ Wildcard ConstraintSolver : *.h *.cpp ] - [ Wildcard Dynamics : *.h *.cpp ] -; -Recurse InstallHeader : .h ; +SubDir TOP BulletDynamics ; + +Library bulletdynamics : + [ Wildcard ConstraintSolver : *.h *.cpp ] + [ Wildcard Dynamics : *.h *.cpp ] + [ Wildcard Vehicle : *.h *.cpp ] +; +Recurse InstallHeader : .h ; diff --git a/BulletDynamics/Vehicle/RaycastVehicle.cpp b/BulletDynamics/Vehicle/RaycastVehicle.cpp new file mode 100644 index 000000000..c2a6f4d5d --- /dev/null +++ b/BulletDynamics/Vehicle/RaycastVehicle.cpp @@ -0,0 +1,605 @@ +/* + * 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 "RaycastVehicle.h" +#include "ConstraintSolver/Solve2LinearConstraint.h" +#include "ConstraintSolver/JacobianEntry.h" +#include "SimdQuaternion.h" +#include "SimdVector3.h" +#include "VehicleRaycaster.h" +#include "WheelInfo.h" + + +#include "Dynamics/MassProps.h" +#include "ConstraintSolver/ContactConstraint.h" + + + +static RigidBody s_fixedObject( MassProps ( 0.0f, SimdVector3(0,0,0) ),0.f,0.f,0.f,0.f); + +RaycastVehicle::RaycastVehicle(const VehicleTuning& tuning,RigidBody* chassis, VehicleRaycaster* raycaster ) +:m_vehicleRaycaster(raycaster), +m_pitchControl(0.f) +{ + m_chassisBody = chassis; + m_indexRightAxis = 0; + m_indexUpAxis = 2; + m_indexForwardAxis = 1; + DefaultInit(tuning); +} + + +void RaycastVehicle::DefaultInit(const VehicleTuning& tuning) +{ + m_currentVehicleSpeedKmHour = 0.f; + m_steeringValue = 0.f; + +} + + + +RaycastVehicle::~RaycastVehicle() +{ +} + + +// +// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed +// +WheelInfo& RaycastVehicle::AddWheel( const SimdVector3& connectionPointCS, const SimdVector3& wheelDirectionCS0,const SimdVector3& wheelAxleCS, SimdScalar suspensionRestLength, SimdScalar wheelRadius,const VehicleTuning& tuning, bool isFrontWheel) +{ + + WheelInfoConstructionInfo ci; + + ci.m_chassisConnectionCS = connectionPointCS; + ci.m_wheelDirectionCS = wheelDirectionCS0; + ci.m_wheelAxleCS = wheelAxleCS; + ci.m_suspensionRestLength = suspensionRestLength; + ci.m_wheelRadius = wheelRadius; + ci.m_suspensionStiffness = tuning.m_suspensionStiffness; + ci.m_wheelsDampingCompression = tuning.m_suspensionCompression; + ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping; + ci.m_frictionSlip = tuning.m_frictionSlip; + ci.m_bIsFrontWheel = isFrontWheel; + ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm; + + m_wheelInfo.push_back( WheelInfo(ci)); + + WheelInfo& wheel = m_wheelInfo[GetNumWheels()-1]; + + UpdateWheelTransformsWS( wheel ); + return wheel; +} + + + + +const SimdTransform& RaycastVehicle::GetWheelTransformWS( int wheelIndex ) const +{ + assert(wheelIndex < GetNumWheels()); + const WheelInfo& wheel = m_wheelInfo[wheelIndex]; + return wheel.m_worldTransform; + +} + +void RaycastVehicle::UpdateWheelTransform( int wheelIndex ) +{ + + WheelInfo& wheel = m_wheelInfo[ wheelIndex ]; + UpdateWheelTransformsWS(wheel); + SimdVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; + const SimdVector3& right = wheel.m_raycastInfo.m_wheelAxleWS; + SimdVector3 fwd = up.cross(right); + fwd = fwd.normalize(); + //rotate around steering over de wheelAxleWS + float steering = wheel.m_steering; + + SimdQuaternion steeringOrn(up,steering);//wheel.m_steering); + SimdMatrix3x3 steeringMat(steeringOrn); + + SimdQuaternion rotatingOrn(right,wheel.m_rotation); + SimdMatrix3x3 rotatingMat(rotatingOrn); + + SimdMatrix3x3 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 RaycastVehicle::ResetSuspension() +{ + + std::vector::iterator wheelIt; + for (wheelIt = m_wheelInfo.begin(); + !(wheelIt == m_wheelInfo.end());wheelIt++) + { + WheelInfo& wheel = *wheelIt; + wheel.m_raycastInfo.m_suspensionLength = wheel.GetSuspensionRestLength(); + wheel.m_suspensionRelativeVelocity = 0.0f; + + wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; + //wheel_info.setContactFriction(0.0f); + wheel.m_clippedInvContactDotSuspension = 1.0f; + } +} + +void RaycastVehicle::UpdateWheelTransformsWS(WheelInfo& wheel ) +{ + wheel.m_raycastInfo.m_isInContact = false; + + const SimdTransform& chassisTrans = GetRigidBody()->getCenterOfMassTransform(); + + 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; +} + +SimdScalar RaycastVehicle::Raycast(WheelInfo& wheel) +{ + UpdateWheelTransformsWS( wheel ); + + + SimdScalar depth = -1; + + SimdScalar raylen = wheel.GetSuspensionRestLength()+wheel.m_wheelsRadius; + + SimdVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); + const SimdVector3& source = wheel.m_raycastInfo.m_hardPointWS; + wheel.m_raycastInfo.m_contactPointWS = source + rayvector; + const SimdVector3& target = wheel.m_raycastInfo.m_contactPointWS; + + SimdScalar param = 0.f; + + VehicleRaycaster::VehicleRaycasterResult rayResults; + + void* object = m_vehicleRaycaster->CastRay(source,target,rayResults); + + wheel.m_raycastInfo.m_groundObject = 0; + + if (object) + { + param = rayResults.m_distFraction; + depth = raylen * rayResults.m_distFraction; + wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; + wheel.m_raycastInfo.m_isInContact = true; + + wheel.m_raycastInfo.m_groundObject = &s_fixedObject;//todo for driving on dynamic/movable objects!; + //wheel.m_raycastInfo.m_groundObject = object; + + + SimdScalar hitDistance = param*raylen; + wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; + //clamp on max suspension travel + + float minSuspensionLength = wheel.GetSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*0.01f; + float maxSuspensionLength = wheel.GetSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*0.01f; + 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; + + SimdScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); + + SimdVector3 chassis_velocity_at_contactPoint; + SimdVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-GetRigidBody()->getCenterOfMassPosition(); + + chassis_velocity_at_contactPoint = GetRigidBody()->getVelocityInLocalPoint(relpos); + + SimdScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); + + if ( denominator >= -0.1f) + { + wheel.m_suspensionRelativeVelocity = 0.0f; + wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f; + } + else + { + SimdScalar inv = -1.f / 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 = 0.0f; + wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; + wheel.m_clippedInvContactDotSuspension = 1.0f; + } + + return depth; +} + + +void RaycastVehicle::UpdateVehicle( SimdScalar step ) +{ + + m_currentVehicleSpeedKmHour = 3.6f * GetRigidBody()->getLinearVelocity().length(); + + const SimdTransform& chassisTrans = GetRigidBody()->getCenterOfMassTransform(); + SimdVector3 forwardW ( + chassisTrans.getBasis()[0][m_indexForwardAxis], + chassisTrans.getBasis()[1][m_indexForwardAxis], + chassisTrans.getBasis()[2][m_indexForwardAxis]); + + if (forwardW.dot(GetRigidBody()->getLinearVelocity()) < 0.f) + { + m_currentVehicleSpeedKmHour *= -1.f; + } + + // + // simulate suspension + // + std::vector::iterator wheelIt; + int i=0; + for (wheelIt = m_wheelInfo.begin(); + !(wheelIt == m_wheelInfo.end());wheelIt++,i++) + { + WheelInfo& wheelInfo = *wheelIt; + + SimdScalar depth; + depth = Raycast( *wheelIt ); + } + + UpdateSuspension(step); + + + for (wheelIt = m_wheelInfo.begin(); + !(wheelIt == m_wheelInfo.end());wheelIt++) + { + //apply suspension force + WheelInfo& wheel = *wheelIt; + + float suspensionForce = wheel.m_wheelsSuspensionForce; + + float gMaxSuspensionForce = 6000.f; + if (suspensionForce > gMaxSuspensionForce) + { + suspensionForce = gMaxSuspensionForce; + } + SimdVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; + SimdVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody()->getCenterOfMassPosition(); + + GetRigidBody()->applyImpulse(impulse, relpos); + + } + + + + UpdateFriction( step); + + + for (wheelIt = m_wheelInfo.begin(); + !(wheelIt == m_wheelInfo.end());wheelIt++) + { + WheelInfo& wheel = *wheelIt; + SimdVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - GetRigidBody()->getCenterOfMassPosition(); + SimdVector3 vel = GetRigidBody()->getVelocityInLocalPoint( relpos ); + + if (wheel.m_raycastInfo.m_isInContact) + { + SimdVector3 fwd ( + GetRigidBody()->getCenterOfMassTransform().getBasis()[0][m_indexForwardAxis], + GetRigidBody()->getCenterOfMassTransform().getBasis()[1][m_indexForwardAxis], + GetRigidBody()->getCenterOfMassTransform().getBasis()[2][m_indexForwardAxis]); + + SimdScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); + fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; + + SimdScalar 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 *= 0.99f;//damping of rotation when not in contact + + } + + + +} + + +void RaycastVehicle::SetSteeringValue(SimdScalar steering,int wheel) +{ + assert(wheel>=0 && wheel < GetNumWheels()); + + WheelInfo& wheelInfo = GetWheelInfo(wheel); + wheelInfo.m_steering = steering; +} + + + +SimdScalar RaycastVehicle::GetSteeringValue(int wheel) const +{ + return GetWheelInfo(wheel).m_steering; +} + + +void RaycastVehicle::ApplyEngineForce(SimdScalar force, int wheel) +{ + for (int i=0;i= 0) && (index < GetNumWheels())); + + return m_wheelInfo[index]; +} + +WheelInfo& RaycastVehicle::GetWheelInfo(int index) +{ + ASSERT((index >= 0) && (index < GetNumWheels())); + + return m_wheelInfo[index]; +} + +void RaycastVehicle::SetBrake(float brake,int wheelIndex) +{ + ASSERT((wheelIndex >= 0) && (wheelIndex < GetNumWheels())); + GetWheelInfo(wheelIndex).m_brake; +} + + +void RaycastVehicle::UpdateSuspension(SimdScalar deltaTime) +{ + + SimdScalar chassisMass = 1.f / m_chassisBody->getInvMass(); + + for (int w_it=0; w_it maximpSquared) + { + sliding = true; + + SimdScalar factor = maximp / SimdSqrt(impulseSquared); + + m_wheelInfo[wheel].m_skidInfo *= factor; + } + } + + } + } + + + + + if (sliding) + { + for (int wheel = 0;wheel < GetNumWheels(); wheel++) + { + if (sideImpulse[wheel] != 0.f) + { + if (m_wheelInfo[wheel].m_skidInfo< 1.f) + { + forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; + sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; + } + } + } + } + + // apply the impulses + { + for (int wheel = 0;wheelgetCenterOfMassPosition(); + + if (forwardImpulse[wheel] != 0.f) + { + m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos); + } + if (sideImpulse[wheel] != 0.f) + { + class RigidBody* groundObject = (class RigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; + + SimdVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - + groundObject->getCenterOfMassPosition(); + + + SimdVector3 sideImp = axle[wheel] * sideImpulse[wheel]; + + rel_pos[2] *= wheelInfo.m_rollInfluence; + m_chassisBody->applyImpulse(sideImp,rel_pos); + + //apply friction impulse on the ground + groundObject->applyImpulse(-sideImp,rel_pos2); + } + } + } + + delete []forwardWS; + delete [] axle; + delete[]forwardImpulse; + delete[] sideImpulse; +} diff --git a/BulletDynamics/Vehicle/RaycastVehicle.h b/BulletDynamics/Vehicle/RaycastVehicle.h new file mode 100644 index 000000000..13993b87d --- /dev/null +++ b/BulletDynamics/Vehicle/RaycastVehicle.h @@ -0,0 +1,160 @@ +/* + * 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 "Dynamics/RigidBody.h" +#include "ConstraintSolver/TypedConstraint.h" + +struct MassProps; +#include "WheelInfo.h" + +struct VehicleRaycaster; +class VehicleTuning; + +///Raycast vehicle, very special constraint that turn a rigidbody into a vehicle. +class RaycastVehicle : public TypedConstraint +{ +public: + class VehicleTuning + { + public: + + VehicleTuning() + :m_suspensionStiffness(5.88f), + m_suspensionCompression(0.83f), + m_suspensionDamping(0.88f), + m_maxSuspensionTravelCm(500.f), + m_frictionSlip(10.5f) + { + } + float m_suspensionStiffness; + float m_suspensionCompression; + float m_suspensionDamping; + float m_maxSuspensionTravelCm; + float m_frictionSlip; + + }; +private: + + SimdScalar m_tau; + SimdScalar m_damping; + VehicleRaycaster* m_vehicleRaycaster; + float m_pitchControl; + float m_steeringValue; + float m_currentVehicleSpeedKmHour; + + RigidBody* m_chassisBody; + + int m_indexRightAxis; + int m_indexUpAxis; + int m_indexForwardAxis; + + void DefaultInit(const VehicleTuning& tuning); + +public: + + //constructor to create a car from an existing rigidbody + RaycastVehicle(const VehicleTuning& tuning,RigidBody* chassis, VehicleRaycaster* raycaster ); + + virtual ~RaycastVehicle() ; + + + + + + SimdScalar Raycast(WheelInfo& wheel); + + virtual void UpdateVehicle(SimdScalar step); + + void ResetSuspension(); + + SimdScalar GetSteeringValue(int wheel) const; + + void SetSteeringValue(SimdScalar steering,int wheel); + + + void ApplyEngineForce(SimdScalar force, int wheel); + + const SimdTransform& GetWheelTransformWS( int wheelIndex ) const; + + void UpdateWheelTransform( int wheelIndex ); + + void SetRaycastWheelInfo( int wheelIndex , bool isInContact, const SimdVector3& hitPoint, const SimdVector3& hitNormal,SimdScalar depth); + + WheelInfo& AddWheel( const SimdVector3& connectionPointCS0, const SimdVector3& wheelDirectionCS0,const SimdVector3& wheelAxleCS,SimdScalar suspensionRestLength,SimdScalar wheelRadius,const VehicleTuning& tuning, bool isFrontWheel); + + inline int GetNumWheels() const { + return m_wheelInfo.size(); + } + + std::vector m_wheelInfo; + + + const WheelInfo& GetWheelInfo(int index) const; + + WheelInfo& GetWheelInfo(int index); + + void UpdateWheelTransformsWS(WheelInfo& wheel ); + + + void SetBrake(float brake,int wheelIndex); + + void SetPitchControl(float pitch) + { + m_pitchControl = pitch; + } + + void UpdateSuspension(SimdScalar deltaTime); + + void UpdateFriction(SimdScalar timeStep); + + + + inline RigidBody* GetRigidBody() + { + return m_chassisBody; + } + + const RigidBody* 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; + } + + virtual void BuildJacobian() + { + //not yet + } + + virtual void SolveConstraint(SimdScalar timeStep) + { + //not yet + } + + +}; + +#endif //RAYCASTVEHICLE_H + diff --git a/BulletDynamics/Vehicle/VehicleRaycaster.h b/BulletDynamics/Vehicle/VehicleRaycaster.h new file mode 100644 index 000000000..f3133c760 --- /dev/null +++ b/BulletDynamics/Vehicle/VehicleRaycaster.h @@ -0,0 +1,32 @@ +/* + * 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 "SimdVector3.h" + +/// VehicleRaycaster is provides interface for between vehicle simulation and raycasting +struct VehicleRaycaster +{ + struct VehicleRaycasterResult + { + VehicleRaycasterResult() :m_distFraction(-1.f){}; + SimdVector3 m_hitPointInWorld; + SimdVector3 m_hitNormalInWorld; + SimdScalar m_distFraction; + }; + + virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result) = 0; + +}; + +#endif //VEHICLE_RAYCASTER_H + diff --git a/BulletDynamics/Vehicle/WheelInfo.cpp b/BulletDynamics/Vehicle/WheelInfo.cpp new file mode 100644 index 000000000..fcc82ea97 --- /dev/null +++ b/BulletDynamics/Vehicle/WheelInfo.cpp @@ -0,0 +1,55 @@ +/* + * 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 "WheelInfo.h" +#include "Dynamics/RigidBody.h" // for pointvelocity + + +SimdScalar WheelInfo::GetSuspensionRestLength() const +{ + + return m_suspensionRestLength1; + +} + +void WheelInfo::UpdateWheel(const RigidBody& chassis,RaycastInfo& raycastInfo) +{ + + + if (m_raycastInfo.m_isInContact) + + { + SimdScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); + SimdVector3 chassis_velocity_at_contactPoint; + SimdVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); + chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); + SimdScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); + if ( project >= -0.1f) + { + m_suspensionRelativeVelocity = 0.0f; + m_clippedInvContactDotSuspension = 1.0f / 0.1f; + } + else + { + SimdScalar inv = -1.f / 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 = 0.0f; + m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; + m_clippedInvContactDotSuspension = 1.0f; + } +} diff --git a/BulletDynamics/Vehicle/WheelInfo.h b/BulletDynamics/Vehicle/WheelInfo.h new file mode 100644 index 000000000..1a47a82b2 --- /dev/null +++ b/BulletDynamics/Vehicle/WheelInfo.h @@ -0,0 +1,116 @@ +/* + * 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 "SimdVector3.h" +#include "SimdTransform.h" + +class RigidBody; + +struct WheelInfoConstructionInfo +{ + SimdVector3 m_chassisConnectionCS; + SimdVector3 m_wheelDirectionCS; + SimdVector3 m_wheelAxleCS; + SimdScalar m_suspensionRestLength; + SimdScalar m_maxSuspensionTravelCm; + SimdScalar m_wheelRadius; + + float m_suspensionStiffness; + float m_wheelsDampingCompression; + float m_wheelsDampingRelaxation; + float m_frictionSlip; + bool m_bIsFrontWheel; + +}; + +/// WheelInfo contains information per wheel about friction and suspension. +struct WheelInfo +{ + struct RaycastInfo + { + //set by raycaster + SimdVector3 m_contactNormalWS;//contactnormal + SimdVector3 m_contactPointWS;//raycast hitpoint + SimdScalar m_suspensionLength; + SimdVector3 m_hardPointWS;//raycast starting point + SimdVector3 m_wheelDirectionWS; //direction in worldspace + SimdVector3 m_wheelAxleWS; // axle in worldspace + bool m_isInContact; + void* m_groundObject; //could be general void* ptr + }; + + RaycastInfo m_raycastInfo; + + SimdTransform m_worldTransform; + + SimdVector3 m_chassisConnectionPointCS; //const + SimdVector3 m_wheelDirectionCS;//const + SimdVector3 m_wheelAxleCS; // const or modified by steering + SimdScalar m_suspensionRestLength1;//const + SimdScalar m_maxSuspensionTravelCm; + SimdScalar GetSuspensionRestLength() const; + SimdScalar m_wheelsRadius;//const + SimdScalar m_suspensionStiffness;//const + SimdScalar m_wheelsDampingCompression;//const + SimdScalar m_wheelsDampingRelaxation;//const + SimdScalar m_frictionSlip; + SimdScalar m_steering; + SimdScalar m_rotation; + SimdScalar m_deltaRotation; + SimdScalar m_rollInfluence; + + SimdScalar m_engineForce; + + SimdScalar m_brake; + + bool m_bIsFrontWheel; + + void* m_clientInfo;//can be used to store pointer to sync transforms... + + WheelInfo(WheelInfoConstructionInfo& 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 = 0.f; + m_engineForce = 0.f; + m_rotation = 0.f; + m_deltaRotation = 0.f; + m_brake = 0.f; + m_rollInfluence = 0.1f; + m_bIsFrontWheel = ci.m_bIsFrontWheel; + + } + + void UpdateWheel(const RigidBody& chassis,RaycastInfo& raycastInfo); + + SimdScalar m_clippedInvContactDotSuspension; + SimdScalar m_suspensionRelativeVelocity; + //calculated by suspension + SimdScalar m_wheelsSuspensionForce; + SimdScalar m_skidInfo; + +}; + +#endif //WHEEL_INFO_H + diff --git a/Demos/CollisionInterfaceDemo/Jamfile b/Demos/CollisionInterfaceDemo/Jamfile index f2704f6c1..dab705b7c 100644 --- a/Demos/CollisionInterfaceDemo/Jamfile +++ b/Demos/CollisionInterfaceDemo/Jamfile @@ -1,3 +1,3 @@ -SubDir TOP Demos CollisionInterfaceDemo ; - -BulletDemo CollisionInterfaceDemo : [ Wildcard *.h *.cpp ] ; +SubDir TOP Demos CollisionInterfaceDemo ; + +BulletDemo CollisionInterfaceDemo : [ Wildcard *.h *.cpp ] ; diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index 44facc69f..936259517 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -335,8 +335,8 @@ m_enableSatCollisionDetection(false) { m_triggerCallbacks[i] = 0; } - //if (!dispatcher) - // dispatcher = new CollisionDispatcher(); + if (!dispatcher) + dispatcher = new CollisionDispatcher(); if(!pairCache) @@ -1834,3 +1834,18 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float conera return sphereController; } +float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid) +{ + std::vector::iterator i; + + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) + { + TypedConstraint* constraint = (*i); + if (constraint->GetUserConstraintId() == constraintid) + { + return constraint->GetAppliedImpulse(); + } + } + return 0.f; +} diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index 3078f51f8..053abcbd3 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -32,7 +32,7 @@ class Dispatcher; //#include "BroadphaseInterface.h" //switch on/off new vehicle support -//#define NEW_BULLET_VEHICLE_SUPPORT 1 +#define NEW_BULLET_VEHICLE_SUPPORT 1 #include "ConstraintSolver/ContactSolverInfo.h" @@ -41,6 +41,7 @@ class PersistentManifold; class BroadphaseInterface; class OverlappingPairCache; class IDebugDraw; +class PHY_IVehicle; /// CcdPhysicsEnvironment is experimental mainloop for physics simulation using optional continuous collision detection. /// Physics Environment takes care of stepping the simulation and is a container for physics entities. @@ -137,6 +138,8 @@ protected: virtual void removeConstraint(int constraintid); + virtual float getAppliedImpulse(int constraintid); + virtual void CallbackTriggers(); diff --git a/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h b/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h index 3b00dad39..cbf909403 100644 --- a/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h +++ b/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h @@ -39,6 +39,8 @@ class PHY_IPhysicsEnvironment float axisX,float axisY,float axisZ)=0; virtual void removeConstraint(int constraintid)=0; + virtual float getAppliedImpulse(int constraintid){ return 0.f;} + virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)=0;