/// <summary>
/// Given two vectors, this method outputs two new orthogonal vectors, where the first one is
/// parallel to the original (although normalized), and the second one is perpendicular to the
/// first, maintaining the orientation of the first one.
/// Returns false if operation could not be processed (both vectors have the same direction)
/// </summary>
/// <param name="X"></param>
/// <param name="Y"></param>
/// <param name="nX"></param>
/// <param name="nY"></param>
public static bool Orthogonalize(Vector X, Vector Y, out Vector nX, out Vector nY, out Vector nZ)
{
// Some sanity
int dir = Vector.CompareDirections(X, Y);
if (dir == 1 || dir == 3)
{
Console.WriteLine("Cannot orthogonalize vectors with the same direction");
nX = null;
nY = null;
nZ = null;
return(false);
}
nX = new Vector(X); // shallow copy
nX.Normalize();
nZ = Vector.CrossProduct(nX, Y);
nZ.Normalize();
nY = Vector.CrossProduct(nZ, nX);
return(true);
}
/// <summary>
/// Is this rotation equivalent to a given one?
/// Equivalence is defined as rotations around vectors sharing the same axis (including opposite directions)
/// and an angle with the same modulated equivalence. This in turn means the same spatial orientation after transformation.
/// For example, the following rotations are equivalent:
/// [0, 0, 1, 315]
/// [0, 0, 1, 675] (one additional turn, same angle)
/// [0, 0, 1, -45] (negative rotation, same angle)
/// [0, 0, -1, 45] (flipped axis, same angle)
/// [0, 0, 10, 315] (same axis and angle, longer vector. note non-unit vectors are not allowed in this AA representation)
///
/// Also, these are equivalent:
/// [0, 0, 0, 0]
/// [0, 0, 1, 720]
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public bool IsEquivalent(AxisAngle other)
{
// Sanity checks
if (this.IsZero())
{
return(Math.Abs(other.Angle % 360) < EPSILON2);
}
else if (other.IsZero())
{
return(Math.Abs(this.Angle % 360) < EPSILON2);
}
//Vector v1 = new Vector(this.X, this.Y, this.Z),
//v2 = new Vector(axisAngle.X, axisAngle.Y, axisAngle.Z);
//int directions = Vector.CompareDirections(v1, v2);
int directions = Vector.CompareDirections(this, other);
// If axes are not parallel, they are not equivalent
if (directions == 0 || directions == 2)
{
return(false);
}
// Bring all angles to [0, 360]
double a1 = this.Angle;
while (a1 < 0)
{
a1 += 360;
}
a1 %= 360;
if (Math.Abs(a1 - 360) < EPSILON2)
{
a1 = 0;
}
double a2 = other.Angle;
while (a2 < 0)
{
a2 += 360;
}
a2 %= 360;
if (Math.Abs(a2 - 360) < EPSILON2)
{
a2 = 0;
}
// If the vectors have the same direction, angles should be module of each other.
if (directions == 1)
{
return(Math.Abs(a1 - a2) < EPSILON2);
}
// If opposite directions, they should add up to 360 degs.
if (directions == 3)
{
return(Math.Abs(a1 + a2 - 360) < EPSILON2);
}
return(false); // if here, something went wrong
}