rotation.cpp

机甲指挥官2源代码

UnitQuaternion&
	UnitQuaternion::operator=(const EulerAngles &angles)
{
	Check_Pointer(this);
	Check_Object(&angles);

	LinearMatrix4D m;
	m.BuildRotation(angles);
	Check_Object(&m);
	*this = m;
	return *this;
}
//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::operator=(const YawPitchRoll &angles)
{
	LinearMatrix4D	lin_matrix;
	lin_matrix.BuildRotation(angles);
	*this = lin_matrix;
   return *this;
}
//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::operator=(const LinearMatrix4D &matrix)
{
	Check_Pointer(this);
	Check_Object(&matrix);

	//
	//------------------------------------------------------------------------
	// Compute the w component.  If it is close enough to zero, then we have a
	// 180 degree pivot, so figure out the correct axis to rotate around
	//------------------------------------------------------------------------
	//
	w = (1.0f + matrix(0,0) + matrix(1,1) + matrix(2,2)) * 0.25f;
	if (Small_Enough(w,1e-2f))
	{
		Verify(w >= -SMALL);
		if (w<0.0f)
		{
			w = 0.0f;
		}

		//
		//----------------------------------------------------------------
		// Figure out the length of each component of the axis of rotation
		//----------------------------------------------------------------
		//
		Scalar temp = (1.0f + matrix(0,0)) * 0.5f - w;
		Min_Clamp(temp, 0.0f);
		x = Sqrt(temp);
		temp = (1.0f + matrix(1,1)) * 0.5f - w;
		Min_Clamp(temp, 0.0f);
		y = Sqrt(temp);
		temp = (1.0f + matrix(2,2)) * 0.5f - w;
		Min_Clamp(temp, 0.0f);
		z = Sqrt(temp);
		w = Sqrt(w);

		//
		//-------------------------------------------
		// Now figure out the signs of the components
		//-------------------------------------------
		//
		if (matrix(0,1) < matrix(1,0))
		{
			z = -z;
		}
		if (matrix(2,0) < matrix(0,2))
		{
			y = -y;
		}
		if (matrix(1,2) < matrix(2,1))
		{
			x = -x;
		}
	}

	//
	//----------------------------------------------------------
	// Otherwise, determine x, y, and z directly from the matrix
	//----------------------------------------------------------
	//
	else
	{
		Verify(w>0.0f);
		w = Sqrt(w);
		x = (matrix(1,2) - matrix(2,1)) * 0.25f / w;
		y = (matrix(2,0) - matrix(0,2)) * 0.25f / w;
		z = (matrix(0,1) - matrix(1,0)) * 0.25f / w;
	}

	Normalize();
	return *this;
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::operator=(const Vector3D &v)
{
	Check_Pointer(this);
	Check_Object(&v);

	//
	//---------------------------------------------------------------
	// See if there is any rotation to apply to the source quaternion
	//---------------------------------------------------------------
	//
	Scalar rotation = v.GetLength();
	if (Small_Enough(rotation))
	{
		return *this = Identity;
	}

	//
	//---------------------------------------------------------------------
	// Build a quaternion from the delta vector, treating the length as the
	// amount of rotation and the direction of the vector as the axis of
	// rotation
	//---------------------------------------------------------------------
	//
	SinCosPair half_angle;
	half_angle = 0.5f * Radian::Normalize(rotation);
	rotation = half_angle.sine / rotation;
	x = v.x * rotation;
	y = v.y * rotation;
	z = v.z * rotation;
	w = half_angle.cosine;
	Check_Object(this);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
bool
	Stuff::Close_Enough(
		const UnitQuaternion& a1,
		const UnitQuaternion& a2,
		Scalar e
	)
{
	Check_Object(&a1);
	Check_Object(&a2);

	Vector4D v(a1.x-a2.x, a1.y-a2.y, a1.z-a2.z, a1.w-a2.w);
	return Small_Enough(v, e);
}

//
//#############################################################################
//#############################################################################
//
Scalar
	UnitQuaternion::GetAngle()
{
	Check_Object(this);

	Scalar sine_of_half = Sqrt(x*x + y*y + z*z);
	if (Small_Enough(sine_of_half))
	{
		return 0.0f;
	}

	SinCosPair half_angle(sine_of_half, w);
	Radian angle;
	angle = half_angle;

	return angle * 2.0f;
}

//
//#############################################################################
//#############################################################################
//
void
	UnitQuaternion::GetAxis(UnitVector3D *axis)
{
	Check_Object(this);
	Check_Pointer(axis);

	Scalar len = Sqrt(x*x + y*y + z*z);
	if (Small_Enough(len))
	{
		axis->x = 1.0f;
		axis->y = 0.0f;
		axis->z = 0.0f;
	}
	else
	{
		axis->x = x / len;
		axis->y = y / len;
		axis->z = z / len;
	}

	Check_Object(axis);
	return;
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::Multiply(const UnitQuaternion &q2, const UnitQuaternion &q1)
{
	Check_Pointer(this);
	Check_Object(&q1);
	Check_Object(&q2);
	Verify(this != &q1 && this != &q2);

	x = q1.w*q2.x + q2.w*q1.x + q1.y*q2.z - q1.z*q2.y;
	y = q1.w*q2.y + q2.w*q1.y + q1.z*q2.x - q1.x*q2.z;
	z = q1.w*q2.z + q2.w*q1.z + q1.x*q2.y - q1.y*q2.x;
	w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;

	Check_Object(this);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::Multiply(const UnitQuaternion &q, const LinearMatrix4D &m)
{
	Check_Pointer(this);
	Check_Object(&q);
	Check_Object(&m);

	LinearMatrix4D t1;
	t1.BuildRotation(q);
	LinearMatrix4D t2;
	t2.Multiply(t1,m);
	*this = t2;

	Check_Object(this);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::Multiply(
		const UnitQuaternion &q,
		Scalar t
	)
{
	Check_Pointer(this);
	Check_Object(&q);

	//
	//---------------------------------------------------------
	// Figure out the half the angle of rotation and scale that
	//---------------------------------------------------------
	//
	Scalar sine_of_half = Sqrt(q.x*q.x + q.y*q.y + q.z*q.z);
	if (Small_Enough(sine_of_half))
	{
		*this = Identity;
		return *this;
	}

	SinCosPair half_angle(sine_of_half, q.w);
	Radian angle;
	angle = half_angle;
	angle *= t;
	half_angle = angle;

	//
	//-----------------------------------------------------------------
	// Build the scaled quaternion out of the components of the old one
	//-----------------------------------------------------------------
	//
	w = half_angle.cosine;
	sine_of_half = half_angle.sine / sine_of_half;
	x = q.x * sine_of_half;
	y = q.y * sine_of_half;
	z = q.z * sine_of_half;

	Check_Object(this);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::MultiplyScaled(
		const UnitQuaternion &q1,
		const UnitQuaternion &q2,
		Scalar t
	)
{
	Check_Pointer(this);
	Verify(this != &q1);
	Check_Object(&q1);
	Check_Object(&q2);
	Verify(t>=0.0f);

	UnitQuaternion scaled_quat;
	scaled_quat.Multiply(q2, t);
	Multiply(q1, scaled_quat);

	Check_Object(this);
   return *this;
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::Normalize()
{
	

	Scalar t = x*x + y*y + z*z;
	if (t <= 1.0f)
	{
		t = Sqrt(1.0f - t);
		if (w<0.0f)
		{
			x = -x;
			y = -y;
			z = -z;
		}
		w = t;
		TestInstance();
	}
	else
	{
		t = Sqrt(t);
		t = 1.0f/t;
		x *= t;
		y *= t;
		z *= t;
		w = 0.0f;
		TestInstance();
	}
	return *this;
}

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

UnitQuaternion&
	UnitQuaternion::FastNormalize()
{
	if (!UseFastNormalize)
		return Normalize();

	Scalar t = x*x + y*y + z*z;
	if (t <= 1.0f)
	{
		t = SqrtApproximate(1.0f - t);
		if (w<0.0f)
		{
			x = -x;
			y = -y;
			z = -z;
		}
		w = t;
		TestInstance();
	}
	else
	{
		t = SqrtApproximate(t);
		t = 1.0f/t;
		x *= t;
		y *= t;
		z *= t;
		w = 0.0f;
		TestInstance();
	}
	return *this;
}


//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::Subtract(
		const UnitQuaternion &end,
		const UnitQuaternion &start
	)
{
	Check_Pointer(this);
	Check_Object(&start);
	Check_Object(&end);

	UnitQuaternion inverse(start);
	inverse.w = -inverse.w;
	Multiply(inverse, end);
	return Normalize();
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::Subtract(
		const UnitVector3D &end,
		const UnitVector3D &start
	)
{
	Check_Pointer(this);
	Check_Object(&start);
	Check_Object(&end);

	Vector3D
		axis;
	SinCosPair
		delta;
	delta.cosine = start*end;

	//
	//----------------------------------------------------------------------
	// See if the vectors point in the same direction.  If so, return a null
	// rotation
	//----------------------------------------------------------------------
	//
	if (Close_Enough(delta.cosine, 1.0f))
	{
		x = 0.0f;
		y = 0.0f;
		z = 0.0f;
		w = 1.0f;
	}

	//
	//-------------------------------------------------------------------------
	// See if the vectors directly oppose each other.  If so, pick the smallest
	// axis coordinate and generate a vector along it.  Project this onto the
	// base vector and subtract it out, leaving a perpendicular projection.
	// Extend that out to unit length, then set the angle to PI
	//-------------------------------------------------------------------------
	//
	else if (Close_Enough(delta.cosine, -1.0f))
	{
		//
		//---------------------------
		// Pick out the smallest axis
		//---------------------------
		//
		int
			smallest=0;
		Scalar
			value=2.0f;
		for (int i=X_Axis; i<=Z_Axis; ++i)
		{
			if (Abs(start[i]) < value)
			{
				smallest = i;
				value = Abs(start[i]);
			}
		}

		//
		//----------------------------------------
		// Set up a vector along the selected axis
		//----------------------------------------
		//
		axis.x = 0.0f;
		axis.y = 0.0f;
		axis.z = 0.0f;
		axis[smallest] = 1.0f;

		//
		//-------------------------------------------------------------------
		// If the value on that axis wasn't zero, subtract out the projection
		//-------------------------------------------------------------------
		//
		if (!Small_Enough(value))
		{
			Vector3D t;
			t.Multiply(start, start*axis);
			axis.Subtract(axis, t);
			axis.Normalize(axis);
		}

		//
		//----------------------
		// Convert to quaternion
		//----------------------
		//
		x = axis.x;
		y = axis.y;
		z = axis.z;
		w = 0.0f;
	}

	//
	//--------------------------------------------------
	// Otherwise, generate the cross product and unitize
	//--------------------------------------------------
	//
	else
	{
		axis.Cross(start, end);
		delta.sine = axis.GetLength();
		axis /= delta.sine;

		//
		//---------------------------------------------------------------
		// Now compute sine and cosine of half the angle and generate the
		// quaternion
		//---------------------------------------------------------------
		//
		delta.sine = Sqrt((1.0f - delta.cosine)*0.5f);
		x = axis.x * delta.sine;
		y = axis.y * delta.sine;
		z = axis.z * delta.sine;
		w = Sqrt((1.0f + delta.cosine)*0.5f);
	}
	return *this;
}

//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
	UnitQuaternion::Subtract(
		const Vector3D &end,
		const Vector3D &start
	)
{
	Check_Pointer(this);
	Check_Object(&start);
	Check_Object(&end);

	UnitVector3D
		s,e;

	s = start;
	e = end;
	return Subtract(e, s);
}