1275 lines
32 KiB
C++
1275 lines
32 KiB
C++
#include <StdAfx.h>
|
|
#include "vtPhysXAll.h"
|
|
|
|
#include <xDebugTools.h>
|
|
|
|
#include "pDifferential.h"
|
|
|
|
#include "pVehicleAll.h"
|
|
|
|
//+ velWheelCC {x=66.735077 y=0.097698294 z=-215.35669 ...} VxVector
|
|
//+ velWheelTC {x=66.735077 y=0.097698294 z=-215.35669 ...} VxVector
|
|
|
|
|
|
enum StateFlags
|
|
{
|
|
ON_SURFACE=1, // Wheel is touching surface?
|
|
ATTACHED=2, // Attached to suspension?
|
|
LOW_SPEED=4 // Wheel is turning slow
|
|
};
|
|
|
|
|
|
#define FRICTION_COEFF 100.0f
|
|
#define MASS 12
|
|
#define TIRE_RATE 50000.0f
|
|
#define WHEEL_PENETRATION_DEPTH -0.0258f
|
|
#define WHEEL_SUSPENSION_FORCE VxVector(0,-1025.0f,0)
|
|
#define RR_EPSILON_VELOCITY 0.001 // Wheel velocity
|
|
#define OPT_SLIPVECTOR_USES_TANSA
|
|
|
|
|
|
|
|
// Use full 3D patch for surface detection?
|
|
#define USE_3D_PATCH
|
|
|
|
#define ENV_INI "env.ini"
|
|
|
|
// For method #3, slipVector (Gregor Veble), use tan(SA) or SA?
|
|
#define OPT_SLIPVECTOR_USES_TANSA
|
|
|
|
// Skid methods
|
|
//#define SKID_METHOD_SEPARATE_LON_LAT
|
|
//#define SKID_METHOD_SLIP_RA_VECTOR
|
|
#define SKID_METHOD_Z_GREGOR
|
|
|
|
// Point at which skidmarks appear
|
|
#define SKIDMARK_SLIPRATIO 0.2
|
|
|
|
// Apply damping to slipRatio/slipAngle differential eq's at low speed?
|
|
//#define DO_DAMPING_LAT
|
|
//#define DO_DAMPING_LONG
|
|
|
|
|
|
// Apply low-speed enhancements? (obsolete since 18-5-01)
|
|
#define DO_LOW_SPEED
|
|
|
|
// Distance (in m) to start with wheel ray-track intersection; this is
|
|
// the height at which the ray is started to avoid getting a ray
|
|
// that starts BENEATH the track surface and therefore not hitting it.
|
|
#define DIST_INTERSECT 1.0
|
|
|
|
// Define the next symbol to check for wheel velocity reversal (vertically),
|
|
// and if so, the wheel is stopped momentarily. Is used to rule out
|
|
// incredibly high damper forces from pushing the wheel to full up or down.
|
|
// Should perhaps not be needed anymore combined with the implicit integrator.
|
|
// Note that this acts at the outermost positions of the wheel.
|
|
//#define DAMP_VERTICAL_VELOCITY_REVERSAL
|
|
|
|
// Damp the wheel when crossing the equilibrium position?
|
|
// As the wheel passes its center position, the spring force reverses. To
|
|
// avoid adding energy into the system, when passing this position, damping
|
|
// is used to avoid overaccelerating the tire to the other side.
|
|
// Note that this acts when the wheel is near its center (rest) position,
|
|
// contrast this with DAMP_VERTICAL_VELOCITY_REVERSAL.
|
|
#define DAMP_VERTICAL_EQUILIBRIUM_REVERSAL
|
|
|
|
// Use implicit integration? This should be more stable with
|
|
// high damper rates.
|
|
#define INTEGRATE_IMPLICIT_VERTICAL
|
|
|
|
// Gregor Veble combined slip algorithm? (instead of Pacejka)
|
|
//#define DO_GREGOR
|
|
#ifdef DO_GREGOR
|
|
#undef DO_DAMPING_LAT
|
|
#undef DO_DAMPING_LONG
|
|
#endif
|
|
|
|
// Delayed slip angle?
|
|
//#define USE_SAE950311_LAT
|
|
|
|
// If not using SAE950311, damp SA at low speed? (to avoid jittering)
|
|
#define USE_SA_DAMPING_AT_LOW_SPEED
|
|
|
|
// Wheel locking results in force going the direction of -slipVector?
|
|
#define USE_WHEEL_LOCK_ADJUST
|
|
|
|
#define USE_NXWHEEL_CONTACT_DATA
|
|
|
|
#define USE_NXWHEEL_NORMALFORCE_LOAD
|
|
|
|
|
|
|
|
|
|
static int getFrictionMethod(){ return FC_SLIPVECTOR; }
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void pWheel2::CalcPacejka()
|
|
{
|
|
|
|
float normalForce;
|
|
|
|
|
|
if (hadContact)
|
|
{
|
|
normalForce=forceRoadTC.y;
|
|
}else{
|
|
normalForce = 0.0f;
|
|
}
|
|
|
|
/*
|
|
if (hadContact)
|
|
{
|
|
normalForce=500.0f;
|
|
}else{
|
|
normalForce = 0.0f;
|
|
}
|
|
*/
|
|
|
|
|
|
pacejka.SetCamber(0);
|
|
|
|
bool FC_SLIPVECTOR =true;
|
|
// Note our positive slipAngle is the reverse of SAE's definition
|
|
if(FC_SLIPVECTOR)
|
|
{
|
|
// Gregor Veble's and also Brian Beckman's idea of mixing Pacejka
|
|
// Note that Gregor's Veble 'z' is 'csSlipLen' here.
|
|
// The load isn't touched
|
|
pacejka.SetNormalForce(normalForce);
|
|
// Lateral
|
|
//qdbg(" csSlipLen=%f, oSA=%f,oSR=%f\n",csSlipLen,optimalSA,optimalSR);
|
|
if(csSlipLen<D3_EPSILON)
|
|
{
|
|
//qdbg(" csSlipLen near 0; load %f\n",normalForce);
|
|
pacejka.SetSlipAngle(0);
|
|
pacejka.SetSlipRatio(0);
|
|
// Calculate separate Flat/Flong (Fy/Fx)
|
|
pacejka.Calculate();
|
|
} else
|
|
{
|
|
if(slipAngle<0)
|
|
pacejka.SetSlipAngle(csSlipLen*optimalSA);
|
|
else
|
|
pacejka.SetSlipAngle(-csSlipLen*optimalSA);
|
|
// Longitudinal
|
|
if(slipRatio<0)
|
|
pacejka.SetSlipRatio(-csSlipLen*optimalSR);
|
|
else
|
|
pacejka.SetSlipRatio(csSlipLen*optimalSR);
|
|
// Calculate separate Flat/Flong (Fy/Fx)
|
|
pacejka.Calculate();
|
|
// Combine
|
|
|
|
#ifdef OPT_SLIPVECTOR_USES_TANSA
|
|
pacejka.SetFy( (fabs(tanSlipAngle)/(tanOptimalSA*csSlipLen))*
|
|
pacejka.GetFy());
|
|
pacejka.SetMz( (fabs(tanSlipAngle)/(tanOptimalSA*csSlipLen))*
|
|
pacejka.GetMz());
|
|
#else
|
|
// Use normal slip angle
|
|
pacejka.SetFy( (fabs(slipAngle)/(optimalSA*csSlipLen))*pacejka.GetFy());
|
|
pacejka.SetMz( (fabs(slipAngle)/(optimalSA*csSlipLen))*pacejka.GetMz());
|
|
#endif
|
|
pacejka.SetFx( (fabs(slipRatio)/(optimalSR*csSlipLen))*pacejka.GetFx());
|
|
|
|
}
|
|
} else
|
|
{
|
|
// Calculate Fx and Fy really separate, and possible combine later
|
|
pacejka.SetSlipAngle(-slipAngle);
|
|
pacejka.SetSlipRatio(slipRatio);
|
|
pacejka.SetNormalForce(normalForce);
|
|
// Calculate separate Flat/Flong (Fy/Fx), and maximum force
|
|
// Combined Pacejka (slipratio & slipangle) will be done later
|
|
pacejka.Calculate();
|
|
}
|
|
|
|
//----------------------------------------------------------------
|
|
//
|
|
//
|
|
//
|
|
|
|
|
|
// Adjust forces according to the surface
|
|
// May also add here some Pacejka scaling to get quick grip changes
|
|
/* RSurfaceType *st=surfaceInfo.surfaceType;
|
|
if(st)
|
|
{
|
|
if(st->frictionFactor!=1.0f)
|
|
{
|
|
|
|
float frictionFactor = 0.1f;
|
|
pacejka.SetFx(pacejka.GetFx()*frictionFactor);
|
|
pacejka.SetFy(pacejka.GetFy()*frictionFactor);
|
|
// Do the same to the aligning moment, although not scientifically
|
|
// based (does aligning torque scale linearly with surface friction?)
|
|
pacejka.SetMz(pacejka.GetMz()*frictionFactor);
|
|
pacejka.SetMaxLongForce(pacejka.GetMaxLongForce()*frictionFactor);
|
|
pacejka.SetMaxLatForce(pacejka.GetMaxLatForce()*frictionFactor);
|
|
}
|
|
*/
|
|
|
|
//forceRoadTC.z=pacejka.GetFx();
|
|
|
|
//----------------------------------------------------------------
|
|
//
|
|
// old
|
|
//
|
|
|
|
/*
|
|
float factor = getVehicle()->getEngine()->getForceFeedbackScale();
|
|
float longitudalImpulse = xCheckFloat(lastContactData->longitudalImpulse);
|
|
if (lastContactData)
|
|
{
|
|
forceRoadTC.z= longitudalImpulse * factor;
|
|
}*/
|
|
|
|
// Put some results in appropriate variables
|
|
#ifdef LTRACE
|
|
qdbg(" pacejka.Fx=%f\n",pacejka.GetFx());
|
|
#endif
|
|
|
|
float fx = pacejka.GetFx();;
|
|
forceRoadTC.z=pacejka.GetFx();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void ConvertTireToCarOrientation(pWheel2 * wheel , VxVector *from,VxVector *to)
|
|
// Convert vector 'from' from tire coordinates
|
|
// to car coordinates (into 'to')
|
|
// Assumes: from!=to
|
|
{
|
|
if (!wheel)
|
|
return;
|
|
|
|
float angle,sinAngle,cosAngle;
|
|
|
|
// Note that the tire is constrained in 5 dimensions, so only
|
|
// 1 rotation needs to be done
|
|
|
|
// Heading
|
|
angle=wheel->GetHeading();
|
|
sinAngle=sin(angle);
|
|
cosAngle=cos(angle);
|
|
|
|
// Rotate around Y axis to get heading of car right
|
|
to->x=from->z*sinAngle+from->x*cosAngle;
|
|
to->y=from->y;
|
|
to->z=from->z*cosAngle-from->x*sinAngle;
|
|
}
|
|
|
|
void ConvertCarToTireOrientation (pWheel2*wheel, VxVector *from,VxVector*to)
|
|
// Convert vector 'from' from car coordinates
|
|
// to tire coordinates (into 'to')
|
|
// Assumes: from!=to
|
|
{
|
|
if (!wheel)
|
|
return;
|
|
|
|
float angle,sinAngle,cosAngle;
|
|
|
|
// Heading
|
|
angle=-wheel->GetHeading();
|
|
|
|
if(fabs(angle)<D3_EPSILON)
|
|
{ *to=*from;
|
|
|
|
return;
|
|
}
|
|
sinAngle=sin(angle);
|
|
cosAngle=cos(angle);
|
|
// Rotate around Y axis to get heading of car right
|
|
to->x=from->z*sinAngle+from->x*cosAngle;
|
|
to->y=from->y;
|
|
to->z=from->z*cosAngle-from->x*sinAngle;
|
|
//to->DbgPrint("C2T to");
|
|
}
|
|
|
|
|
|
void pWheel2::calcVerticalForces()
|
|
{
|
|
VxVector downGravity;
|
|
//float mass = getVehicle()->getBody()->getMass();
|
|
|
|
NxVec3 grav;
|
|
getVehicle()->getBody()->getActor()->getScene().getGravity(grav);
|
|
|
|
|
|
downGravity.y = -mass * -grav.y ;
|
|
if (getEntity())
|
|
{
|
|
getEntity()->TransformVector(&forceGravityCC,&downGravity);
|
|
}
|
|
|
|
forceVerticalCC=forceGravityCC+forceRoadTC+WHEEL_SUSPENSION_FORCE;
|
|
|
|
float inverseWheelMass = mWheelShape->getInverseWheelMass();
|
|
|
|
|
|
}
|
|
void pWheel2::calcLongForces()
|
|
{
|
|
if (lastContactData)
|
|
{
|
|
|
|
//
|
|
// LONGITUDINAL FORCES
|
|
//
|
|
|
|
// Differential will do resulting long. torques for the engine
|
|
|
|
// Calculate longitudinal road reaction force using Pacejka's magic formula
|
|
float pf;
|
|
|
|
// Pacejka
|
|
float factor = getVehicle()->getEngine()->getForceFeedbackScale();
|
|
float longitudalImpulse = xCheckFloat(lastContactData->longitudalImpulse);
|
|
|
|
//qdbg("Pacejka Fx=%f, signU=%f\n",pf,signU);
|
|
forceRoadTC.z= factor * longitudalImpulse * signU ;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
void pWheel2::calcBreakForces()
|
|
{
|
|
|
|
|
|
}
|
|
|
|
|
|
void pWheel2::preAnimate()
|
|
{
|
|
|
|
//----------------------------------------------------------------
|
|
//
|
|
// store last contact
|
|
//
|
|
hadContact = getContact(*lastContactData);
|
|
//----------------------------------------------------------------
|
|
//
|
|
// reset
|
|
//
|
|
forceRoadTC.Set(0,0,0);
|
|
torqueTC.Set(0,0,0);
|
|
torqueFeedbackTC.Set(0,0,0);
|
|
dampingFactorLong=dampingFactorLat=0;
|
|
forceVerticalCC.Set(0,0,0);
|
|
forceDampingTC.Set(0,0,0);
|
|
torqueTC.Set(0,0,0);
|
|
torqueFeedbackTC.Set(0,0,0);
|
|
velWheelTC.Set(0,0,0);
|
|
velWheelCC.Set(0,0,0);
|
|
|
|
|
|
|
|
|
|
|
|
//signU = ( getVehicle()->_currentStatus & VS_IsRollingForward ) ? 1.0f : - 1.0f;
|
|
|
|
CalcLoad();
|
|
CalcSlipAngle();
|
|
CalcSlipRatio(&velWheelTC);
|
|
|
|
|
|
float tanOptimalSA=tan(0.18296f);
|
|
float optimalSR=0.0965f;
|
|
bool make =true;
|
|
|
|
if(getFrictionMethod() == FC_SLIPVECTOR)
|
|
{
|
|
// Calculate SA/SR vector length (>1 = wanting more than tire supports)
|
|
|
|
float lat,lon;
|
|
if (getVehicle()->getProcessOptions() & pVPO_SV_Tansa )
|
|
lat=tanSlipAngle/tanOptimalSA;
|
|
else
|
|
lat=slipAngle/optimalSA;
|
|
|
|
|
|
lon=slipRatio/optimalSR;
|
|
csSlipLen=sqrtf(lat*lat+lon*lon);
|
|
|
|
//qdbg(" latNrm=%f, lonNrm=%f, csSlipLen=%f\n",lat,lon,csSlipLen);
|
|
}
|
|
|
|
|
|
|
|
CalcSlipVelocity();
|
|
|
|
// Low speed detection
|
|
|
|
|
|
if (getVehicle()->getProcessOptions() & pVPO_CheckLowSpeed)
|
|
{
|
|
float lowSpeed = 0.03f;
|
|
if(rotationV.x>-lowSpeed&&rotationV.x<lowSpeed)
|
|
{
|
|
stateFlags|=LOW_SPEED;
|
|
// Calculate factor from 0..1. 0 means real low, 1 is on the edge
|
|
// of becoming high-enough speed.
|
|
lowSpeedFactor=rotationV.x/lowSpeed;
|
|
// Don't zero out all
|
|
if(lowSpeedFactor<0.01f)
|
|
lowSpeedFactor=0.01f;
|
|
}
|
|
else
|
|
{
|
|
stateFlags&=~LOW_SPEED;
|
|
}
|
|
}
|
|
CalcPacejka();
|
|
CalcDamping();
|
|
|
|
//CalcSlipAngle();
|
|
//CalcSlipRatio(&velWheelTC);
|
|
}
|
|
|
|
void pWheel2::CalcSlipVelocity()
|
|
{
|
|
|
|
slipVectorTC.x=0;
|
|
slipVectorTC.y=0;
|
|
slipVectorTC.z=rotationV.x*radius;
|
|
|
|
ConvertTireToCarOrientation(this,&slipVectorTC,&slipVectorCC);
|
|
|
|
/*
|
|
VxVector a0(1,2,3);
|
|
VxVector a1(3,4,5);
|
|
ConvertCarToTireOrientation(this,&a0,&a1);*/
|
|
//a1 = {x=1.0522050 y=2.0000000 z=2.9820907 ...}
|
|
|
|
|
|
|
|
|
|
slipVectorCC-=velWheelCC;
|
|
|
|
// Calculate length of slip
|
|
slipVectorLength=slipVectorCC.Magnitude();
|
|
|
|
// Calculate resulting friction coefficient
|
|
// This will be used to determine the tire's maximum force
|
|
|
|
float coeff = slipVectorLength/slip2FCVelFactor;
|
|
|
|
bool make = true;
|
|
/*if(make)
|
|
frictionCoeff=crvSlip2FC->GetValue(coeff);
|
|
else*/
|
|
this->frictionCoeff=1.0f;
|
|
|
|
|
|
|
|
}
|
|
void pWheel2::CalcLoad()
|
|
{
|
|
if (hadContact)
|
|
{
|
|
//forceRoadTC.y=-50*0.057598811f;// float; lastContactData->contactForce;
|
|
|
|
if ( (getVehicle()->getProcessOptions() & pVPO_Wheel_UsePHYSX_Load) &&
|
|
(getVehicle()->getProcessOptions() & pVPO_Wheel_UsePHYSX_CONTACT_DATA)
|
|
)
|
|
{
|
|
forceRoadTC.y = load;
|
|
}
|
|
else{
|
|
|
|
forceRoadTC.y=-tireRate * WHEEL_PENETRATION_DEPTH;// float; lastContactData->contactForce;
|
|
}
|
|
|
|
stateFlags|=ON_SURFACE;
|
|
}else
|
|
{
|
|
stateFlags&=~ON_SURFACE;
|
|
forceRoadTC.y = 0.0f;
|
|
}
|
|
|
|
//load=forceRoadTC.y;
|
|
|
|
radiusLoaded=getWheelShape()->getRadius();
|
|
}
|
|
void pWheel2::calcForces()
|
|
{
|
|
|
|
if (hadContact)
|
|
{
|
|
|
|
|
|
float len;
|
|
|
|
|
|
float pf=0.0f;
|
|
float radiusLoaded = getRadius();
|
|
|
|
//----------------------------------------------------------------
|
|
//
|
|
// vertical forces
|
|
//
|
|
|
|
NxVec3 grav;getBody()->getActor()->getScene().getGravity(grav);
|
|
|
|
VxVector forceGravityWC;
|
|
forceGravityWC.y = getMass() * grav.y;
|
|
|
|
if (getEntity())
|
|
getEntity()->TransformVector(&forceGravityCC,&forceGravityWC);
|
|
|
|
VxVector suspension;
|
|
suspension.y = -load;
|
|
forceVerticalCC=forceGravityCC+forceRoadTC+suspension;
|
|
|
|
|
|
|
|
//----------------------------------------------------------------
|
|
//
|
|
// long forces
|
|
//
|
|
|
|
//
|
|
// LONGITUDINAL FORCES
|
|
//
|
|
|
|
// Differential will do resulting long. torques for the engine
|
|
|
|
// Calculate longitudinal road reaction force using Pacejka's magic formula
|
|
// Pacejka
|
|
//float factor = getVehicle()->getEngine()->getForceFeedbackScale();
|
|
//float longitudalImpulse = xCheckFloat(lastContactData->longitudalImpulse);
|
|
//qdbg("Pacejka Fx=%f, signU=%f\n",pf,signU);
|
|
//forceRoadTC.z= factor * longitudalImpulse * signU ;
|
|
|
|
//----------------------------------------------------------------
|
|
//
|
|
//
|
|
//
|
|
|
|
|
|
#ifdef DO_GREGOR
|
|
pf=Fx;
|
|
#else
|
|
// Pacejka
|
|
pf=pacejka.GetFx()*this->frictionCoeff;
|
|
#endif
|
|
|
|
//qdbg("Pacejka Fx=%f, signU=%f\n",pf,signU);
|
|
forceRoadTC.z=signU*pf;
|
|
|
|
|
|
if ( getVehicle()->getProcessOptions() & pVPO_Lat_Damping )
|
|
{
|
|
// Add damping force because of slipRatio vibrations
|
|
// at low speed (SAE950311)
|
|
forceRoadTC.z+=forceDampingTC.z;
|
|
//qdbg("FroadTC.z=%f, Fdamp=%f\n",forceRoadTC.z,forceDampingTC.z);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------
|
|
//
|
|
// Calculate braking forces & torques (brakes & rolling resistance)
|
|
//
|
|
float curBrakingTorque;
|
|
// Calculate braking torque more directly
|
|
|
|
float bTorqueV = getVehicle()->calculateBraking( getVehicle()->_lastDT );
|
|
|
|
curBrakingTorque= bTorqueV * maxBrakingTorque/1000.0f;
|
|
curBrakingTorque*=brakingFactor;
|
|
// Apply braking torque in the reverse direction of the wheel's rotation
|
|
if(rotationV.x>0)
|
|
{
|
|
torqueBrakingTC.x=curBrakingTorque;
|
|
}
|
|
else
|
|
{
|
|
torqueBrakingTC.x=-curBrakingTorque;
|
|
}
|
|
forceBrakingTC.z=torqueBrakingTC.x/radiusLoaded;
|
|
|
|
// Calculate feedback torque (goes back to differential)
|
|
// This doesn't include braking and rolling resistance, which are
|
|
// potential braking forces (which may not be fully used if the
|
|
// wheel is not rotating), but only the forces which are always
|
|
// there, like road reaction.
|
|
torqueFeedbackTC.x=-forceRoadTC.z*radiusLoaded;
|
|
|
|
//----------------------------------------------------------------
|
|
//
|
|
//
|
|
//
|
|
// Calculate rolling resistance (from John Dixon, Fr=u*Fv
|
|
// where Fv is the normal force, and 'u' the rolling coefficient)
|
|
// Rolling coefficient may need to go up with speed though
|
|
//forceRoadTC.y = pf;
|
|
|
|
|
|
if(rotationV.x>0)
|
|
torqueRollingTC.x=-rollingCoeff*forceRoadTC.y*radiusLoaded;
|
|
else
|
|
torqueRollingTC.x=rollingCoeff*forceRoadTC.y*radiusLoaded;
|
|
|
|
// Calculate total braking torque (this is POTENTIAL, not always acted upon!)
|
|
torqueBrakingTC.x=-forceBrakingTC.z*radiusLoaded;
|
|
torqueBrakingTC.x+=torqueRollingTC.x;
|
|
|
|
//
|
|
// lateral forces :
|
|
//
|
|
/*float lateralForce = xCheckFloat(lastContactData->lateralImpulse);
|
|
factor = getVehicle()->getEngine()->getForceFeedbackScale();
|
|
forceRoadTC.x= signU*lateralForce * factor ;
|
|
*/
|
|
|
|
#ifdef DO_GREGOR
|
|
pf=Fy;
|
|
#else
|
|
pf=pacejka.GetFy()*frictionCoeff;
|
|
#endif
|
|
//pf=forceRoadTC.x;
|
|
|
|
#ifdef OBS
|
|
qdbg("Pacejka Fy(lat)=%f, SR=%f, SA=%f, load=%f\n",pf,slipRatio,slipAngle,load);
|
|
#endif
|
|
|
|
forceRoadTC.x=pf;
|
|
|
|
|
|
if(getProcessOptions() & pVPO_Forces_No_Lateral)
|
|
{
|
|
// Cancel lateral forces
|
|
pacejka.SetFy(0);
|
|
forceRoadTC.x=0;
|
|
}
|
|
|
|
if (getProcessOptions() & pVPO_Lat_Damping )
|
|
{
|
|
|
|
// Add damping force because of slipAngle vibrations
|
|
// at low speed (SAE950311)
|
|
// Note the mix of 2 coordinate systems here
|
|
forceRoadTC.x+=forceDampingTC.x;
|
|
//qdbg("Damping fRoad.x=%f, Fdamp=%f\n",forceRoadTC.x,forceDampingTC.x);
|
|
}
|
|
|
|
// WHEEL LOCKING
|
|
|
|
if(getVehicle()->getProcessOptions() & pVPO_Wheel_LockAdjust )
|
|
goto skip_wla;
|
|
|
|
|
|
|
|
if(slipRatio<0)
|
|
{
|
|
|
|
VxVector forceLockedCC,forceLockedTC;
|
|
float slideFactor,oneMinusSlideFactor,lenSlip,lenNormal,y;
|
|
|
|
// As the wheel is braked, more and more sliding kicks in.
|
|
// At the moment of 100% slide, the braking force points
|
|
// in the reverse direction of the slip velocity. Inbetween
|
|
// SR=0 and SR=-1 (and beyond), there is more and more sliding,
|
|
// so the force begins to point more and more like the slip vel.
|
|
|
|
|
|
// Calculate sliding factor (0..1)
|
|
if(slipRatio<-1.0f)
|
|
slideFactor=1.0f;
|
|
else
|
|
slideFactor=-slipRatio;
|
|
|
|
//slideFactor*=.75f;
|
|
oneMinusSlideFactor=1.0f-slideFactor;
|
|
// Calculate 100% wheel lock direction
|
|
forceLockedCC.x=slipVectorCC.x;
|
|
forceLockedCC.y=0;
|
|
forceLockedCC.z=slipVectorCC.z;
|
|
// Make it match forceRoadTC's coordinate system
|
|
|
|
ConvertCarToTireOrientation(this,&forceLockedCC,&forceLockedTC);
|
|
|
|
|
|
// Calc magnitude of normal and locked forces
|
|
lenSlip=forceLockedTC.Magnitude();
|
|
|
|
y=forceRoadTC.y; forceRoadTC.y=0;
|
|
lenNormal=forceRoadTC.Magnitude();
|
|
forceRoadTC.y=y;
|
|
if(lenSlip<D3_EPSILON)
|
|
{
|
|
// No force
|
|
forceLockedTC.Set(0,0,0);
|
|
} else
|
|
{
|
|
// Equalize force magnitude
|
|
forceLockedTC *=(lenNormal/lenSlip);
|
|
}
|
|
|
|
// Interpolate between both extreme forces
|
|
forceRoadTC.x=oneMinusSlideFactor*forceRoadTC.x+slideFactor*forceLockedTC.x;
|
|
forceRoadTC.z=oneMinusSlideFactor*forceRoadTC.z+slideFactor*forceLockedTC.z;
|
|
|
|
}
|
|
|
|
|
|
|
|
skip_wla:
|
|
|
|
|
|
// ALIGNING TORQUE
|
|
|
|
aligningTorque=pacejka.GetMz();
|
|
// Damp aligning torque at low speed to avoid jittering of wheel
|
|
// Friction will take care of forces in that case
|
|
len=velWheelCC.SquareMagnitude();
|
|
if(len<1.0)
|
|
{
|
|
// Damp
|
|
aligningTorque*=len;
|
|
}
|
|
|
|
//----------------------------------------------------------------
|
|
//
|
|
// aligning torque
|
|
//
|
|
|
|
|
|
// ALIGNING TORQUE
|
|
|
|
aligningTorque=pacejka.GetMz();
|
|
|
|
// Damp aligning torque at low speed to avoid jittering of wheel
|
|
// Friction will take care of forces in that case
|
|
|
|
len=velWheelCC.SquareMagnitude();
|
|
if(len<1.0)
|
|
{
|
|
// Damp
|
|
aligningTorque*=len;
|
|
}
|
|
|
|
|
|
if((stateFlags&LOW_SPEED))
|
|
{
|
|
//qdbg("LS: factor=%f\n",lowSpeedFactor);
|
|
// Long. force is often too high at low speed
|
|
forceRoadTC.z*=lowSpeedFactor;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void pWheel2::Integrate()
|
|
{
|
|
|
|
VxVector translation;
|
|
float oldVX;
|
|
|
|
|
|
float mTorque = getVehicle()->getGearBox()->GetTorqueForWheel(this);
|
|
|
|
//
|
|
// ROTATIONAL acceleration
|
|
//
|
|
oldVX=rotationV.x;
|
|
|
|
|
|
//----------------------------------------------------------------
|
|
//
|
|
//
|
|
//
|
|
//
|
|
// ROTATIONAL acceleration
|
|
//
|
|
//#define OBS
|
|
#ifdef OBS
|
|
// Check for locked wheel braking
|
|
//float netForce= getVehicle()->getEngine()->forEngin .z+forceRoadTC.z;
|
|
//qdbg("Fe=%f, Fr=%f, Fb=%f\n",forceEngineTC.z,forceRoadTC.z,forceBrakingTC.z);
|
|
if(rotationV.x>-RR_EPSILON_VELOCITY&&rotationV.x<RR_EPSILON_VELOCITY)
|
|
{
|
|
if((netForce<0&&forceBrakingTC.z>-netForce)||
|
|
(netForce>0&&forceBrakingTC.z<-netForce))
|
|
//if(forceBrakingTC.z<-netForce||forceBrakingTC.z>netForce)
|
|
{
|
|
//qdbg("RWh:Int; braking force keeps wheel still\n");
|
|
rotationV.x=0;
|
|
goto skip_rot;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
|
|
|
|
|
|
float timeStep= lastStepTimeSec;
|
|
|
|
|
|
|
|
float a = rotationA.x*timeStep ;
|
|
rotationV.x+=rotationA.x*timeStep;
|
|
//skip_rot:
|
|
|
|
// Check for wheel velocity reversal; in this case, damp the
|
|
// velocity to a minimum to get rid of jittering wheels at low
|
|
// speed and/or high braking.
|
|
if(differential)
|
|
{
|
|
if(oldVX>0&&rotationV.x<0)
|
|
{
|
|
// Lock the side
|
|
//qdbg("RWh%d; vel reversal, lock\n",wheelIndex);
|
|
differential->Lock(differentialSide);
|
|
rotationV.x=0;
|
|
differentialSlipRatio=0;
|
|
} else if(oldVX<0&&rotationV.x>0)
|
|
{
|
|
//qdbg("RWh%d; vel reversal, lock\n",wheelIndex);
|
|
// Lock the side
|
|
differential->Lock(differentialSide);
|
|
rotationV.x=0;
|
|
differentialSlipRatio=0;
|
|
}
|
|
} else
|
|
{
|
|
// No differential, use controlled jittering
|
|
if(oldVX>=0&&rotationV.x<0)
|
|
{
|
|
// Just lift the rotation over the 0 barrier; this will induce
|
|
// jittering near 0, but it's so small that it's unnoticeable.
|
|
// If we keep it at 0, you get larger jitters either way (+/-)
|
|
// until the wheel rotates again and starts the reversal again.
|
|
rotationV.x=-0.0001;
|
|
} else if(oldVX<=0&&rotationV.x>0)
|
|
{
|
|
rotationV.x=0.0001;
|
|
}
|
|
}
|
|
//qdbg(" rotV after velocity reversal=%f\n",rotationV.x);
|
|
|
|
// Wheel rotation (spinning)
|
|
//
|
|
|
|
float rotVx =rotation.x;
|
|
rotation.x+=rotationV.x*timeStep;
|
|
//rotation.x+=(rotationV.x+0.5f*rotationA.x*timeStep)*timeStep;
|
|
//rotation.x+=rotationV.x*timeStep;
|
|
// Keep rotation in limits
|
|
while(rotation.x>=2*PI)
|
|
rotation.x-=2*PI;
|
|
while(rotation.x<0)
|
|
rotation.x+=2*PI;
|
|
|
|
// Friction reversal measures
|
|
if( (oldVX<0&&rotationV.x>0) || (oldVX>0&&rotationV.x<0) )
|
|
{
|
|
rotationV.x=0;
|
|
|
|
//qdbg("RWh:Int; friction reversal halt; %f -> %f\n",oldVX,rotationV.x);
|
|
}
|
|
}
|
|
|
|
void pWheel2::CalcWheelAngAcc()
|
|
{
|
|
if(differential)
|
|
{
|
|
// Use differential to determine acceleration
|
|
if(differential->IsLocked(differentialSide))
|
|
{
|
|
// Wheel is held still by braking torques
|
|
rotationA.x=0;
|
|
} else
|
|
{
|
|
// Wheel is allowed to roll
|
|
rotationA.x=differential->GetAccOut(differentialSide);
|
|
}
|
|
//qdbg("diff=%p, diffSide=%d\n",differential,differentialSide);
|
|
} else
|
|
{
|
|
// Uses torque directly (no engine/differential forces)
|
|
rotationA.x=(torqueFeedbackTC.x+torqueBrakingTC.x)/GetInertia();
|
|
}
|
|
}
|
|
|
|
void pWheel2::CalcAccelerations()
|
|
{
|
|
|
|
rotationA.Set(0,0,0);
|
|
|
|
CalcWheelAngAcc();
|
|
//CalcBodyForce();
|
|
|
|
// Vertical forces; suspension, gravity, ground, tire rate
|
|
//acceleration=forceVerticalCC/GetMass();
|
|
acceleration.x=acceleration.z=0.0f;
|
|
acceleration.y=forceVerticalCC.y/mass;
|
|
|
|
|
|
|
|
#ifdef OBS
|
|
qdbg("RWh:AF; forceVerticalCC.y=%f, mass=%f\n",forceVerticalCC.y,MASS);
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
void pWheel2::CalcSlipRatio(VxVector *velWheelCC)
|
|
{
|
|
|
|
//rfloat vGround,vFreeRolling;
|
|
|
|
float lastTime =lastStepTimeSec;
|
|
|
|
/*
|
|
lastTime*=0.1f;
|
|
*/
|
|
|
|
//velWheelTC->DbgPrint("velWheelTC in CalcSR");
|
|
if(!hadContact)
|
|
{
|
|
//qdbg("CalcSR; not on surface\n");
|
|
// Not touching the surface, no slip ratio
|
|
slipRatio=0;
|
|
// Tire vibrations stop
|
|
differentialSlipRatio=0;
|
|
return;
|
|
}
|
|
|
|
// SAE950311 algorithm
|
|
float u,delta,B;
|
|
u=velWheelTC.z;
|
|
B=relaxationLengthLong;
|
|
|
|
// Switch to normal slip ratio calculations when there is
|
|
// speed (in the case when the velocities catch up on the timestep)
|
|
// Notice this test can be optimized, because the timestep and B
|
|
// are constants, so a 'turningPointU' can be defined.
|
|
//qdbg("Threshold: %f\n",u*RR_TIMESTEP/B);
|
|
|
|
float t0 = u*lastTime;
|
|
bool tc = B>0.5 ? true : false;
|
|
|
|
|
|
|
|
if(u*lastTime/B>0.5)
|
|
{
|
|
//----------------------------------------------------------------
|
|
//
|
|
// changed :
|
|
//
|
|
//float wheelSlipRatio
|
|
|
|
slipRatio=rotationV.x*radiusLoaded/u-1;
|
|
//slipRatio = lastContactData->longitudalSlip;
|
|
|
|
return;
|
|
// Use straightforward slip ratio
|
|
slipRatio=rotationV.x*radiusLoaded/u-1;
|
|
//qdbg("'u' big enough; straight SR %f\n",slipRatio);
|
|
return;
|
|
}
|
|
|
|
if((lastU<0&&u>=0)||(lastU>=0&&u<0))
|
|
{
|
|
// 'u' changed sign, reverse slip
|
|
//qdbg("'u' changed sign from %f to %f; SR=-SR\n",u,lastU);
|
|
//differentialSlipRatio=-differentialSlipRatio;
|
|
// Damp while reversing slip; we're close to a standstill.
|
|
float dampSRreversal = 0.2f;
|
|
differentialSlipRatio=-dampSRreversal*differentialSlipRatio;
|
|
}
|
|
lastU=u;
|
|
// Detect sign of 'u' (real velocity of wheel)
|
|
if(u<0)signU=-1.0f; else signU=1.0f;
|
|
|
|
// Eq. 26
|
|
//delta=(fabs(u)-rotationV.x*radius*signU)/B-(fabs(u)/B)*differentialSlipRatio;
|
|
if(u<0)
|
|
{
|
|
// Eq. 25
|
|
delta=(-u+rotationV.x*radius)/B+(u/B)*differentialSlipRatio;
|
|
} else
|
|
{
|
|
// Eq. 20
|
|
delta=(u-rotationV.x*radius)/B-(u/B)*differentialSlipRatio;
|
|
}
|
|
|
|
|
|
// Integrate
|
|
differentialSlipRatio+=delta*lastTime;
|
|
if(differentialSlipRatio>10)differentialSlipRatio=10;
|
|
else if(differentialSlipRatio<-10)differentialSlipRatio=-10;
|
|
// Set slip ratio for the rest of the sim. Note that
|
|
// we use a different slip ratio definition from that of SAE950311
|
|
//qdbg(" old SR=%f => new SR=%f\n",slipRatio,-differentialSlipRatio);
|
|
slipRatio=-differentialSlipRatio;
|
|
|
|
|
|
}
|
|
|
|
void pWheel2::updatePosition()
|
|
{
|
|
NxMat34 &wheelPose = getWheelPose();
|
|
NxWheelContactData wcd;
|
|
NxShape* contactShape = mWheelShape->getContact(wcd);
|
|
NxReal stravel = mWheelShape->getSuspensionTravel();
|
|
NxReal radius = mWheelShape->getRadius();
|
|
//have ground contact?
|
|
if( contactShape && wcd.contactPosition <= (stravel + radius) ) {
|
|
wheelPose.t = NxVec3( wheelPose.t.x, wcd.contactPoint.y + getRadius(), wheelPose.t.z );
|
|
}
|
|
else {
|
|
wheelPose.t = NxVec3( wheelPose.t.x, wheelPose.t.y - getSuspensionTravel(), wheelPose.t.z );
|
|
}
|
|
}
|
|
|
|
void pWheel2::updateSteeringPose(float rollangle,float steerAngle,float dt)
|
|
{
|
|
|
|
float rollAngle = getWheelRollAngle();
|
|
|
|
rollAngle+=getWheelShape()->getAxleSpeed() * (dt * 0.01f);
|
|
|
|
|
|
while (rollAngle > NxTwoPi) //normally just 1x
|
|
rollAngle-= NxTwoPi;
|
|
while (rollAngle< -NxTwoPi) //normally just 1x
|
|
rollAngle+= NxTwoPi;
|
|
|
|
setWheelRollAngle(rollAngle);
|
|
|
|
NxMat34& wheelPose = mWheelShape->getGlobalPose();
|
|
|
|
|
|
NxReal stravel = mWheelShape->getSuspensionTravel();
|
|
NxReal radius = mWheelShape->getRadius();
|
|
|
|
|
|
|
|
float rAngle = getWheelRollAngle();
|
|
float steer = mWheelShape->getSteerAngle();
|
|
|
|
|
|
NxVec3 dir;
|
|
|
|
NxReal r = mWheelShape->getRadius();
|
|
NxReal st = mWheelShape->getSuspensionTravel();
|
|
|
|
|
|
wheelPose.M.getColumn(1, dir);
|
|
dir = -dir; //cast along -Y.
|
|
NxReal castLength = r + st; //cast ray this long
|
|
|
|
|
|
NxMat33 rot, axisRot, rollRot;
|
|
rot.rotY( mWheelShape->getSteerAngle() );
|
|
axisRot.rotY(0);
|
|
rollRot.rotX(rAngle);
|
|
wheelPose.M = rot * wheelPose.M * axisRot * rollRot;
|
|
|
|
setWheelPose(wheelPose);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void pWheel2::CalcSlipAngle()
|
|
// Based on the wheel world velocity and heading, calculate
|
|
// the angle from the wheel heading to the wheel's actual velocity
|
|
// Contrary to SAE, our Y-axis points up, and slipAngle>0 means
|
|
// in our case that the you're in a right turn (basically)
|
|
// 31-03-01: SAE950311 implementation (using relaxation lengths
|
|
// and a differential equation for the slip angle) for low-speed etc.
|
|
{
|
|
|
|
|
|
VxVector velWheelWC;
|
|
|
|
if(!hadContact)
|
|
{
|
|
//qdbg("CalcSA; not on surface\n");
|
|
// Not touching the surface, no need to do slip angle
|
|
slipAngle=0;
|
|
// Tire springs back to 0 instantly (should be slowly actually)
|
|
tanSlipAngle=0;
|
|
velWheelTC.Set(0,0,0);
|
|
velWheelCC.Set(0,0,0);
|
|
return;
|
|
}
|
|
|
|
|
|
int options = getProcessOptions();
|
|
|
|
if (options & pVPO_Wheel_UsePHYSX_CONTACT_DATA )
|
|
{
|
|
|
|
/*
|
|
VxVector dir,up,right;
|
|
getEntity()->GetOrientation(&dir,&up,&right);
|
|
*/
|
|
|
|
VxVector point= lastContactData->contactPoint;
|
|
if( lastContactData->contactPosition <= ( getWheelShape()->getSuspensionTravel() + radius) )
|
|
point.y+=getRadius();
|
|
else
|
|
point.y = getWheelPose().t.y - getSuspensionTravel();
|
|
|
|
//getEntity()->TransformVector(&point,&point);
|
|
/*
|
|
getBody()->GetVT3DObject()->InverseTransformVector(&point,&point);
|
|
|
|
/*
|
|
|
|
getEntity()->InverseTransformVector(&contactLocal,&point);
|
|
|
|
VxVector contactLocal2;
|
|
getEntity()->InverseTransform(&contactLocal,&point);
|
|
*/
|
|
|
|
getBody()->GetVT3DObject()->InverseTransform(&point,&point);
|
|
|
|
velWheelCC = getBody()->getLocalPointVelocity(point);
|
|
//getEntity()->TransformVector(&velWheelCC,&velWheelCC);
|
|
ConvertCarToTireOrientation(this,&velWheelCC,&velWheelTC);
|
|
|
|
|
|
}
|
|
|
|
if ( options & pVPO_SA_Delay )
|
|
{
|
|
// Derive change in tan(slipAngle) using SAE950311
|
|
// u=longitudinal velocity, v=lateral velocity
|
|
float u,b,v,delta;
|
|
u=velWheelTC.z;
|
|
v=velWheelTC.x;
|
|
b=relaxationLengthLat;
|
|
|
|
if ( options & pVPO_SA_DownSettle)
|
|
{
|
|
// Make sure SA settles down at low speeds
|
|
float min=5.0f;
|
|
if(u<min&&u>=0.0f)u=min;
|
|
else if(u>-min&&u<=0)u=-min;
|
|
}
|
|
|
|
|
|
// Calculate change in tan(SA)
|
|
delta=(v/b)-(fabs(u)/b)*tanSlipAngle;
|
|
if( getWheelFlag(WF_Accelerated) && getVehicle() )
|
|
{
|
|
//qdbg("CSA; u=%f, v=%f, delta=%f, tanSA=%f, SA=%f\n",u,v,delta,tanSlipAngle,slipAngle);
|
|
// qdbg("delta=%f, tanSA=%f, SA=%f\n",delta,tanSlipAngle,atan(tanSlipAngle));
|
|
// Integrate
|
|
tanSlipAngle+=delta*lastStepTimeMS;
|
|
// Derive new slip angle from state variable 'tanSlipAngle'
|
|
slipAngle=atanf(tanSlipAngle);
|
|
|
|
}
|
|
}else
|
|
{
|
|
|
|
// Calculate wheel velocity because of rotation around yaw (Y) axis
|
|
|
|
/*velWheelCC.x+=car->GetBody()->GetRotVel()->y*cos(-angleCGM)*distCGM;
|
|
velWheelCC.z+=car->GetBody()->GetRotVel()->y*sin(-angleCGM)*distCGM;
|
|
ConvertCarToTireOrientation(&velWheelCC,&velWheelTC);
|
|
*/
|
|
|
|
/*
|
|
// Get velocity of contact patch wrt the track (=world)
|
|
DVector3 cpPos,*p;
|
|
p=susp->GetPosition();
|
|
// Taking the tire movement from really the contact patch location
|
|
// seems to overrate the lateral movement the tire goes through.
|
|
// This is due because the body rolls, but normally the suspension
|
|
// won't roll the tire in exactly the same way, but instead,
|
|
// the tire rotates with respect to the body, so the lateral
|
|
// movements are actually quite small.
|
|
// To simulate this, I approximate the movement with respect
|
|
// to the suspension attachment point, which moves far less
|
|
// laterally. This became apparent when dealing with much stiffer
|
|
// tire and suspension spring rates, which made the car wobbly.
|
|
// Taking a less laterally sensitive position solved this.
|
|
cpPos.x=p->x;
|
|
//cpPos.y=p->y+susp->GetLength()+radius;
|
|
cpPos.y=p->y;
|
|
cpPos.z=p->z; // Kingpin effects are ignored
|
|
car->GetBody()->CalcBodyVelForBodyPos(&cpPos,&velWheelCC);
|
|
car->ConvertCarToWorldOrientation(&velWheelCC,&velWheelWC);
|
|
ConvertCarToTireOrientation(&velWheelCC,&velWheelTC);
|
|
*/
|
|
|
|
//qdbg("velWheelTC.z=%f, rotZ=%f\n",velWheelTC.z,rotationV.x*radius);
|
|
|
|
// Non-SAE950311 method (no lag in slip angle buildup)
|
|
// Calculate slip angle as the angle between the wheel velocity vector
|
|
// and the wheel direction vector
|
|
|
|
if(velWheelCC.x>-RR_EPSILON_VELOCITY&&velWheelCC.x<RR_EPSILON_VELOCITY &&
|
|
velWheelCC.z>-RR_EPSILON_VELOCITY&&velWheelCC.z<RR_EPSILON_VELOCITY)
|
|
{
|
|
//Very low speed; no slip angle
|
|
//slipAngle=-GetHeading();
|
|
slipAngle=0;
|
|
//qdbg(" RWh;CSA; LS => slipAngle=0\n");
|
|
}
|
|
else
|
|
{
|
|
// Enough velocity, calculate real angle
|
|
float h = GetHeading();
|
|
|
|
float realSteer = mWheelShape->getSteerAngle();
|
|
|
|
slipAngle=atan2(velWheelCC.x,velWheelCC.z)-GetHeading();
|
|
|
|
if (getProcessOptions() & pVPO_SA_Damping )
|
|
{
|
|
// Damp down at low velocity
|
|
if(fabs(velWheelCC.x)<0.3f&&fabs(velWheelCC.z)<0.3f)
|
|
{
|
|
float max=fabs(velWheelCC.x);
|
|
if(fabs(velWheelCC.z)<max)
|
|
max=fabs(velWheelCC.z);
|
|
//qdbg("damp SA %f factor %f => %f\n",slipAngle*RR_RAD2DEG,max,slipAngle*max);
|
|
slipAngle*=max*0.5f;
|
|
}
|
|
}
|
|
// Keep slipAngle between -180..180 degrees
|
|
if(slipAngle<-PI)
|
|
slipAngle+=2*PI;
|
|
else if(slipAngle>PI)
|
|
slipAngle-=2*PI;
|
|
|
|
}
|
|
if ( options & pVPO_SV_Tansa)
|
|
{
|
|
bool make = true ;
|
|
if(make)
|
|
{
|
|
// We need tan(SA) when combining Pacejka Fx/Fy later
|
|
tanSlipAngle=tan(slipAngle);
|
|
//qdbg("FCSV: tanSA=%f\n",tanSlipAngle);
|
|
}
|
|
}
|
|
}
|
|
} |