예제 #1
0
        /// <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);
        }
예제 #2
0
 // [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);
 }
예제 #3
0
 // 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
                ));
 }
예제 #4
0
        /// <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;
        }
예제 #5
0
        /// <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);
        }
예제 #6
0
        // 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
                       ));
        }
예제 #7
0
        // [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);
        }
예제 #8
0
        /// <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);
        }
예제 #9
0
        // 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);
        }
예제 #10
0
 // 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));
 }
예제 #11
0
        // 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));
        }
예제 #12
0
 /// <summary>
 /// Divides 2 quaternions.
 /// </summary>
 public static __rot3t__ Divide(__rot3t__ a, __rot3t__ b)
 {
     return(Multiply(a, Reciprocal(b)));
 }
예제 #13
0
 /// <summary>
 /// Returns (q.w / s, q.v * (1/s)).
 /// </summary>
 public static __rot3t__ Divide(__rot3t__ q, __ft__ s)
 {
     return(Multiply(q, 1 / s));
 }
예제 #14
0
 // [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));
 }
예제 #15
0
 /// <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));
 }
예제 #16
0
 /// <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));
 }
예제 #17
0
 /// <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));
 }
예제 #18
0
 /// <summary>
 /// Returns (a.w - b.w, a.v - b.v).
 /// </summary>
 public static __rot3t__ Subtract(__rot3t__ a, __rot3t__ b)
 {
     return(Add(a, -b));
 }
예제 #19
0
 /// <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));
 }
예제 #20
0
 public static bool ApproxEqual(__rot3t__ r0, __rot3t__ r1)
 {
     return(ApproxEqual(r0, r1, Constant <__ft__> .PositiveTinyValue));
 }
예제 #21
0
 /// <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));
 }
예제 #22
0
 /// <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));
 }
예제 #23
0
 public static __m33t__ Multiply(__rot2t__ rot2, __rot3t__ rot3)
 {
     return(__rot2t__.Multiply(rot2, (__m33t__)rot3));
 }