C++ QUATERNION::Rotate方法代码示例

//  old
bool Scene::LoadStartPos(Ogre::String file)
{
	CONFIGFILE param;
	if (!param.Load(file))
		return false;

	float f3[3], f1;
	QUATERNION <float> fixer;  fixer.Rotate(3.141593, 0,0,1);
	
	param.GetParam("start position 0", f3);
	MATHVECTOR <float, 3> pos(f3[2], f3[0], f3[1]);

	if (!param.GetParam("start orientation-xyz 0", f3))
		return false;

	if (!param.GetParam("start orientation-w 0", f1))
		return false;

	QUATERNION <float> rot(f3[2], f3[0], f3[1], f1);
	rot = fixer * rot;

	startPos = pos;
	startRot = rot;
	return true;
}

QUATERNION<Dbl> CARDYNAMICS::GetWheelOrientation(WHEEL_POSITION wp) const
{
	QUATERNION<Dbl> siderot;
	if(wp == FRONT_RIGHT || wp == REAR_RIGHT)
	{
		siderot.Rotate(PI_d, 0, 0, 1);
	}
	return chassisRotation * GetWheelSteeringAndSuspensionOrientation(wp) * wheel[wp].GetOrientation() * siderot;
}

///returns the orientation of the wheel due only to steering and suspension
QUATERNION<Dbl> CARDYNAMICS::GetWheelSteeringAndSuspensionOrientation(WHEEL_POSITION wp) const
{
	QUATERNION<Dbl> steer;
	steer.Rotate( -wheel[wp].GetSteerAngle() * PI_d/180.0, 0,0,1);

	QUATERNION<Dbl> camber;
	Dbl camber_rotation = -suspension[wp].GetCamber() * PI_d/180.0;
	if (wp == 1 || wp == 3)
		camber_rotation = -camber_rotation;
	camber.Rotate( camber_rotation, 1,0,0);

	QUATERNION<Dbl> toe;
	Dbl toe_rotation = suspension[wp].GetToe() * PI_d/180.0;
	if (wp == 0 || wp == 2)
		toe_rotation = -toe_rotation;
	toe.Rotate( toe_rotation, 0,0,1);

	return camber * toe * steer;
}

static QUATERNION <float> GetCubeSideOrientation(int i, const QUATERNION <float> & origorient, std::ostream & error_output)
{
	QUATERNION <float> orient = origorient;

	switch (i)
	{
		case 0:
		orient.Rotate(M_PI*0.5, 0,1,0);
		break;

		case 1:
		orient.Rotate(-M_PI*0.5, 0,1,0);
		break;

		case 2:
		orient.Rotate(M_PI*0.5, 1,0,0);
		break;

		case 3:
		orient.Rotate(-M_PI*0.5, 1,0,0);
		break;

		case 4:
		// orient is already set up for us!
		break;

		case 5:
		orient.Rotate(M_PI, 0,1,0);
		break;

		default:
		error_output << "Reached odd spot while building cubemap orientation. How many sides are in a cube, anyway? " << i << "?" << std::endl;
		assert(0);
		break;
	};

	return orient;
}

//simple hinge (arc) suspension displacement
MATHVECTOR<Dbl,3> CARDYNAMICS::GetLocalWheelPosition(WHEEL_POSITION wp, Dbl displacement_percent) const
{
	//const
	const MATHVECTOR<Dbl,3> & wheelext = wheel[wp].GetExtendedPosition();
	const MATHVECTOR<Dbl,3> & hinge = suspension[wp].GetHinge();
	MATHVECTOR<Dbl,3> relwheelext = wheelext - hinge;
	MATHVECTOR<Dbl,3> up(0,0,1);
	MATHVECTOR<Dbl,3> rotaxis = up.cross ( relwheelext.Normalize() );
	Dbl hingeradius = relwheelext.Magnitude();
	Dbl travel = suspension[wp].GetTravel();
	//const

	Dbl displacement = displacement_percent * travel;
	Dbl displacementradians = displacement / hingeradius;
	QUATERNION<Dbl> hingerotate;
	hingerotate.Rotate ( -displacementradians, rotaxis[0], rotaxis[1], rotaxis[2] );
	MATHVECTOR<Dbl,3> localwheelpos = relwheelext;
	hingerotate.RotateVector ( localwheelpos );
	return localwheelpos + hinge;
}

RENDER_INPUT_SCENE::RENDER_INPUT_SCENE()
:	last_transform_valid(false),
	shaders(false),
	clearcolor(false),
	cleardepth(false),
	orthomode(false),
	contrast(1.0),
	depth_mode(GL_LEQUAL),
	writecolor(true),
	writedepth(true),
	carpainthack(false),
	blendmode(BLENDMODE::DISABLED)
{
	MATHVECTOR <float, 3> front(1,0,0);
	lightposition = front;
	QUATERNION <float> ldir;
				//ldir.Rotate(3.141593*0.4,0,0,1);
				//ldir.Rotate(3.141593*0.5*0.7,1,0,0);
	ldir.Rotate(3.141593*0.5,1,0,0);
	ldir.RotateVector(lightposition);
}

