void DrawWheelShape(NxShape* wheel)
{
if(wheel->is(NX_SHAPE_WHEEL) != NULL)
{
NxWheelShape* wheelShape = (NxWheelShape*)wheel;
glPushMatrix();
float glmat[16];
wheel->getGlobalPose().getColumnMajor44(glmat);
NxWheelShapeDesc wheelShapeDesc;
float r = wheelShape->getRadius();
float a = wheelShape->getSteerAngle();
glMultMatrixf(&(glmat[0]));
glTranslatef(-r/2,0,0);
glRotatef(90,0,1,0);
glRotatef(NxMath::radToDeg(a),0,1,0);
glScalef(r, r, r);
RenderCylinder();
glPopMatrix();
}
}
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 Vehicle::updataBodyAndWheelNode(NxReal timeSinceLastFrame)
{//更新车身和轮子节点
NxQuat nxquat;
//更新父节点
mBaseCarNode->setPosition(mActor->getGlobalPosition().x, mActor->getGlobalPosition().y,
mActor->getGlobalPosition().z);
mActor->getGlobalOrientation().toQuat(nxquat);
mBaseCarNode->setOrientation(nxquat.w, nxquat.x, nxquat.y, nxquat.z);
//更新车身节点
//mBodyNode->setPosition(mBodyShape->getLocalPosition().x, mBodyShape->getLocalPosition().y,
// mBodyShape->getLocalPosition().z);
//mBodyShape->getLocalPose().M.toQuat(nxquat);
//mBodyNode->setOrientation(nxquat.w, nxquat.x, nxquat.y, nxquat.z);
//更新轮子节点
for (uint i = 0;i<mWheels.size();i++)
{
NxMat33 wheelMat;
NxMat33 steerMat, rollMat;
NxVec3 wheelPos;
NxWheelShape* wheel = mWheels[i].mWheel;
wheelPos = wheel->getLocalPosition();
//Ogre::Vector3 vec3(UtilityFunc::NxVec3_To_OgreVec3(mWheels[i].mWheel->getLocalPosition()));
//nxquat.fromAngleAxis(NxMath::radToDeg(mWheels[i].mWheel->getSteerAngle()), NxVec3(0, 1, 0));
//轮子角度
steerMat.rotY(wheel->getSteerAngle());
mRollAngle += wheel->getAxleSpeed() * timeSinceLastFrame;
//防溢出
while (mRollAngle > NxTwoPi)
mRollAngle -= NxTwoPi;
while (mRollAngle < -NxTwoPi)
mRollAngle += NxTwoPi;
//轮子转动
rollMat.rotX(mRollAngle);
wheelMat = steerMat * rollMat;
wheelMat.toQuat(nxquat);
NxWheelContactData wcd;
NxShape* shape = mWheels[i].mWheel->getContact(wcd);
if (shape)
{
NxReal radius = wheel->getRadius();
wheelPos.y += radius - wcd.contactPosition;
}
else
{
NxReal suspensionTravel = wheel->getSuspensionTravel();
wheelPos.y += -suspensionTravel;
}
mWheels[i].mSceneNode->setPosition(UtilityFunc::NxVec3_To_OgreVec3(wheelPos));
mWheels[i].mSceneNode->setOrientation(UtilityFunc::NxQuat_ToOgre_Quat(nxquat));
//mWheels[i].mSceneNode->rotate(Ogre::Vector3(1, 0, 0), Ogre::Radian(mRollAngle));
}
}
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);
*/
}