rotation.cpp

机甲指挥官2源代码

//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::Lerp(
		const EulerAngles& v1,
		const EulerAngles& v2,
		Scalar t
	)
	{
		UnitQuaternion q1;
		q1 = v1;
		UnitQuaternion q2;
		q2 = v2;
		return Lerp(q1,q2,t);
	}

//#############################################################################
//#############################################################################
//
#define SLERP_THRESHOLD (float)0.00001f


UnitQuaternion &UnitQuaternion::Lerp(const UnitQuaternion& p, const UnitQuaternion& q, Scalar t)
{

	Start_Timer(SlerpTime);

	Set_Statistic(SlerpCount, SlerpCount+1);

	Scalar omega,cosom,sinom,sclp,sclq;
	//UnitQuaternion qt;


	//UnitQuaternion q = q_temp;
	//UnitQuaternion p = p_temp;

	cosom = p.x*q.x + p.y*q.y + p.z*q.z + p.w*q.w;


	if ( (1.0f + cosom) > 0.01f)
	{
		// usual case 
	
		
		if ( (1.0f - cosom) > 0.00001f ) 
		{ 
			//usual case 
			omega = Arccos(cosom);
			sinom = Sin(omega);
			
			//SPEW(("jerryeds","omega:%f sinom:%f", omega, sinom));

			sclp = Sin((1.0f - t)*omega) / sinom;
			sclq = Sin(t*omega) / sinom;
		
			//SPEW(("jerryeds", "* %f %f", sclp, sclq));
		}
		else 
		{ 
			// ends very close -- just lerp
			sclp = 1.0f - t;
			sclq = t;

			//SPEW(("jerryeds", "# %f %f", sclp, sclq));
		}


		x = sclp*p.x + sclq*q.x;
		y = sclp*p.y + sclq*q.y;
		z = sclp*p.z + sclq*q.z;
		w = sclp*p.w + sclq*q.w;

		//SPEW(("jerryeds", "r:<%f,%f,%f,%f>",x,y,z,w));
	}
	else 
	{
		//SPEW(("jerryeds","SPECIAL CASE"));
		/* p and q nearly opposite on sphere-- this is a 360 degree
		   rotation, but the axis of rotation is undefined, so
		   slerp really is undefined too.  So this apparently picks 
		   an arbitrary plane of rotation. However, I think this 
		   code is incorrect.
		   */

		//really we want the shortest distance.  They are almost on top of each other.

		UnitQuaternion r;
		r.Subtract(q, p);

		Vector3D scaled_rotation;
		scaled_rotation = r;
		scaled_rotation *= t;
		UnitQuaternion scaled_quat;
		scaled_quat = scaled_rotation;

		Multiply(scaled_quat, p);

	}

	Stop_Timer(SlerpTime);

	return *this;
}

//
//#############################################################################
//#############################################################################
//

