void pWheel2::tick(bool handBrake, float motorTorque, float brakeTorque, float dt)
{
if(handBrake && getWheelFlag(WF_AffectedByHandbrake))
brakeTorque = 1000.0f;
if(getWheelFlag(WF_Accelerated))
mWheelShape->setMotorTorque(motorTorque);
mWheelShape->setBrakeTorque(brakeTorque);
NxWheelShape *wShape = getWheelShape();
float rollAngle = getWheelRollAngle();
rollAngle+=wShape->getAxleSpeed() * (dt* 0.01f);
while (rollAngle > NxTwoPi) //normally just 1x
rollAngle-= NxTwoPi;
while (rollAngle< -NxTwoPi) //normally just 1x
rollAngle+= NxTwoPi;
setWheelRollAngle(rollAngle);
NxMat34& wheelPose = wShape->getGlobalPose();
NxWheelContactData wcd;
NxShape* cShape = wShape->getContact(wcd);
NxReal stravel = wShape->getSuspensionTravel();
NxReal radius = wShape->getRadius();
//have ground contact?
if( cShape && 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 );
}
float rAngle = rollAngle;
float steer = wShape->getSteerAngle();
NxMat33 rot, axisRot, rollRot;
rot.rotY( wShape->getSteerAngle() );
axisRot.rotY(0);
rollRot.rotX(rollAngle);
wheelPose.M = rot * wheelPose.M * axisRot * rollRot;
setWheelPose(wheelPose);
}
void UpdateWheelShapeUserData()
{
// Look for wheel shapes
NxU32 nbActors = gScene->getNbActors();
NxActor** actors = gScene->getActors();
while (nbActors--)
{
NxU32 nbShapes = actors[nbActors]->getNbShapes();
NxShape*const* shapes = actors[nbActors]->getShapes();
while (nbShapes--)
{
NxShape* shape = shapes[nbShapes];
if (shape->getType() == NX_SHAPE_WHEEL)
{
NxWheelShape* ws = (NxWheelShape*)shape;
ShapeUserData* sud = (ShapeUserData*)(shape->userData);
if (sud)
{
// Need to save away roll angle in wheel shape user data
NxReal rollAngle = sud->wheelShapeRollAngle;
// rollAngle += ws->getAxleSpeed() * 1.0f/60.0f;
rollAngle += ws->getAxleSpeed() * gDeltaTime;
while (rollAngle > NxTwoPi) //normally just 1x
rollAngle -= NxTwoPi;
while (rollAngle < -NxTwoPi) //normally just 1x
rollAngle += NxTwoPi;
// We have the roll angle for the wheel now
sud->wheelShapeRollAngle = rollAngle;
NxMat34 pose;
pose = ws->getGlobalPose();
NxWheelContactData wcd;
NxShape* s = ws->getContact(wcd);
NxReal r = ws->getRadius();
NxReal st = ws->getSuspensionTravel();
NxReal steerAngle = ws->getSteerAngle();
// NxWheelShapeDesc state;
// ws->saveToDesc(state);
NxVec3 p0;
NxVec3 dir;
/*
getWorldSegmentFast(seg);
seg.computeDirection(dir);
dir.normalize();
*/
p0 = pose.t; //cast from shape origin
pose.M.getColumn(1, dir);
dir = -dir; //cast along -Y.
NxReal castLength = r + st; //cast ray this long
// renderer.addArrow(p0, dir, castLength, 1.0f);
//have ground contact?
// This code is from WheelShape.cpp in SDKs/core/common/src
// if (contactPosition != NX_MAX_REAL)
if (s && wcd.contactForce > -1000)
{
// pose.t = p0 + dir * wcd.contactPoint;
// pose.t -= dir * state.radius; //go from contact pos to center pos.
pose.t = wcd.contactPoint;
pose.t -= dir * r; //go from contact pos to center pos.
NxMat33 rot, axisRot;
rot.rotY(steerAngle);
axisRot.rotY(0);
// NxReal rollAngle = ((ShapeUserData*)(wheel->userData))->rollAngle;
NxMat33 rollRot;
rollRot.rotX(rollAngle);
pose.M = rot * pose.M * axisRot * rollRot;
sud->wheelShapePose = pose;
}
else
{
pose.t = p0 + dir * st;
sud->wheelShapePose = pose;
}
}
}
}
}
}
void pWheel2::_tick(float dt)
{
NxWheelShape *wShape = getWheelShape();
if (!wShape) return;
NxVec3 _localVelocity;
bool _breaking=false;
//////////////////////////////////////////////////////////////////////////
//
//
//
NxWheelContactData wcd;
NxShape* contactShape = wShape->getContact(wcd);
if (contactShape)
{
NxVec3 relativeVelocity;
if ( !contactShape->getActor().isDynamic())
{
relativeVelocity = getActor()->getLinearVelocity();
} else {
relativeVelocity = getActor()->getLinearVelocity() - contactShape->getActor().getLinearVelocity();
}
NxQuat rotation = getActor()->getGlobalOrientationQuat();
_localVelocity = relativeVelocity;
rotation.inverseRotate(_localVelocity);
_breaking = false; //NxMath::abs(_localVelocity.z) < ( 0.1 );
// wShape->setAxleSpeed()
}
float rollAngle = getWheelRollAngle();
rollAngle+=wShape->getAxleSpeed() * (dt* 0.01f);
//rollAngle+=wShape->getAxleSpeed() * (1.0f/60.0f /*dt* 0.01f*/);
while (rollAngle > NxTwoPi) //normally just 1x
rollAngle-= NxTwoPi;
while (rollAngle< -NxTwoPi) //normally just 1x
rollAngle+= NxTwoPi;
setWheelRollAngle(rollAngle);
NxMat34& wheelPose = wShape->getGlobalPose();
NxReal stravel = wShape->getSuspensionTravel();
NxReal radius = wShape->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 );
}
float rAngle = getWheelRollAngle();
float steer = wShape->getSteerAngle();
NxVec3 p0;
NxVec3 dir;
/*
getWorldSegmentFast(seg);
seg.computeDirection(dir);
dir.normalize();
*/
NxReal r = wShape->getRadius();
NxReal st = wShape->getSuspensionTravel();
NxReal steerAngle = wShape->getSteerAngle();
p0 = wheelPose.t; //cast from shape origin
wheelPose.M.getColumn(1, dir);
dir = -dir; //cast along -Y.
NxReal castLength = r + st; //cast ray this long
NxMat33 rot, axisRot, rollRot;
rot.rotY( wShape->getSteerAngle() );
axisRot.rotY(0);
rollRot.rotX(rAngle);
wheelPose.M = rot * wheelPose.M * axisRot * rollRot;
setWheelPose(wheelPose);
}
void pWheel2::_updateVirtoolsEntity(bool position,bool rotation)
{
CK3dEntity *ent = static_cast<CK3dEntity*>(GetPMan()->GetContext()->GetObject(getEntID()));
if (ent && position)
{
NxWheelShape *wShape = getWheelShape();
NxMat34 pose = wShape->getGlobalPose();
NxWheelContactData wcd;
NxShape* contactShape = wShape->getContact(wcd);
NxVec3 suspensionOffsetDirection;
pose.M.getColumn(1, suspensionOffsetDirection);
suspensionOffsetDirection =-suspensionOffsetDirection;
if (contactShape && wcd.contactForce > -1000)
{
NxVec3 toContact = wcd.contactPoint - pose.t;
double alongLength = suspensionOffsetDirection.dot(toContact);
NxVec3 across = toContact - alongLength * suspensionOffsetDirection;
double r = wShape->getRadius();
double pullBack = sqrt(r*r - across.dot(across));
pose.t += (alongLength - pullBack) * suspensionOffsetDirection;
} else {
pose.t += wShape->getSuspensionTravel() * suspensionOffsetDirection;
}
VxVector oPos = getFrom(pose.t);
ent->SetPosition(&oPos);
if (hasGroundContact())
{
NxWheelShape *wShape = getWheelShape();
NxMat34& wheelPose = wShape->getGlobalPose();
/* NxWheelContactData wcd;
NxShape* cShape = wShape->getContact(wcd);
NxReal stravel = wShape->getSuspensionTravel();
NxReal radius = wShape->getRadius();
VxVector gPos = getFrom(getWheelPose().t);
/*
if( cShape && wcd.contactPosition <= (stravel + radius) )
{
}*/
//////////////////////////////////////////////////////////////////////////
/*VxVector gPos = getFrom(getWheelPose().t);
//gPos*=-1.0f;
gPos -=getWheelPos();
V 3.
xVector gPos2 = getFrom(getWheelShape()->getLocalPose().t);
ent->SetPosition(&gPos2,getBody()->GetVT3DObject());
*/
}else
{
// VxVector gPos = getWheelPos();
// ent->SetPosition(&gPos,getBody()->GetVT3DObject());
}
}
if (ent && rotation)
{
//float rollAngle = getWheelRollAngle();
//rollAngle+=wShape->getAxleSpeed() * (dt* 0.01f);
VxQuaternion rot = pMath::getFrom( getWheelPose().M );
ent->SetQuaternion(&rot,NULL);
}
/*
NxWheelShape *wShape = getWheelShape();
while (rollAngle > NxTwoPi) //normally just 1x
rollAngle-= NxTwoPi;
while (rollAngle< -NxTwoPi) //normally just 1x
rollAngle+= NxTwoPi;
setWheelRollAngle(rollAngle);
NxMat34& wheelPose = wShape->getGlobalPose();
NxWheelContactData wcd;
NxShape* cShape = wShape->getContact(wcd);
NxReal stravel = wShape->getSuspensionTravel();
NxReal radius = wShape->getRadius();
//have ground contact?
if( cShape && 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 );
}
float rAngle = rollAngle;
float steer = wShape->getSteerAngle();
NxMat33 rot, axisRot, rollRot;
rot.rotY( wShape->getSteerAngle() );
axisRot.rotY(0);
rollRot.rotX(rollAngle);
wheelPose.M = rot * wheelPose.M * axisRot * rollRot;
setWheelPose(wheelPose);
*/
}