/// <summary> /// Creates quaternion representing a rotation around /// an axis by an angle. /// </summary> // todo ISSUE 20090420 andi : sm, rft: What about adding an AxisAngle struct?. public __rot3t__(__v3t__ axis, __ft__ angleInRadians) { var halfAngle = angleInRadians / 2; W = halfAngle.Cos(); V = axis.Normalized * halfAngle.Sin(); }
/// <summary> /// Creates a quaternion representing a rotation from one /// vector into another. /// </summary> public __rot3t__(__v3t__ from, __v3t__ into) { var a = from.Normalized; var b = into.Normalized; var angle = Fun.Clamp(__v3t__.Dot(a, b), -1, 1).Acos(); var angleAbs = angle.Abs(); __v3t__ axis; // some vectors do not normalize to 1.0 -> Vec.Dot = -0.99999999999999989 || -0.99999994f // acos => 3.1415926386886319 or 3.14124632f -> delta of 1e-7 or 1e-3 if (angle < __rotIntoEps__) { axis = a; angle = 0; } else if (__pi__ - angleAbs < __rotIntoEps__) { axis = a.AxisAlignedNormal(); angle = __pi__; } else { axis = __v3t__.Cross(a, b).Normalized; } this = new __rot3t__(axis, angle); }
/// <summary> /// Transforms direction vector v (v.w is presumed 0.0) by quaternion q. /// </summary> public static __v3t__ TransformDir(__rot3t__ q, __v3t__ v) { // first transforming quaternion to __m33t__ is approximately equal in terms of operations ... return(((__m33t__)q).Transform(v)); // ... than direct multiplication ... //QuaternionF r = q.Conjugated() * new QuaternionF(0, v) * q; //return new __v3t__(r.X, r.Y, r.Z); }
public static __v3t__ Transform(this __type__ rot, __v3t__ v) { __ftype__ a = Fun.Cos(rot.Angle); __ftype__ b = Fun.Sin(rot.Angle); return(new __v3t__(a * v.X + -b * v.Y, b * v.X + a * v.Y, v.Z)); }
public static __v3t__ Multiply(__rot2t__ rot, __v3t__ vec) { __ft__ ca = (__ft__)System.Math.Cos(rot.Angle); __ft__ sa = (__ft__)System.Math.Sin(rot.Angle); return(new __v3t__(ca * vec.X + sa * vec.Y, -sa * vec.X + ca * vec.Y, vec.Z)); }
public static __v3t__ InvTransform(this __type__ r, __v3t__ v) { var w = r.X * v.X + r.Y * v.Y + r.Z * v.Z; var x = r.W * v.X - r.Y * v.Z + r.Z * v.Y; var y = r.W * v.Y - r.Z * v.X + r.X * v.Z; var z = r.W * v.Z - r.X * v.Y + r.Y * v.X; return(new __v3t__( w * r.X + x * r.W + y * r.Z - z * r.Y, w * r.Y + y * r.W + z * r.X - x * r.Z, w * r.Z + z * r.W + x * r.Y - y * r.X )); }
// https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another public static __rot3t__ HalfWayVec(__v3t__ from, __v3t__ into) { if (from.Dot(into).ApproximateEquals(-1)) { return(new __rot3t__(0, from.AxisAlignedNormal())); } else { __v3t__ half = Vec.Normalized(from + into); __quatt__ q = new __quatt__(Vec.Dot(from, half), Vec.Cross(from, half)); return(new __rot3t__(q.Normalized)); } }
/// <summary> /// Inverts this quaternion (multiplicative inverse). /// Returns this. /// </summary> public void Invert() { var norm = NormSquared; if (norm == 0) { throw new ArithmeticException("quaternion is not invertible"); } var scale = 1 / norm; W *= scale; V *= -scale; }
public static __rot3t__ HalfWayQuat(__v3t__ from, __v3t__ into) { var d = Vec.Dot(from, into); if (d.ApproximateEquals(-1)) { return(new __rot3t__(0, from.AxisAlignedNormal())); } else { __quatt__ q = new __quatt__(d + 1, Vec.Cross(from, into)); return(new __rot3t__(q.Normalized)); } }
/// <summary> /// Normalizes this quaternion. /// </summary> /// <returns>This.</returns> public void Normalize() { var norm = Norm; if (norm == 0) { this = __rot3t__.Identity; } else { var scale = 1 / norm; W *= scale; V *= scale; } }
/// <summary> /// Create from Rodrigues axis-angle vactor /// </summary> public static __rot3t__ FromAngleAxis(__v3t__ angleAxis) { __ft__ theta2 = angleAxis.LengthSquared; if (theta2 > Constant <__ft__> .PositiveTinyValue) { var theta = Fun.Sqrt(theta2); var thetaHalf = theta / 2; var k = Fun.Sin(thetaHalf) / theta; return(new __rot3t__(Fun.Cos(thetaHalf), k * angleAxis)); } else { return(new __rot3t__(1, 0, 0, 0)); } }
public static void ToAxisAngle(this __type__ r, ref __v3t__ axis, ref __ftype__ angleInRadians) { angleInRadians = 2 * Fun.Acos(r.W); var s = Fun.Sqrt(1 - r.W * r.W); // assuming quaternion normalised then w is less than 1, so term always positive. if (s < 0.001) { // test to avoid divide by zero, s is always positive due to sqrt // if s close to zero then direction of axis not important axis.X = r.X; // if it is important that axis is normalised then replace with x=1; y=z=0; axis.Y = r.Y; axis.Z = r.Z; } else { axis.X = r.X / s; // normalise axis axis.Y = r.Y / s; axis.Z = r.Z / s; } }
public static __rot3t__ Original(__v3t__ from, __v3t__ into) { var angle = from.AngleBetween(into); //# var rotIntoEps = isDouble ? "1e-15" : "1e-6f"; if (angle < __rotIntoEps__) { return(__rot3t__.Identity); } else if (__pi__ - angle < __rotIntoEps__) { return(new __rot3t__(0, from.AxisAlignedNormal())); } else { __v3t__ axis = Vec.Cross(from, into).Normalized; return(__rot3t__.Rotation(axis, angle)); } }
/// <summary> /// Converts this Rotation to the axis angle representation. /// </summary> /// <param name="axis">Output of normalized axis of rotation.</param> /// <param name="angleInRadians">Output of angle of rotation about axis (Right Hand Rule).</param> public void ToAxisAngle(ref __v3t__ axis, ref __ft__ angleInRadians) { if (!Fun.ApproximateEquals(NormSquared, 1, 0.001)) { throw new ArgumentException("Quaternion needs to be normalized to represent a rotation."); } angleInRadians = 2 * (__ft__)System.Math.Acos(W); var s = (__ft__)System.Math.Sqrt(1 - W * W); // assuming quaternion normalised then w is less than 1, so term always positive. if (s < 0.001) { // test to avoid divide by zero, s is always positive due to sqrt // if s close to zero then direction of axis not important axis.X = X; // if it is important that axis is normalised then replace with x=1; y=z=0; axis.Y = Y; axis.Z = Z; } else { axis.X = X / s; // normalise axis axis.Y = Y / s; axis.Z = Z / s; } }
/// <summary> /// Transforms direction vector v (v.w is presumed 0.0) by the inverse of this rigid transformation. /// Actually, only the rotation is used. /// </summary> public __v3t__ InvTransformDir(__v3t__ v) { return(InvTransformDir(this, v)); }
/// <summary> /// Transforms direction vector v (v.w is presumed 0.0) by this rigid transformation. /// Actually, only the rotation is used. /// </summary> public __v3t__ TransformDir(__v3t__ v) { return(TransformDir(this, v)); }
/// <summary> /// Transforms point p (p.w is presumed 1.0) by the inverse of the rigid transformation r. /// </summary> public static __v3t__ InvTransformPos(__e3t__ r, __v3t__ p) { return(r.Rot.InvTransformPos(p - r.Trans)); }
/// <summary> /// Transforms direction vector v (v.w is presumed 0.0) by the inverse of the rigid transformation r. /// Actually, only the rotation is used. /// </summary> public static __v3t__ InvTransformDir(__e3t__ r, __v3t__ v) { return(r.Rot.InvTransformDir(v)); }
/// <summary> /// Transforms point p (p.w is presumed 1.0) by rigid transformation r. /// </summary> public static __v3t__ TransformPos(__e3t__ r, __v3t__ p) { return(r.Rot.TransformPos(p) + r.Trans); }
/// <summary> /// Creates quaternion (w, (x, y, z)). /// </summary> // todo ISSUE 20090420 andi : sm, rft: Add asserts for unit-length constraint public __rot3t__(__ft__ w, __ft__ x, __ft__ y, __ft__ z) { W = w; V = new __v3t__(x, y, z); }
/// <summary> /// Transforms point p (p.w is presumed 1.0) by this quaternion. /// For quaternions, this method is equivalent to TransformDir, and /// is made available only to provide a consistent set of operations /// for all transforms. /// </summary> public __v3t__ TransformPos(__v3t__ p) { return(TransformDir(this, p)); }
/// <summary> /// WARNING: UNTESTED!!! /// </summary> public static __rot3t__ FromFrame(__v3t__ x, __v3t__ y, __v3t__ z) { return(From__m33t__(__m33t__.FromCols(x, y, z))); }
/// <summary> /// Creates quaternion from array starting at specified index. /// (w = a[start], (x = a[start+1], y = a[start+2], z = a[start+3])). /// </summary> public __rot3t__(__ft__[] a, int start) { W = a[start]; V = new __v3t__(a[start + 1], a[start + 2], a[start + 3]); }
/// <summary> /// Creates quaternion (w, (v.x, v.y, v.z)). /// </summary> public __rot3t__(__ft__ w, __v3t__ v) { W = w; V = v; }
/// <summary> /// Creates a rigid transformation from a rotation <paramref name="rot"/> and a (subsequent) translation <paramref name="trans"/>. /// </summary> public __e3t__(__r3t__ rot, __v3t__ trans) { Rot = rot; Trans = trans; }
/// <summary> /// Transforms point p (p.w is presumed 1.0) by the inverse of this rigid transformation. /// </summary> public __v3t__ InvTransformPos(__v3t__ p) { return(InvTransformPos(this, p)); }
/// <summary> /// Inverts this rigid transformation (multiplicative inverse). /// this = [Rot^T,-Rot^T Trans] /// </summary> public void Invert() { Rot.Invert(); Trans = -Rot.TransformDir(Trans); }
/// <summary> /// Conjugates this quaternion. Returns this. /// For normalized rotation-quaternions this is the same as Invert(). /// </summary> public void Conjugate() { V = -V; }
/// <summary> /// Creates a rigid transformation from a rotation matrix <paramref name="rot"/> and a (subsequent) translation <paramref name="trans"/>. /// </summary> public __e3t__(M3__s3f__ rot, __v3t__ trans, __ft__ epsilon = __eps__) { Rot = __r3t__.FromM3__s3f__(rot, epsilon); Trans = trans; }
/// <summary> /// Creates quaternion from array. /// (w = a[0], (x = a[1], y = a[2], z = a[3])). /// </summary> public __rot3t__(__ft__[] a) { W = a[0]; V = new __v3t__(a[1], a[2], a[3]); }