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);
*/
}
void NxRobot::cloneRobot(int indexNewScene, int indexNewRobot, NxVec3 newPosition, int indexNewTeam)
{
NxRobot* nxRobotSource = Simulation::gScenes[this->indexScene]->allRobots->getRobotByIdByTeam(this->id, this->idTeam);
NxActor* robotActor = Simulation::cloneActor(nxRobotSource->getActor(),indexNewScene);
//NxBounds3 bodyBounds;
//robotShapes[0]->getWorldBounds(bodyBounds);
NxVehicleDesc vehicleDesc;
vehicleDesc.position = NxVec3(robotActor->getGlobalPosition());
vehicleDesc.mass = robotActor->getMass();
//vehicleDesc.motorForce = 70000;
//vehicleDesc.maxVelocity = 300.f;
//vehicleDesc.cameraDistance = 8.0f;
vehicleDesc.centerOfMass.set(robotActor->getCMassLocalPosition());
//vehicleDesc.differentialRatio = 3.42f;
//vehicleDesc.transmissionEfficiency
vehicleDesc.actor = robotActor;
//Motor descricao
//NxVehicleMotorDesc motorsDesc[4];
//for(NxU32 i=0;i<4;i++)
//{
//motorsDesc[i].setToCorvette();
//vehicleDesc.motorsDesc.push_back(&motorsDesc[i]);
//}
//Roda (Wheel) descricao
int numberWheels = nxRobotSource->getNbWheels();
NxWheelDesc* wheelDesc = new NxWheelDesc[numberWheels];
for(NxU32 i=0; i<numberWheels; i++)
{
//NxActor* wheelModel = Simulation::getActorWheel(indexSourceScene,indexNewRobot,i);
//NxActorDesc wheelActorDesc;
//wheelModel->saveToDesc(wheelActorDesc);
//Simulation::gScenes[0]->releaseActor(*wheelModel);
//NxShape*const* wheelShapes = actorWheel->getShapes();
//NxBounds3 wheelBounds;
//wheelShapes[0]->getWorldBounds(wheelBounds);
const NxWheel* wheel = nxRobotSource->getWheel(i);
NxWheelShape* wheelShape = ((NxWheel2*)wheel)->getWheelShape();
//wheelDesc[i].wheelApproximation = 10;
wheelDesc[i].wheelOrientation = wheelShape->getGlobalOrientation();
wheelDesc[i].position.set(wheelShape->getGlobalPosition()-robotActor->getGlobalPosition());
//wheelDesc[i].position.z = 0;
wheelDesc[i].id = i;
wheelDesc[i].wheelFlags = ((NxWheel*)wheel)->getWheelFlags();
wheelDesc[i].wheelRadius = wheel->getRadius();
//wheelDesc[i].wheelWidth = 100;
wheelDesc[i].wheelSuspension = wheelShape->getSuspensionTravel();
wheelDesc[i].springRestitution = 0;
wheelDesc[i].springDamping = 0;
wheelDesc[i].springBias = 0.0f;
wheelDesc[i].maxBrakeForce = 100;
wheelDesc[i].frictionToFront = 0.1f;//0.1f;
wheelDesc[i].frictionToSide = 0;//0.02f;//
wheelDesc[i].inverseWheelMass = wheelShape->getInverseWheelMass(); //TODO: CONFIGURAR PARÂMETRO
//Angulo das Rodas (Omni)
wheelDesc[i].angWheelRelRobot = ((NxWheel2*)wheel)->angWheelRelRobot;
vehicleDesc.robotWheels.pushBack(&wheelDesc[i]);
}
//Criar robot, vehicle base
NxRobot* robot = (NxRobot*)NxRobot::createVehicle(Simulation::gScenes[indexNewScene], &vehicleDesc);
//NxRobot* robot = (NxRobot*)NxRobot::createVehicle(gScenes[indexSourceScene], &vehicleDesc);
robot->setId(indexNewRobot);
robot->setIdTeam(indexNewTeam);
robot->indexScene = indexNewScene;
robot->bodyRadius = nxRobotSource->bodyRadius;
//Dribbler and Kicker
NxShape*const* robotShapes = robotActor->getShapes();
for(int i=0; i<robotActor->getNbShapes(); i++) {
const char* shapeName = robotShapes[i]->getName();
if(shapeName) {
char* dribblerName = new char[10];//"Driblador\0"
dribblerName[9] = 0;
memcpy(dribblerName, shapeName, strlen(dribblerName));
if(strcmp(dribblerName, "Driblador") == 0) {
robot->dribbler->dribblerShapes.push_back(robotShapes[i]);
}
delete dribblerName;
}
}
robot->kicker->kickerShapeDesc = new NxBoxShapeDesc();
NxShapeDesc* shapeDesc = nxRobotSource->kicker->kickerShapeDesc;
robot->kicker->kickerShapeDesc->localPose = shapeDesc->localPose;
((NxBoxShapeDesc*)(robot->kicker->kickerShapeDesc))->dimensions = ((NxBoxShapeDesc*)shapeDesc)->dimensions;
//Initial Pose
robot->setInitialPose(robotActor->getGlobalPose());
robotActor->putToSleep();
//Transladando o robo
robot->getActor()->setGlobalPosition(newPosition);
string label;
string plabel = "Robo";
stringstream out;
out << indexNewRobot;
out << "-";
out << indexNewTeam;
//out << "-";
//out << indexNewScene;
label.append(plabel);
label.append(out.str());
char* arrayLabel = new char[label.size()+1];
arrayLabel[label.size()]=0;
memcpy(arrayLabel, label.c_str(), label.size());
robotActor->setName(arrayLabel);
//delete arrayLabel;
}