//  Load CAR
//------------------------------------------------------------------------------------------------------
void CarModel::Load(int startId)
{
	//  names for local play
	if (isGhostTrk())    sDispName = TR("#{Track}");
	else if (isGhost())  sDispName = TR("#{Ghost}");
	else if (eType == CT_LOCAL)
		sDispName = TR("#{Player}") + toStr(iIndex+1);
	

	///  load config .car
	std::string pathCar;
	pApp->gui->GetCarPath(&pathCar, 0, 0, sDirname, pApp->mClient);  // force orig for newtorked games
	LoadConfig(pathCar);
	
	
	///  Create CAR (dynamic)
	if (!isGhost())  // ghost has pCar, dont create
	{
		if (startId == -1)  startId = iIndex;
		if (pSet->game.start_order == 1)
		{	//  reverse start order
			int numCars = pApp->mClient ? pApp->mClient->getPeerCount()+1 : pSet->game.local_players;  // networked or splitscreen
			startId = numCars-1 - startId;
		}
		int i = pSet->game.collis_cars ? startId : 0;  // offset when cars collide

		//  start pos
		MATHVECTOR<float,3> pos(0,0,0);  pos = pApp->scn->sc->GetStart(i).first;
		QUATERNION<float> rot;  rot = pApp->scn->sc->GetStart(i).second;
		vStartPos = Vector3(pos[0], pos[2], -pos[1]);
		if (pSet->game.trackreverse)
		{	rot.Rotate(PI_d, 0,0,1);  rot[0] = -rot[0];  rot[1] = -rot[1];  }

		pCar = pGame->LoadCar(pathCar, sDirname, pos, rot, true, false, eType == CT_REMOTE, iIndex);
		bIsCar = !(pCar->dynamics.hover || pCar->dynamics.sphere);

		if (!pCar)  LogO("Error creating car " + sDirname + "  path: " + pathCar);
		else  pCar->pCarM = this;
	}
}