UnitQuaternion&
	UnitQuaternion::FastLerp(
		const UnitQuaternion& p, 
		const UnitQuaternion& q, 
		Scalar t
	)
{


	if (!UseFastLerp)
		return Lerp(p,q,t);


	Start_Timer(SlerpTime);

	Set_Statistic(SlerpCount, SlerpCount+1);

	Verify(quaternionFastLerpTableBuilt);

	Scalar cosom,sclp,sclq;
	
	cosom = p.x*q.x + p.y*q.y + p.z*q.z + p.w*q.w;


	if ( (1.0f + cosom) > 0.01f)
	{
		// usual case 
	

		
		if ( (1.0f - cosom) > 0.00001f ) 
		{ 
			//usual case
			
			

			//table_entry = (int)Scaled_Float_To_Bits(cosom, MinCosom, MaxCosom, 10);


			float tabled_float =  cosom - MinCosom;
			int cos_table_entry = Truncate_Float_To_Word(((tabled_float*CosomRangeOverOne) * CosBiggestNumber));
			
			Verify(cos_table_entry >= 0);
			Verify(cos_table_entry <= QuaternionLerpTableSize);


#if 0
			sclp = Sin((1.0f - t)*Omega_Table[cos_table_entry]) * SinomOverOne_Table[cos_table_entry];
			sclq = Sin(t*Omega_Table[cos_table_entry]) * SinomOverOne_Table[cos_table_entry];
	
#else
		
			float difference, percent, lerped_sin;


			tabled_float =  ((1.0f - t)*Omega_Table[cos_table_entry]) - MinSin;
			int sclp_table_entry = Truncate_Float_To_Word(((tabled_float*SinRangeOverOne) * SinBiggestNumber));


			if (!(sclp_table_entry < SinTableSize))
			{
				Max_Clamp(sclp_table_entry, SinTableSize-1);
			}


			Verify(sclp_table_entry >= 0 && sclp_table_entry < SinTableSize);
			difference = tabled_float - (SinIncrement * sclp_table_entry);
			percent = difference / SinIncrement;
			int lerp_to_entry = sclp_table_entry + 1;
			Max_Clamp(lerp_to_entry, SinTableSize-1);
			lerped_sin = Stuff::Lerp(Sin_Table[sclp_table_entry], Sin_Table[lerp_to_entry], percent);
			sclp = lerped_sin * SinomOverOne_Table[cos_table_entry];



			tabled_float =  (t*Omega_Table[cos_table_entry]) - MinSin;
			int sclq_table_entry = Truncate_Float_To_Word(((tabled_float*SinRangeOverOne) * SinBiggestNumber));
			Verify(sclq_table_entry >= 0 && sclq_table_entry < SinTableSize);
			difference = tabled_float - (SinIncrement * sclq_table_entry);
			percent = difference / SinIncrement;
			lerp_to_entry = sclq_table_entry + 1;
			Max_Clamp(lerp_to_entry, SinTableSize-1);
			lerped_sin = Stuff::Lerp(Sin_Table[sclq_table_entry], Sin_Table[lerp_to_entry], percent);
			sclq = lerped_sin * SinomOverOne_Table[cos_table_entry];
#endif
			
		}
		else 
		{ 
			// ends very close -- just lerp
			sclp = 1.0f - t;
			sclq = t;

			
		}


		x = sclp*p.x + sclq*q.x;
		y = sclp*p.y + sclq*q.y;
		z = sclp*p.z + sclq*q.z;
		w = sclp*p.w + sclq*q.w;

		
	}
	else 
	{



		//SPEW(("jerryeds","SPECIAL CASE"));
		/* p and q nearly opposite on sphere-- this is a 360 degree
		   rotation, but the axis of rotation is undefined, so
		   slerp really is undefined too.  So this apparently picks 
		   an arbitrary plane of rotation. However, I think this 
		   code is incorrect.
		   */

		//really we want the shortest distance.  They are almost on top of each other.
	
		UnitQuaternion r;
		r.Subtract(q, p);

		Vector3D scaled_rotation;
		scaled_rotation = r;
		scaled_rotation *= t;
		UnitQuaternion scaled_quat;
		scaled_quat = scaled_rotation;

		Multiply(scaled_quat, p);

		Normalize();

	}


	Stop_Timer(SlerpTime);

	return *this;

}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion
	UnitQuaternion::Squad(
		const UnitQuaternion& p, // start quaternion
		const UnitQuaternion& a, // start tangent quaternion
		const UnitQuaternion& b, // end tangent quaternion
		const UnitQuaternion& q, // end quaternion
		Scalar t
	)
{
	Scalar k = 2.0f * (1.0f - t)*t;
    return(Lerp(Lerp(p,q,t),Lerp(a,b,t),k));
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion
	UnitQuaternion::SquadRev(
		Scalar angle,			// angle of rotation 
		const Point3D& axis,	// the axis of rotation 
		const UnitQuaternion& p,	// start quaternion 
		const UnitQuaternion& a, 	// start tangent quaternion 
		const UnitQuaternion& b, 	// end tangent quaternion 
		const UnitQuaternion& q,	// end quaternion 
		Scalar t 				// parameter, in range [0.0,1.0] 
	)
{
	Scalar s,v;
	Scalar omega = angle*0.5f;
	Scalar nrevs = 0.0f;
	UnitQuaternion r,pp,qq;


	if (omega<Pi-0.0001f) 
	{ 
		r = Squad(p,a,b,q,t); 

		return(r); 
	}

	while (omega > (Pi-0.0001f)) 
	{  
		omega -= Pi; nrevs += (float)1.0; 
	}

	if (omega<0.0f) 
	{
		omega = (float)0.0;
	}

	s = t*angle/Pi;		  /* 2t(omega+Npi)/pi */
	
	if (s < (float)1.0) 
	{
		pp.Orthog(p,axis);
		r = Squad(p,a,pp,pp,s); 	/* in first 90 degrees */
	}
	else 
	{
		if ( ( v = s + 1.0f - 2.0f*(nrevs+(omega/Pi)) ) <=  0.0f)  
		{
			/* middle part, on great circle(p,q) */
			while (s >= 2.0f) 
			{
				s -= 2.0f;
			}
			pp.Orthog(p,axis);
			r = Lerp(p,pp,s);
		}
		else 
		{ 	  /* in last 90 degrees */ 
			qq.Orthog(q,axis);
			qq.Negate();
			r= Squad(qq,qq,b,q,v);
		}
	}

	return(r);
}

//
//#############################################################################
//#############################################################################
//

UnitQuaternion& 
	UnitQuaternion::MakeClosest(const UnitQuaternion& qto)
{
	Scalar dot =  x*qto.x + y*qto.y + z*qto.z+ w*qto.w;
	if (dot<0.0f) 
	{
		x = -x; y = -y; z = -z; w = -w;
	}
	return *this;
}

//
//#############################################################################
//#############################################################################
//
Scalar
	UnitQuaternion::Dot(
		const UnitQuaternion& p, 
		const UnitQuaternion& q
	)
{
	return (q.x*p.x + q.y*p.y + q.z*p.z + q.w*p.w);
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::Inverse(const UnitQuaternion& q)
{
	Scalar l,norminv;
	
	l =	(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
	if (l==0.0f)
	{
		l = 1.0f;
	}
	norminv = 1.0f/l;
	x = -q.x * norminv;
	y = -q.y * norminv;
	z = -q.z * norminv;
	w = q.w * norminv;
	return *this;
}

//
//#############################################################################
//#############################################################################
//

// Ratio of two quaternions: This creates a result quaternion r = p/q, such
// that q*r = p.  (order of multiplication is important)

UnitQuaternion&
	UnitQuaternion::Divide(
		const UnitQuaternion& p, 
		const UnitQuaternion& q
	)
{
	UnitQuaternion i;
	i.Inverse(q);
	Multiply(i, p);
	
	return *this;
}

//
//#############################################################################
//#############################################################################
//

UnitQuaternion&
	UnitQuaternion::LnDif(
		const UnitQuaternion& p, 
		const UnitQuaternion& q
	)
{
	UnitQuaternion r;
	r.Divide(q,p);
	return(LogN(r));
}

//
//#############################################################################
//#############################################################################
//

//  natural logarithm of UNIT quaternion 

UnitQuaternion&
	UnitQuaternion::LogN(
		const UnitQuaternion& q
	)	
{
	Scalar theta,scale;
	scale = Sqrt(q.x*q.x + q.y*q.y + q.z*q.z );
	theta = Arctan(scale,q.w);
	
	if (scale > 0.0f)
	{
		scale = theta/scale;
	}

	x = scale*q.x;
	y = scale*q.y;
	z = scale*q.z;
	w = 0.0f;
	return *this;
}
//
//#############################################################################
//#############################################################################
//


UnitQuaternion& 
	UnitQuaternion::Exp(const UnitQuaternion& q) 
{
	Scalar theta,scale;
	theta = Sqrt(q.x*q.x + q.y*q.y + q.z*q.z );
	scale = 1.0f;
	if (theta >0.0001f) 
	{
		scale = Sin(theta)/theta;
	}
	x = scale*q.x;
	y = scale*q.y;
	z = scale*q.z;
	w = Cos(theta);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion
	UnitQuaternion::CompA(
		const UnitQuaternion& qprev,
		const UnitQuaternion& q,
		const UnitQuaternion& qnext
	)
{
	UnitQuaternion qm,qp,r;

	qm.LnDif(q,qprev);
	qp.LnDif(q,qnext);

	r.x= (qm.x+qp.x) * -0.25f;
	r.y= (qm.y+qp.y) * -0.25f;
	r.z= (qm.z+qp.z) * -0.25f;
	r.w= (qm.w+qp.w) * -0.25f;

	r.Exp(r);
	Multiply(q,r);
	
	return *this;
}

//
//#############################################################################
//#############################################################################
//

UnitQuaternion&
	UnitQuaternion::Orthog(
		const UnitQuaternion& p, 
		const Point3D& axis
	)
{
	Multiply(p, UnitQuaternion(axis.x,axis.y,axis.z,0.0f));
	return *this;
}



//
//#############################################################################
//#############################################################################
//
#if !defined(Spew)
	void
		Spew(
			const char* group,
			const UnitQuaternion &quat
		)
	{
		Check_Object(&quat);
		SPEW((group, "<%f, %f, %f, %f>+", quat.x, quat.y, quat.z, quat.w));
	}
#endif

//
//#############################################################################
//#############################################################################
//
void
	UnitQuaternion::TestInstance() const
{

	Scalar diff = x*x + y*y + z*z + w*w - 1.0f;
	if (!Small_Enough(diff))
	{
		UnitQuaternion q2 = *this;
		q2.Normalize();
		diff = q2.x*q2.x + q2.y*q2.y + q2.z*q2.z + q2.w*q2.w - 1.0f;
		if (Small_Enough(diff))
			STOP(("UnitQuaternion needs normalizing"));
	}
	Verify(Small_Enough(diff));

}