template <class T> bool
Small_Enough(const Vector2DOf<T> &v,Scalar e)
{
Check_Object(&v);
return
Small_Enough(static_cast<Scalar>(v.x),e)
&& Small_Enough(static_cast<Scalar>(v.y),e);
}
inline float
Arctan(
float y,
float x
)
{
Verify(!Small_Enough(y) || !Small_Enough(x));
return static_cast<float>(atan2(y, x));
}
Vector3D&
Divide(
const Vector3D& v1,
const Vector3D& v2
)
{
Check_Pointer(this); Check_Object(&v1); Check_Object(&v2);
Verify(!Small_Enough(v2.x)); Verify(!Small_Enough(v2.y));
Verify(!Small_Enough(v2.z));
x = v1.x/v2.x; y = v1.y/v2.y; z = v1.z/v2.z; return *this;
}
Vector2DOf<T>&
Divide(
const Vector2DOf<T>& v1,
const Vector2DOf<T>& v2
)
{
Check_Object(this); Check_Object(&v1); Check_Object(&v2);
Verify(!Small_Enough(static_cast<Scalar>(v2.x)));
Verify(!Small_Enough(static_cast<Scalar>(v2.y)));
x = v1.x / v2.x; y = v1.y / v2.y; return *this;
}
//
//#############################################################################
//#############################################################################
//
bool
Stuff::Small_Enough(
const YawPitchRoll& angles,
Scalar e
)
{
Check_Object(&angles);
return
Small_Enough(angles.pitch,e)
&& Small_Enough(angles.yaw,e)
&& Small_Enough(angles.roll,e);
}
Vector2DOf<T>&
Normalize(const Vector2DOf<T> &v)
{
Check_Pointer(this); Check_Object(&v);
Scalar len = v.GetLength(); Verify(!Small_Enough(len));
x = v.x/len; y = v.y/len; return *this;
}
//
//#############################################################################
//#############################################################################
//
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));
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Ray3D&
Ray3D::SetDirection(const Vector3D &vector)
{
Check_Pointer(this);
Check_Object(&vector);
//
//---------------------------------------
// Make sure length of vector is non-zero
//---------------------------------------
//
Scalar length = vector.GetLength();
Verify(!Small_Enough(length));
length = 1.0f / length;
//
//----------------------------------------------
// Normalize the vector and put it into the line
//----------------------------------------------
//
direction.x = vector.x*length;
direction.y = vector.y*length;
direction.z = vector.z*length;
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;
}
Scalar
CalculateZ(Scalar x, Scalar y)
{
Check_Object(this); Verify(!Small_Enough(normal.z));
Scalar result = (offset - x*normal.x - y*normal.y) / normal.z;
return result;
}
Scalar
CalculateY(Scalar x, Scalar z)
{
Check_Object(this); Verify(!Small_Enough(normal.y));
Scalar result = (offset - x*normal.x - z*normal.z) / normal.y;
return result;
}
//
// Equation solutions
//
Scalar
CalculateX(Scalar y, Scalar z)
{
Check_Object(this); Verify(!Small_Enough(normal.x));
Scalar result = (offset - y*normal.y - z*normal.z) / normal.x;
return result;
}
Vector3D&
Normalize(const Vector3D &v)
{
Check_Object(this); Check_Object(&v);
Scalar len = v.GetLength();
Verify(!Small_Enough(len)); len = 1.0f / len;
x = v.x*len; y = v.y*len; z = v.z*len; return *this;
}
//
//#############################################################################
//#############################################################################
//
YawPitchRange&
YawPitchRange::operator=(const Vector3D &vector)
{
Check_Pointer(this);
Check_Object(&vector);
//
//------------------------------------------------------------------------
// See if we have a zero length vector. If so, convert it to the identity
//------------------------------------------------------------------------
//
Verify(
Vector3D::Forward.z == 1.0f && Vector3D::Left.x == 1.0f && Vector3D::Up.y == 1.0f
);
Scalar sub_range = vector.x*vector.x + vector.z*vector.z;
range = Sqrt(sub_range + vector.y*vector.y);
if (Small_Enough(range))
{
yaw = 0.0f;
pitch = 0.0f;
}
else
{
//
//---------------------------------------------------------------------
// Isolate the yaw element. If the vector is vertical, yaw will simply
// be zero. If not, the yaw will indicate counter-clockwise deviation
// from the negative Z axis
//---------------------------------------------------------------------
//
sub_range = Sqrt(sub_range);
if (Small_Enough(sub_range))
{
yaw = 0.0f;
pitch = (vector.y > 0.0f) ? -Pi_Over_2 : Pi_Over_2;
}
else
{
yaw = Arctan(vector.x, vector.z);
pitch = -Arctan(vector.y, sub_range);
}
}
return *this;
}
Vector2DOf<T>&
Divide(
const Vector2DOf<T>& v,
T scale
)
{
Check_Object(this); Check_Object(&v);
Verify(!Small_Enough(static_cast<Scalar>(scale)));
x = v.x / scale; y = v.y / scale; return *this;
}
Vector3D&
Divide(
const Vector3D& v,
Scalar scale
)
{
Check_Pointer(this); Check_Object(&v);
Verify(!Small_Enough(scale));
scale = 1.0f / scale;
x = v.x*scale; y = v.y*scale; z = v.z*scale; return *this;
}
//
//###########################################################################
//###########################################################################
//
bool
Stuff::Close_Enough(
const Vector3D &V1,
const Vector3D &V2,
Scalar e
)
{
Check_Object(&V1);
Check_Object(&V2);
Vector3D v(V1.x-V2.x,V1.y-V2.y,V1.z-V2.z);
return Small_Enough(v, e);
}
//
//#############################################################################
//#############################################################################
//
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);
}
//
//###########################################################################
//###########################################################################
//
Point3D&
Point3D::operator=(const Vector4D& v)
{
Check_Pointer(this);
Check_Object(&v);
Verify(!Small_Enough(v.w));
Scalar scale = 1.0f / v.w;
x = v.x*scale;
y = v.y*scale;
z = v.z*scale;
return *this;
}
//
//#############################################################################
//#############################################################################
//
Scalar
Stuff::Find_Closest_Approach(
const Point3D& origin1,
const Vector3D& velocity1,
Point3D *result1,
const Point3D& origin2,
const Vector3D& velocity2,
Point3D *result2,
Scalar *time,
bool *constant
)
{
Vector3D a,b;
a.Subtract(origin1, origin2);
b.Subtract(velocity1, velocity2);
//
//--------------------------------------------------------------------
// If the velocities are identical, any point will do for the test, so
// simply return the difference between the starting points
//--------------------------------------------------------------------
//
Scalar d = b.GetLengthSquared();
if (Small_Enough(d))
{
*constant = true;
d = a.GetLength();
return d;
}
//
//-------------------------------------------------------------------------
// The velocities are not parallel, so figure out when the closest approach
// is via the derivative
//-------------------------------------------------------------------------
//
*constant = false;
*time = (a * b) / -d;
//
//------------------------------------------------------
// Now, plot the resultant points of both line equations
//------------------------------------------------------
//
Vector3D closest;
closest.AddScaled(a, b, *time);
result1->AddScaled(origin1, velocity1, *time);
result2->AddScaled(origin2, velocity2, *time);
d = closest.GetLength();
return d;
}
//
//#############################################################################
//#############################################################################
//
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;
}
//
//#############################################################################
//#############################################################################
//
Scalar
Ray3D::GetDistanceTo(
const Plane &plane,
Scalar *product
) const
{
Check_Object(this);
Check_Object(&plane);
Check_Pointer(product);
*product = direction * plane.normal;
if (Small_Enough(*product))
{
return -1.0f;
}
Scalar result = -plane.GetDistanceTo(origin) / *product;
return result;
}
//
//#############################################################################
//#############################################################################
//
bool
SinCosPair::TestClass()
{
SPEW((GROUP_STUFF_TEST, "Starting SinCos test..."));
Radian
s,r(Pi_Over_2);
SinCosPair a;
a = r;
Test_Assumption(Close_Enough(a.sine,1.0f));
Test_Assumption(Small_Enough(a.cosine));
s = a;
Test_Assumption(s == r);
return true;
}
//
//#############################################################################
//#############################################################################
//
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;
}
//
//###########################################################################
//###########################################################################
//
Vector3D&
Vector3D::operator=(const UnitQuaternion &q)
{
Check_Pointer(this);
Check_Object(&q);
Scalar sine_of_half = Sqrt(q.x*q.x + q.y*q.y + q.z*q.z);
if (Small_Enough(sine_of_half))
{
return *this = Identity;
}
SinCosPair half_angle(sine_of_half, q.w);
Radian angle;
angle = half_angle;
Scalar len = angle * 2.0f / sine_of_half;
x = q.x * len;
y = q.y * len;
z = q.z * len;
return *this;
}
//
//#############################################################################
//#############################################################################
//
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;
}
bool
operator!() const
{return Small_Enough(*this,SMALL);}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
LinearMatrix4D::AlignLocalAxisToWorldVector(
const Vector3D &world_target,
int pointing_axis,
int rotating_axis,
int minor_axis
)
{
Check_Object(this);
Check_Object(&world_target);
Verify(static_cast<unsigned>(pointing_axis) <= Z_Axis);
Verify(static_cast<unsigned>(rotating_axis) <= Z_Axis);
Verify(rotating_axis != pointing_axis);
//
//------------------------------------------------------------------
// These are the variables that the alignment algorithm must fill in
//------------------------------------------------------------------
//
UnitVector3D
rotation_vector,
pointing_vector,
minor_vector;
//
//------------------------------------------------------------------
// Extract the current target axis direction, then cross it with the
// plane target to find the minor axis direction (unsigned)
//------------------------------------------------------------------
//
if (Small_Enough(world_target.GetLengthSquared()))
return;
rotation_vector.x = (*this)(rotating_axis, X_Axis);
rotation_vector.y = (*this)(rotating_axis, Y_Axis);
rotation_vector.z = (*this)(rotating_axis, Z_Axis);
Check_Object(&rotation_vector);
Vector3D temp;
temp.Cross(rotation_vector, world_target);
//
//----------------------------------------------------------------------
// First check to see if we are rotating around a frozen axis. If so,
// if the axes specified are in the right-handed configuration, simply
// generate the new pointing axis values, otherwise negate the minor
// axis and generate the pointing vector appropriately
//----------------------------------------------------------------------
//
if (minor_axis == -1)
{
minor_axis = 3 - pointing_axis - rotating_axis;
minor_vector.Normalize(temp);
if ((rotating_axis+1)%3 == pointing_axis)
pointing_vector.Vector3D::Cross(minor_vector, rotation_vector);
else
{
minor_vector.Vector3D::Negate(minor_vector);
pointing_vector.Vector3D::Cross(rotation_vector, minor_vector);
}
Check_Object(&pointing_vector);
}
//
//------------------------------------------------------------------------
// The next case to check is non-frozen rotation. In this case, maximum
// effort is taken to preserve the rotating matrix, but it will be rotated
// around the minor axis so that the pointing axis is exactly aligned with
// the target vector
//------------------------------------------------------------------------
//
else
{
//
//--------------------------------------------------------------------
// If the resultant vector is zero, it means the rotating axis is
// parallel to the target vector, and thus a correct orthogonal set of
// axis vectors can already be found in the matrix
//--------------------------------------------------------------------
//
Verify(minor_axis == 3 - pointing_axis - rotating_axis);
if (Small_Enough(temp.GetLengthSquared()))
{
if (world_target*rotation_vector > 0.0f)
{
pointing_vector.x = (*this)(rotating_axis, X_Axis);
pointing_vector.y = (*this)(rotating_axis, Y_Axis);
pointing_vector.z = (*this)(rotating_axis, Z_Axis);
rotation_vector.x = -(*this)(pointing_axis, X_Axis);
rotation_vector.y = -(*this)(pointing_axis, Y_Axis);
rotation_vector.z = -(*this)(pointing_axis, Z_Axis);
}
else
{
pointing_vector.x = -(*this)(rotating_axis, X_Axis);
pointing_vector.y = -(*this)(rotating_axis, Y_Axis);
pointing_vector.z = -(*this)(rotating_axis, Z_Axis);
rotation_vector.x = (*this)(pointing_axis, X_Axis);
rotation_vector.y = (*this)(pointing_axis, Y_Axis);
rotation_vector.z = (*this)(pointing_axis, Z_Axis);
}
minor_vector.x = (*this)(minor_axis, X_Axis);
minor_vector.y = (*this)(minor_axis, Y_Axis);
minor_vector.z = (*this)(minor_axis, Z_Axis);
}
//
//---------------------------------------------------------------------
// We have a non-trivial minor vector, so use it to generate the real
// minor axis, then calculate the new rotation axis. If the axes
// specified are in the right-handed configuration, simply generate the
// new pointing axis values, otherwise negate the minor axis and
// generate the pointing vector appropriately
//---------------------------------------------------------------------
//
else
{
pointing_vector.Normalize(world_target);
minor_vector.Normalize(temp);
if ((rotating_axis+1)%3 == pointing_axis)
rotation_vector.Vector3D::Cross(pointing_vector, minor_vector);
else
{
minor_vector.Vector3D::Negate(minor_vector);
rotation_vector.Vector3D::Cross(minor_vector, pointing_vector);
}
Check_Object(&rotation_vector);
}
}
//
//------------------------------------------------
// Now stuff the unit vectors back into the matrix
//------------------------------------------------
//
Check_Object(&pointing_vector);
(*this)(pointing_axis, X_Axis) = pointing_vector.x;
(*this)(pointing_axis, Y_Axis) = pointing_vector.y;
(*this)(pointing_axis, Z_Axis) = pointing_vector.z;
Check_Object(&rotation_vector);
(*this)(rotating_axis, X_Axis) = rotation_vector.x;
(*this)(rotating_axis, Y_Axis) = rotation_vector.y;
(*this)(rotating_axis, Z_Axis) = rotation_vector.z;
Check_Object(&minor_vector);
(*this)(minor_axis, X_Axis) = minor_vector.x;
(*this)(minor_axis, Y_Axis) = minor_vector.y;
(*this)(minor_axis, Z_Axis) = minor_vector.z;
Check_Object(this);
}
//
//#############################################################################
//#############################################################################
//
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::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;
}
