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