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