C++ QUATERNION类代码示例

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

VERTEX QUATERNION::RotateVec(VERTEX vec)
{
	QUATERNION dirconj;
	
	QUATERNION dir;
	dir.w = w;
	dir.x = x;
	dir.y = y;
	dir.z = z;

	dirconj = dir.ReturnConjugate();
	QUATERNION qtemp;
	qtemp.w = 0;
	qtemp.x = vec.x;
	qtemp.y = vec.y;
	qtemp.z = vec.z;
	QUATERNION qout;
	qout = dir * qtemp * dirconj;
	
	VERTEX vout;
	
	vout.x += qout.x;
	vout.y += qout.y;
	vout.z += qout.z;
	
	return vout;
}

//i was getting angry because the quats couldn't do this, so i added it....
void QUATERNION::Rotate(float a, float ax, float ay, float az)
{
	//NOTE: ax, ay, az is assumed to be a UNIT VECTOR!!
	
	QUATERNION qtemp;
	qtemp.SetAxisAngle(a, ax, ay, az);
	PostMultiply(qtemp);
}

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

// V 经过 R 变换后为 V', 函数返回 V'
// 要求 R 先归一化
PNT  Ti_Rotation::VectorTransform(const PNT& V)const
{
	Ti_Rotation R = *this;
	if( (R.m_Angle>-DELTA_ROT) && (R.m_Angle<DELTA_ROT) ) 
		return V;
	R.m_Axis.Normalize();
	QUATERNION Q = RotationToQuaternion(R);
	Q.QuaternionNormalize();
	return Q.VectorTransform(V);
}

void CAMERA::PutTransformInto(GLdouble *matrix16)
{		
	int i;
	float transform_matrix[16];
	QUATERNION qtemp;
	qtemp = dir_coupled.ReturnConjugate();
	qtemp.PostMultiply(dir);
	qtemp.GetMat(transform_matrix);
	
	for (i=0;i<16;i++)
	{
		matrix16[i] = transform_matrix[i];
	}
}

void CAMERA_MOUNT::Update(const MATHVECTOR <float, 3> & newpos, const QUATERNION <float> & newdir, float dt)
{
	rotation = newdir * offsetrot;

	MATHVECTOR <float, 3> pos = offset;
	newdir.RotateVector(pos);
	pos = pos + newpos;

	MATHVECTOR <float, 3> vel = pos - position;
	effect = (vel.Magnitude() - 0.02) / 0.04;
	if (effect < 0) effect = 0;
	else if (effect > 1) effect = 1;

	float bumpdiff = randgen.Get();
	float power = pow(bumpdiff, 32);
	if (power < 0) power = 0;
	else if (power > 0.2) power = 0.2;
	float veleffect = std::min(pow(vel.Magnitude() * ( 2.0 - stiffness), 3.0), 1.0);
	float bumpimpulse = power * 130.0 * veleffect;

	float k = 800.0 + stiffness * 800.0 * 4.0;
	float c = 2.0 * std::sqrt(k * mass) * 0.35;
	MATHVECTOR <float, 3> bumpforce = direction::Up * bumpimpulse;
	MATHVECTOR <float, 3> springforce = -displacement * k;
	MATHVECTOR <float, 3> damperforce = -velocity * c;
	
	velocity = velocity + (springforce + damperforce + bumpforce) * dt;
	displacement = displacement + velocity * dt;

	UpdatePosition(pos);
}

void CAMERA_MOUNT::Reset(const MATHVECTOR <float, 3> & newpos, const QUATERNION <float> & newquat)
{
	MATHVECTOR <float, 3> pos = offset;
	newquat.RotateVector(pos);
	rotation = newquat * offsetrot;
	displacement.Set(0, 0, 0);
	velocity.Set(0, 0, 0);
	UpdatePosition(pos + newpos);
}

Ti_Rotation  Ti_Rotation::operator * (const  Ti_Rotation& R2)const
{
     Ti_Rotation R1 = *this;
       if( (R1.m_Angle>-DELTA_ROT) && (R1.m_Angle<DELTA_ROT) )
	{
		return R2;
	}
	else if( (R2.m_Angle>-DELTA_ROT) && (R2.m_Angle<DELTA_ROT) )
	{
		return R1;
	}
	
	// 原始角度不为 0
	Ti_Rotation   R; 
	QUATERNION Q1= RotationToQuaternion(R1);
	QUATERNION Q2 =RotationToQuaternion(R2);
	QUATERNION Q = Q1*Q2; // 注意顺序
	R=Q.QuaternionToRotation();
	return R;
}

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

void CAMERA_CHASE::Update(const MATHVECTOR <float, 3> & newfocus, const QUATERNION <float> & focus_facing, float dt)
{
	focus = newfocus;
	MATHVECTOR <float, 3> view_offset = offset;
	focus_facing.RotateVector(view_offset);
	MATHVECTOR <float, 3> target_position = focus + view_offset;

	float posblend = 10.0 * dt;
	if (posblend > 1.0) posblend = 1.0;
	if (!posblend_on) posblend = 1.0;
	position = position * (1.0 - posblend) + target_position * posblend;
	rotation = LookAt(position, focus, direction::Up);
}

QUATERNION QUATERNION::Slerp(QUATERNION quat2, float t)
{
	QUATERNION qout, p1, p2;
	float theta = GetAngleBetween(quat2);
	
	//qout = (*this).MultScalar(sin((1.0f-t)*theta)).Add(quat2.MultScalar(sin(t*theta))).MultScalar(1.0f/sin(theta));
	
	p1 = (*this).MultScalar((sin((1.0f-t)*theta))/sin(theta));
	p2 = quat2.MultScalar(sin(theta*t)/sinh(theta));
	qout = p1.Add(p2);
	
	
	return qout;
}

float QUATERNION::GetAngleBetween(QUATERNION quat2)
{
	//establish a forward vector
	VERTEX forward;
	forward.x = 0;
	forward.y = 0;
	forward.z = 1;
	
	//create vectors for quats
	VERTEX vec1, vec2;
	vec1 = RotateVec(forward);
	vec2 = quat2.RotateVec(forward);
	
	//return the angle between the vectors
	return acos(vec1.dot(vec2));
}