//  newPoses - Get new car pos from game
//  caution: called from GAME, 2nd thread, no Ogre stuff here
/// Todo: move arrow update and ChampionshipAdvance to updatePoses ...
//---------------------------------------------------------------------------------------------------------------
void App::newPoses(float time)  // time only for camera update
{
    if (!pGame || bLoading || pGame->cars.empty() /*|| carPoses.empty() || iCurPoses.empty()*/)
        return;
    PROFILER.beginBlock(".newPos ");

    double rplTime = pGame->timer.GetReplayTime(0);  // from start
    double lapTime = pGame->timer.GetPlayerTime(0);
    double rewTime = pSet->rpl_ghostrewind ? pGame->timer.GetRewindTimeGh(0) : lapTime;

    //  iterate through all car models and set new pos info (from vdrift sim or replay)
    CarModel* carM0 = carModels[0];
    for (int c = 0; c < carModels.size(); ++c)
    {
        CarModel* carM = carModels[c];
        bool bGhost = carM->isGhost();
        CAR* pCar = carM->pCar;

        PosInfo pi;  // new, to fill data


        //  car perf test  logic
        //--------------------------------
        if (bPerfTest && c==0)
            newPerfTest(time);


        //  play  get data from replay / ghost
        ///-----------------------------------------------------------------------
        if (bGhost)
        {   ///>>  track's ghost
            if (carM->isGhostTrk())
            {
                TrackFrame tf;       // par: sec after, 1st lap
                float lap1 = pGame->timer.GetCurrentLap(0) > 0 ? 2.f : 0.f;
                bool ok = ghtrk.GetFrame(rewTime + lap1, &tf);
                //  car
                Axes::toOgre(pi.pos, tf.pos);
                pi.rot = Axes::toOgre(tf.rot);
                pi.braking = tf.brake;
                pi.steer = tf.steer / 127.f;
                //pi.fboost = 0.f;  pi.speed = 0.f;  pi.percent = 0.f;
                //pi.fHitTime = 0.f;  pi.fParIntens = 0.f;  pi.fParVel = 0.f;
                //pi.vHitPos = Vector3::ZERO;  pi.vHitNorm = Vector3::UNIT_Y;

                //  wheels
                //dynamics.SetSteering(state.steer, pGame->GetSteerRange());  //peers can have other game settins..
                for (int w=0; w < carM->numWheels; ++w)
                {
                    MATHVECTOR<float,3> whP = carM->whPos[w];
                    whP[2] += 0.05f;  // up
                    tf.rot.RotateVector(whP);
                    Axes::toOgre(pi.whPos[w], tf.pos + whP);

                    if (w < 2)  // front steer
                    {   float a = (pi.steer * carM->maxangle) * -PI_d/180.f;
                        QUATERNION<float> q;
                        q.Rotate(a, 0,0,1);
                        pi.whRot[w] = Axes::toOgreW(tf.rot * carM->qFixWh[w%2] * q);
                    } else
                        pi.whRot[w] = Axes::toOgreW(tf.rot * carM->qFixWh[w%2]);
                }
            } else  ///>>  ghost
            {
                ReplayFrame2 gf;
                float ti = std::min((float)rewTime, ghplay.GetTimeLength());
                bool ok = ghplay.GetFrame(ti, &gf, 0);
                if (ok)
                    pi.FromRpl2(&gf, 0);

                if (carM->vtype == V_Sphere)
                {   //  weird fix, mini rot
                    pi.carY = Vector3::UNIT_Y;
                    pi.hov_roll = -pi.hov_roll;
                }
            }
        }
        else  ///>>  replay
            if (bRplPlay)
            {
#ifdef DEBUG
                assert(c < frm.size());
#endif
                ReplayFrame2& rf = frm[c];  // frm also used in car.cpp for sounds
                if (c < replay.header.numPlayers)
                {
                    bool ok = replay.GetFrame(rplTime, &rf, c);
                    if (ok)
                    {   pi.FromRpl2(&rf, &pCar->dynamics);
                        pCar->SetPosition(rf.pos, rf.rot);  // for objs hit
                        carM->trackPercent = rf.percent /255.f*100.f;
                    } else
                    {   carM->fCam->First();
                        pGame->timer.RestartReplay(0);  //at end
                    }
                }
//.........这里部分代码省略.........

void CAR::HandleInputs(const std::vector <float> & inputs)
{
	 // ensure that our inputs vector contains exactly one item per input
	assert(inputs.size() == CARINPUT::INVALID);

	// recover from a rollover
	if (inputs[CARINPUT::ROLLOVER_RECOVER])
		dynamics.RolloverRecover();

	// set brakes
	dynamics.SetBrake(inputs[CARINPUT::BRAKE]);
	dynamics.SetHandBrake(inputs[CARINPUT::HANDBRAKE]);

	// do steering
	float steer_value = inputs[CARINPUT::STEER_RIGHT];
	if (std::abs(inputs[CARINPUT::STEER_LEFT]) > std::abs(inputs[CARINPUT::STEER_RIGHT])) //use whichever control is larger
		steer_value = -inputs[CARINPUT::STEER_LEFT];
	dynamics.SetSteering(steer_value);
	last_steer = steer_value;
	QUATERNION<float> steer;
	steer.Rotate(-steer_value * steer_angle_max, 0, 0, 1);
	steer_rotation = steer_orientation * steer;

    // start the engine if requested
	if (inputs[CARINPUT::START_ENGINE])
		dynamics.StartEngine();

	// do shifting
	int gear_change = 0;
	if (inputs[CARINPUT::SHIFT_UP] == 1.0)
		gear_change = 1;
	if (inputs[CARINPUT::SHIFT_DOWN] == 1.0)
		gear_change = -1;
	int cur_gear = dynamics.GetTransmission().GetGear();
	int new_gear = cur_gear + gear_change;

	if (inputs[CARINPUT::REVERSE])
		new_gear = -1;
	if (inputs[CARINPUT::NEUTRAL])
		new_gear = 0;
	if (inputs[CARINPUT::FIRST_GEAR])
		new_gear = 1;
	if (inputs[CARINPUT::SECOND_GEAR])
		new_gear = 2;
	if (inputs[CARINPUT::THIRD_GEAR])
		new_gear = 3;
	if (inputs[CARINPUT::FOURTH_GEAR])
		new_gear = 4;
	if (inputs[CARINPUT::FIFTH_GEAR])
		new_gear = 5;
	if (inputs[CARINPUT::SIXTH_GEAR])
		new_gear = 6;

	applied_brakes = inputs[CARINPUT::BRAKE];

	float throttle = inputs[CARINPUT::THROTTLE];
	float clutch = 1 - inputs[CARINPUT::CLUTCH];
	float nos = inputs[CARINPUT::NOS];

	nos_active = nos > 0;

	dynamics.ShiftGear(new_gear);
	dynamics.SetThrottle(throttle);
	dynamics.SetClutch(clutch);
	dynamics.SetNOS(nos);

	// do driver aid toggles
	if (inputs[CARINPUT::ABS_TOGGLE])
		dynamics.SetABS(!dynamics.GetABSEnabled());

	if (inputs[CARINPUT::TCS_TOGGLE])
		dynamics.SetTCS(!dynamics.GetTCSEnabled());

	// update interior sounds
	if (!psound || !driver_view) return;

	// brake sound
	if (inputs[CARINPUT::BRAKE] > 0 && !brakesound_check)
	{
		if (!psound->GetSourcePlaying(brakesound))
		{
			psound->ResetSource(brakesound);
			psound->SetSourceGain(brakesound, 0.5);
		}
		brakesound_check = true;
	}
	if (inputs[CARINPUT::BRAKE] <= 0)
		brakesound_check = false;

	// handbrake sound
	if (inputs[CARINPUT::HANDBRAKE] > 0 && !handbrakesound_check)
	{
		if (!psound->GetSourcePlaying(handbrakesound))
		{
			psound->ResetSource(handbrakesound);
			psound->SetSourceGain(handbrakesound, 0.5);
		}
		handbrakesound_check = true;
	}
	if (inputs[CARINPUT::HANDBRAKE] <= 0)
//.........这里部分代码省略.........

void RENDER_INPUT_SCENE::Render(GLSTATEMANAGER & glstate, std::ostream & error_output)
{
	if (shaders)
		assert(shader);

	glMatrixMode( GL_PROJECTION );
	glLoadIdentity();
	if (orthomode)
	{
		glOrtho(orthomin[0], orthomax[0], orthomin[1], orthomax[1], orthomin[2], orthomax[2]);
		//std::cout << "ortho near/far: " << orthomin[2] << "/" << orthomax[2] << std::endl;
	}
	else
	{
		gluPerspective( camfov, w/(float)h, 0.1f, lod_far );
	}
	glMatrixMode( GL_MODELVIEW );
	float temp_matrix[16];
	(cam_rotation).GetMatrix4(temp_matrix);
	glLoadMatrixf(temp_matrix);
	glTranslatef(-cam_position[0],-cam_position[1],-cam_position[2]);
	ExtractFrustum(frustum);

	//send information to the shaders
	if (shaders)
	{
		//camera transform goes in texture3
		glActiveTexture(GL_TEXTURE3);
		glMatrixMode(GL_TEXTURE);
		glLoadIdentity();
		(cam_rotation).GetMatrix4(temp_matrix);
		glLoadMatrixf(temp_matrix);
		glTranslatef(-cam_position[0],-cam_position[1],-cam_position[2]);

		//cubemap transform goes in texture2
		glActiveTexture(GL_TEXTURE2);
		glMatrixMode(GL_TEXTURE);
		glLoadIdentity();
		QUATERNION <float> camlook;
		camlook.Rotate(3.141593*0.5,1,0,0);
		camlook.Rotate(-3.141593*0.5,0,0,1);
		QUATERNION <float> cuberotation;
		cuberotation = (-camlook) * (-cam_rotation); //experimentally derived
		(cuberotation).GetMatrix4(temp_matrix);
		//(cam_rotation).GetMatrix4(temp_matrix);
		glLoadMatrixf(temp_matrix);
		//glTranslatef(-cam_position[0],-cam_position[1],-cam_position[2]);
		//glLoadIdentity();

		glActiveTexture(GL_TEXTURE0);
		glMatrixMode(GL_MODELVIEW);

		//send light position to the shaders
		MATHVECTOR <float, 3> lightvec = lightposition;
		(cam_rotation).RotateVector(lightvec);
		//(cuberotation).RotateVector(lightvec);
		shader->Enable();
		shader->UploadActiveShaderParameter3f("lightposition", lightvec[0], lightvec[1], lightvec[2]);
		shader->UploadActiveShaderParameter1f("contrast", contrast);
		/*float lightarray[3];
		for (int i = 0; i < 3; i++)
		lightarray[i] = lightvec[i];
		glLightfv(GL_LIGHT0, GL_POSITION, lightarray);*/

		// if we have no reflection texture supplied, don't touch the TU because
		// someone else may be supplying one
		/*if (reflection && reflection->Loaded())
		{
			glActiveTexture(GL_TEXTURE2);
			reflection->Activate();
			glActiveTexture(GL_TEXTURE0);
		}*/

		/*glActiveTexture(GL_TEXTURE3);
		if (ambient && ambient->Loaded())
		{
			ambient->Activate();
		}
		else
		{
			glBindTexture(GL_TEXTURE_CUBE_MAP,0);
			//assert(0);
		}*/
		glActiveTexture(GL_TEXTURE0);

		PushShadowMatrices();
	}

	//std::cout << "scene: " << std::endl;

	glstate.SetColorMask(writecolor, writealpha);
	glstate.SetDepthMask(writedepth);

	if (clearcolor && cleardepth)
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	else if (clearcolor)
		glClear(GL_COLOR_BUFFER_BIT);
	else if (cleardepth)
		glClear(GL_DEPTH_BUFFER_BIT);

//.........这里部分代码省略.........