Exemplo n.º 1
0
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);
}
Exemplo n.º 2
0
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));
	}
}
Exemplo n.º 3
0
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;
					}
				}
			}
		}
	}
}
Exemplo n.º 4
0
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);
}
Exemplo n.º 5
0
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);

	*/
}
Exemplo n.º 6
0
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;
}