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; }