First stage in refactoring Bullet: moved Bullet Collision and Dynamics and LinearMath into src folder, and all files in Collision Detection and Dynamics have bt prefix.
Made all buildsystems to work again (jam, msvc, cmake)
This commit is contained in:
ejcoumans
596
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
Normal file
596
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
Normal file
@@ -0,0 +1,596 @@
|
||||
/*
|
||||
* 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 "btRaycastVehicle.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
|
||||
#include "LinearMath/SimdQuaternion.h"
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "btVehicleRaycaster.h"
|
||||
#include "btWheelInfo.h"
|
||||
|
||||
|
||||
#include "BulletDynamics/Dynamics/btMassProps.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btContactConstraint.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 );
|
||||
UpdateWheelTransform(GetNumWheels()-1);
|
||||
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<WheelInfo>::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<WheelInfo>::iterator wheelIt;
|
||||
int i=0;
|
||||
for (wheelIt = m_wheelInfo.begin();
|
||||
!(wheelIt == m_wheelInfo.end());wheelIt++,i++)
|
||||
{
|
||||
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)
|
||||
{
|
||||
assert(wheel>=0 && wheel < GetNumWheels());
|
||||
WheelInfo& wheelInfo = GetWheelInfo(wheel);
|
||||
wheelInfo.m_engineForce = force;
|
||||
}
|
||||
|
||||
|
||||
const WheelInfo& RaycastVehicle::GetWheelInfo(int index) const
|
||||
{
|
||||
ASSERT((index >= 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<GetNumWheels(); w_it++)
|
||||
{
|
||||
WheelInfo &wheel_info = m_wheelInfo[w_it];
|
||||
|
||||
if ( wheel_info.m_raycastInfo.m_isInContact )
|
||||
{
|
||||
SimdScalar force;
|
||||
// Spring
|
||||
{
|
||||
SimdScalar susp_length = wheel_info.GetSuspensionRestLength();
|
||||
SimdScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
|
||||
|
||||
SimdScalar length_diff = (susp_length - current_length);
|
||||
|
||||
force = wheel_info.m_suspensionStiffness
|
||||
* length_diff * wheel_info.m_clippedInvContactDotSuspension;
|
||||
}
|
||||
|
||||
// Damper
|
||||
{
|
||||
SimdScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
|
||||
{
|
||||
SimdScalar susp_damping;
|
||||
if ( projected_rel_vel < 0.0f )
|
||||
{
|
||||
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 < 0.f)
|
||||
{
|
||||
wheel_info.m_wheelsSuspensionForce = 0.f;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
wheel_info.m_wheelsSuspensionForce = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float sideFrictionStiffness2 = 1.0f;
|
||||
void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
|
||||
{
|
||||
|
||||
//calculate the impulse, so that the wheels don't move sidewards
|
||||
int numWheel = GetNumWheels();
|
||||
if (!numWheel)
|
||||
return;
|
||||
|
||||
|
||||
SimdVector3* forwardWS = new SimdVector3[numWheel];
|
||||
SimdVector3* axle = new SimdVector3[numWheel];
|
||||
SimdScalar* forwardImpulse = new SimdScalar[numWheel];
|
||||
SimdScalar* sideImpulse = new SimdScalar[numWheel];
|
||||
|
||||
int numWheelsOnGround = 0;
|
||||
|
||||
|
||||
//collapse all those loops into one!
|
||||
for (int i=0;i<GetNumWheels();i++)
|
||||
{
|
||||
WheelInfo& wheelInfo = m_wheelInfo[i];
|
||||
class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
|
||||
if (groundObject)
|
||||
numWheelsOnGround++;
|
||||
sideImpulse[i] = 0.f;
|
||||
forwardImpulse[i] = 0.f;
|
||||
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
for (int i=0;i<GetNumWheels();i++)
|
||||
{
|
||||
|
||||
WheelInfo& wheelInfo = m_wheelInfo[i];
|
||||
|
||||
class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
|
||||
|
||||
if (groundObject)
|
||||
{
|
||||
|
||||
const SimdTransform& wheelTrans = GetWheelTransformWS( i );
|
||||
|
||||
SimdMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
|
||||
axle[i] = SimdVector3(
|
||||
wheelBasis0[0][m_indexRightAxis],
|
||||
wheelBasis0[1][m_indexRightAxis],
|
||||
wheelBasis0[2][m_indexRightAxis]);
|
||||
|
||||
const SimdVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
|
||||
SimdScalar proj = axle[i].dot(surfNormalWS);
|
||||
axle[i] -= surfNormalWS * proj;
|
||||
axle[i] = axle[i].normalize();
|
||||
|
||||
forwardWS[i] = surfNormalWS.cross(axle[i]);
|
||||
forwardWS[i].normalize();
|
||||
|
||||
|
||||
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
|
||||
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
|
||||
0.f, axle[i],sideImpulse[i],timeStep);
|
||||
|
||||
sideImpulse[i] *= sideFrictionStiffness2;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
SimdScalar sideFactor = 1.f;
|
||||
SimdScalar fwdFactor = 0.5;
|
||||
|
||||
bool sliding = false;
|
||||
{
|
||||
for (int wheel =0;wheel <GetNumWheels();wheel++)
|
||||
{
|
||||
WheelInfo& wheelInfo = m_wheelInfo[wheel];
|
||||
class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
|
||||
|
||||
|
||||
forwardImpulse[wheel] = 0.f;
|
||||
m_wheelInfo[wheel].m_skidInfo= 1.f;
|
||||
|
||||
if (groundObject)
|
||||
{
|
||||
m_wheelInfo[wheel].m_skidInfo= 1.f;
|
||||
|
||||
SimdScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
|
||||
SimdScalar maximpSide = maximp;
|
||||
|
||||
SimdScalar maximpSquared = maximp * maximpSide;
|
||||
|
||||
forwardImpulse[wheel] = wheelInfo.m_engineForce* timeStep;
|
||||
|
||||
float x = (forwardImpulse[wheel] ) * fwdFactor;
|
||||
float y = (sideImpulse[wheel] ) * sideFactor;
|
||||
|
||||
float impulseSquared = (x*x + y*y);
|
||||
|
||||
if (impulseSquared > 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;wheel<GetNumWheels() ; wheel++)
|
||||
{
|
||||
WheelInfo& wheelInfo = m_wheelInfo[wheel];
|
||||
|
||||
SimdVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
|
||||
m_chassisBody->getCenterOfMassPosition();
|
||||
|
||||
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;
|
||||
}
|
||||
167
src/BulletDynamics/Vehicle/btRaycastVehicle.h
Normal file
167
src/BulletDynamics/Vehicle/btRaycastVehicle.h
Normal file
@@ -0,0 +1,167 @@
|
||||
/*
|
||||
* 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"
|
||||
|
||||
struct MassProps;
|
||||
#include "btWheelInfo.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<WheelInfo> 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 SetCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
|
||||
{
|
||||
m_indexRightAxis = rightIndex;
|
||||
m_indexUpAxis = upIndex;
|
||||
m_indexForwardAxis = forwardIndex;
|
||||
}
|
||||
|
||||
virtual void BuildJacobian()
|
||||
{
|
||||
//not yet
|
||||
}
|
||||
|
||||
virtual void SolveConstraint(SimdScalar timeStep)
|
||||
{
|
||||
//not yet
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //RAYCASTVEHICLE_H
|
||||
|
||||
35
src/BulletDynamics/Vehicle/btVehicleRaycaster.h
Normal file
35
src/BulletDynamics/Vehicle/btVehicleRaycaster.h
Normal file
@@ -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/SimdVector3.h"
|
||||
|
||||
/// VehicleRaycaster is provides interface for between vehicle simulation and raycasting
|
||||
struct VehicleRaycaster
|
||||
{
|
||||
virtual ~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
|
||||
|
||||
55
src/BulletDynamics/Vehicle/btWheelInfo.cpp
Normal file
55
src/BulletDynamics/Vehicle/btWheelInfo.cpp
Normal file
@@ -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 "btWheelInfo.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.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;
|
||||
}
|
||||
}
|
||||
116
src/BulletDynamics/Vehicle/btWheelInfo.h
Normal file
116
src/BulletDynamics/Vehicle/btWheelInfo.h
Normal file
@@ -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 "LinearMath/SimdVector3.h"
|
||||
#include "LinearMath/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
|
||||
|
||||
Reference in New Issue
Block a user