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;
|
||||
}
|
||||
Reference in New Issue
Block a user