/// <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); }
// [todo ISSUE 20090225 andi : andi] Wir sollten auch folgendes beruecksichtigen -q == q, weil es die selbe rotation definiert. // [todo ISSUE 20090427 andi : andi] use an angle-tolerance // [todo ISSUE 20090427 andi : andi] add __rot3t__.ApproxEqual(__rot3t__ other); public static bool ApproxEqual(__rot3t__ r0, __rot3t__ r1, __ft__ tolerance) { return((r0.W - r1.W).Abs() <= tolerance && (r0.X - r1.X).Abs() <= tolerance && (r0.Y - r1.Y).Abs() <= tolerance && (r0.Z - r1.Z).Abs() <= tolerance); }
// todo andi { public static __rot3t__ Divide(__ft__ s, __rot3t__ q) { return(new __rot3t__( s / q.W, s / q.X, s / q.Y, s / q.Z )); }
/// <summary> /// Creates quaternion from euler angles [yaw, pitch, roll]. /// </summary> /// <param name="yawInRadians">Rotation around X</param> /// <param name="pitchInRadians">Rotation around Y</param> /// <param name="rollInRadians">Rotation around Z</param> public __rot3t__(__ft__ yawInRadians, __ft__ pitchInRadians, __ft__ rollInRadians) { var qx = new __rot3t__(__v3t__.XAxis, yawInRadians); var qy = new __rot3t__(__v3t__.YAxis, pitchInRadians); var qz = new __rot3t__(__v3t__.ZAxis, rollInRadians); this = qz * qy * qx; }
/// <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); }
// todo andi } /// <summary> /// Multiplies 2 quaternions. /// This concatenates the two rotations into a single one. /// Attention: Multiplication is NOT commutative! /// </summary> public static __rot3t__ Multiply(__rot3t__ a, __rot3t__ b) { return(new __rot3t__( a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z, a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y, a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z, a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X )); }
// [todo ISSUE 20090225 andi : andi> // Investigate if we can use power to "scale" the rotation (=enlarging the angle of rotation by a factor.) // If so, reenable the Log and Exp methods if they are useful then. // <] #if false /// <summary> /// Calculates the logarithm of the quaternion. /// </summary> public static __rot3t__ Log(__rot3t__ a) { var result = __rot3t__.Zero; if (a.W.Abs() < 1) { var angle = a.W.Acos(); var sin = angle.Sin(); result.V = (sin.Abs() >= 0) ? (a.V * (angle / sin)) : a.V; } return(result); }
/// <summary> /// Calculates exponent of the quaternion. /// </summary> public __rot3t__ Exp(__rot3t__ a) { var result = __rot3t__.Zero; var angle = (a.X * a.X + a.Y * a.Y + a.Z * a.Z).Sqrt(); var sin = angle.Sin(); if (sin.Abs() > 0) { var coeff = angle / sin; result.V = coeff * a.V; } else { result.V = a.V; } return(result); }
// todo andi } /// <summary> /// Returns the dot-product of 2 quaternions. /// </summary> public static __ft__ Dot(__rot3t__ a, __rot3t__ b) { return(a.W * b.W + a.X * b.X + a.Y * b.Y + a.Z * b.Z); }
// todo andi { /// <summary> /// Returns the component-wise reciprocal (1/q.w, 1/q.x, 1/q.y, 1/q.z) /// of quaternion q. /// </summary> public static __rot3t__ Reciprocal(__rot3t__ q) { return(new __rot3t__(1 / q.W, 1 / q.X, 1 / q.Y, 1 / q.Z)); }
// todo andi } /// <summary> /// Returns the component-wise negation (-q.w, -q.v) of quaternion q. /// This represents the same rotation. /// </summary> public static __rot3t__ Negated(__rot3t__ q) { return(new __rot3t__(-q.W, -q.X, -q.Y, -q.Z)); }
/// <summary> /// Divides 2 quaternions. /// </summary> public static __rot3t__ Divide(__rot3t__ a, __rot3t__ b) { return(Multiply(a, Reciprocal(b))); }
/// <summary> /// Returns (q.w / s, q.v * (1/s)). /// </summary> public static __rot3t__ Divide(__rot3t__ q, __ft__ s) { return(Multiply(q, 1 / s)); }
// [todo ISSUE 20090421 andi : andi> // Operations like Add, Subtract and Multiplication with scalar, Divide, Reciprocal // should not be defined in a Rot3*. // These are perfectly valid for a quaternion, but a rotation is defined on a // NORMALIZED quaternion. This Norm-Constraint would be violated with above operations. // <] // todo andi { /// <summary> /// Returns the sum of 2 quaternions (a.w + b.w, a.v + b.v). /// </summary> public static __rot3t__ Add(__rot3t__ a, __rot3t__ b) { return(new __rot3t__(a.W + b.W, a.X + b.X, a.Y + b.Y, a.Z + b.Z)); }
/// <summary> /// Returns (q.w + s, (q.x + s, q.y + s, q.z + s)). /// </summary> public static __rot3t__ Add(__rot3t__ q, __ft__ s) { return(new __rot3t__(q.W + s, q.X + s, q.Y + s, q.Z + s)); }
/// <summary> /// Returns (q.w - s, (q.x - s, q.y - s, q.z - s)). /// </summary> public static __rot3t__ Subtract(__rot3t__ q, __ft__ s) { return(Add(q, -s)); }
/// <summary> /// Returns (q.w * s, q.v * s). /// </summary> public static __rot3t__ Multiply(__rot3t__ q, __ft__ s) { return(new __rot3t__(q.W * s, q.X * s, q.Y * s, q.Z * s)); }
/// <summary> /// Returns (a.w - b.w, a.v - b.v). /// </summary> public static __rot3t__ Subtract(__rot3t__ a, __rot3t__ b) { return(Add(a, -b)); }
/// <summary> /// Returns (s - q.w, (s - q.x, s- q.y, s- q.z)). /// </summary> public static __rot3t__ Subtract(__ft__ s, __rot3t__ q) { return(Add(-q, s)); }
public static bool ApproxEqual(__rot3t__ r0, __rot3t__ r1) { return(ApproxEqual(r0, r1, Constant <__ft__> .PositiveTinyValue)); }
/// <summary> /// Transforms direction vector v (v.w is presumed 0.0) by the inverse of quaternion q. /// </summary> public static __v3t__ InvTransformDir(__rot3t__ q, __v3t__ v) { //for Rotation Matrices R^-1 == R^T: return(((__m33t__)q).TransposedTransform(v)); }
/// <summary> /// Transforms point p (p.w is presumed 1.0) by the incerse of quaternion q. /// For quaternions, this method is equivalent to InvTransformDir, and /// is made available only to provide a consistent set of operations /// for all transforms. /// </summary> public static __v3t__ InvTransformPos(__rot3t__ q, __v3t__ p) { return(InvTransformDir(q, p)); }
public static __m33t__ Multiply(__rot2t__ rot2, __rot3t__ rot3) { return(__rot2t__.Multiply(rot2, (__m33t__)rot3)); }