Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added basic RaycastVehicle support, and CcdPhysicsEnvironment::getAppliedImpulse(int constraintId), this value is useful as treshold to break constraints.

Browse Source
This commit is contained in:
ejcoumans
2006-08-29 23:55:32 +00:00
parent e1b85d1969
commit 334ce42650
15 changed files with 1033 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
BulletDynamics/CMakeLists.txt
View File

@@ -14,4 +14,6 @@ ADD_LIBRARY(LibBulletDynamics
Dynamics/BU_Joint.cpp
Dynamics/ContactJoint.cpp
Dynamics/RigidBody.cpp
Vehicle/RaycastVehicle.cpp
Vehicle/WheelInfo.cpp
)

6
BulletDynamics/ConstraintSolver/HingeConstraint.cpp
View File

@@ -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());

4
BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp
View File

@@ -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());

9
BulletDynamics/ConstraintSolver/TypedConstraint.cpp
View File

@@ -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));

16
BulletDynamics/ConstraintSolver/TypedConstraint.h
View File

@@ -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

15
BulletDynamics/Jamfile
View File

@@ -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 ;

605
BulletDynamics/Vehicle/RaycastVehicle.cpp Normal file
View File

@@ -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<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++)
{
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<GetNumWheels();i++)
{
WheelInfo& wheelInfo = GetWheelInfo(i);
bool applyOnFrontWheel = !wheel;
if (applyOnFrontWheel == wheelInfo.m_bIsFrontWheel)
{
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;
}

160
BulletDynamics/Vehicle/RaycastVehicle.h Normal file
View File

@@ -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<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 BuildJacobian()
{
//not yet
}
virtual void SolveConstraint(SimdScalar timeStep)
{
//not yet
}
};
#endif //RAYCASTVEHICLE_H

32
BulletDynamics/Vehicle/VehicleRaycaster.h Normal file
View File

@@ -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

55
BulletDynamics/Vehicle/WheelInfo.cpp Normal file
View 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 "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;
}
}

116
BulletDynamics/Vehicle/WheelInfo.h Normal file
View 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 "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

6
Demos/CollisionInterfaceDemo/Jamfile
View File

@@ -1,3 +1,3 @@
SubDir TOP Demos CollisionInterfaceDemo ;
BulletDemo CollisionInterfaceDemo : [ Wildcard *.h *.cpp ] ;
SubDir TOP Demos CollisionInterfaceDemo ;
BulletDemo CollisionInterfaceDemo : [ Wildcard *.h *.cpp ] ;

19
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
View File

@@ -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<TypedConstraint*>::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;
}

5
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
View File

@@ -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();

2
Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h
View File

@@ -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;