bgc-net/Geometry/SPQuaternion.cs

using System;

namespace Geometry
{
    public struct SPQuaternion
    {
        public float s0, x1, x2, x3;

        public SPQuaternion(float s0, float x1, float x2, float x3)
        {
            this.s0 = s0;
            this.x1 = x1;
            this.x2 = x2;
            this.x3 = x3;
        }

        public SPQuaternion(in SPQuaternion quaternion)
        {
            this.s0 = quaternion.s0;
            this.x1 = quaternion.x1;
            this.x2 = quaternion.x2;
            this.x3 = quaternion.x3;
        }

        public SPQuaternion(in DPQuaternion quaternion)
        {
            this.s0 = (float)quaternion.s0;
            this.x1 = (float)quaternion.x1;
            this.x2 = (float)quaternion.x2;
            this.x3 = (float)quaternion.x3;
        }

        public void Reset()
        {
            this.s0 = 0.0f;
            this.x1 = 0.0f;
            this.x2 = 0.0f;
            this.x3 = 0.0f;
        }

        public void Conjugate()
        {
            this.x1 = -this.x1;
            this.x2 = -this.x2;
            this.x3 = -this.x3;
        }

        public readonly void GetConjugate(out SPQuaternion conjugated)
        {
            conjugated.s0 = this.s0;
            conjugated.x1 = -this.x1;
            conjugated.x2 = -this.x2;
            conjugated.x3 = -this.x3;
        }

        public void SetValues(float s0, float x1, float x2, float x3)
        {
            this.s0 = s0;
            this.x1 = x1;
            this.x2 = x2;
            this.x3 = x3;
        }

        public void SetValues(in SPQuaternion quaternion)
        {
            this.s0 = quaternion.s0;
            this.x1 = quaternion.x1;
            this.x2 = quaternion.x2;
            this.x3 = quaternion.x3;
        }

        public void SetValues(in DPQuaternion quaternion)
        {
            this.s0 = (float)quaternion.s0;
            this.x1 = (float)quaternion.x1;
            this.x2 = (float)quaternion.x2;
            this.x3 = (float)quaternion.x3;
        }

        public readonly void MakeRotationMatrix(out SPMatrix3x3 matrix)
        {
            float s0s0 = this.s0 * this.s0;
            float x1x1 = this.x1 * this.x1;
            float x2x2 = this.x2 * this.x2;
            float x3x3 = this.x3 * this.x3;

            float squareModule = (s0s0 + x1x1) + (x2x2 + x3x3);

            if (-SPUtility.EPSYLON <= squareModule && squareModule <= SPUtility.EPSYLON)
            {
                SPMatrix3x3.LoadIdentity(out matrix);
                return;
            }

            float corrector1;
            float corrector2;

            if (1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + SPUtility.TWO_EPSYLON) {
                corrector1 = 2.0f - squareModule;
                corrector2 = 2.0f * corrector1;
            }
            else {
                corrector1 = 1.0f / squareModule;
                corrector2 = 2.0f / squareModule;
            }

            float s0x1 = this.s0 * this.x1;
            float s0x2 = this.s0 * this.x2;
            float s0x3 = this.s0 * this.x3;
            float x1x2 = this.x1 * this.x2;
            float x1x3 = this.x1 * this.x3;
            float x2x3 = this.x2 * this.x3;

            matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
            matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
            matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));

            matrix.r1c2 = corrector2 * (x1x2 - s0x3);
            matrix.r2c3 = corrector2 * (x2x3 - s0x1);
            matrix.r3c1 = corrector2 * (x1x3 - s0x2);

            matrix.r2c1 = corrector2 * (x1x2 + s0x3);
            matrix.r3c2 = corrector2 * (x2x3 + s0x1);
            matrix.r1c3 = corrector2 * (x1x3 + s0x2);
        }

        public readonly void MakeReverseMatrix(out SPMatrix3x3 matrix)
        {
            float s0s0 = this.s0 * this.s0;
            float x1x1 = this.x1 * this.x1;
            float x2x2 = this.x2 * this.x2;
            float x3x3 = this.x3 * this.x3;

            float squareModule = (s0s0 + x1x1) + (x2x2 + x3x3);

            if (-SPUtility.EPSYLON <= squareModule && squareModule <= SPUtility.EPSYLON)
            {
                SPMatrix3x3.LoadIdentity(out matrix);
                return;
            }

            float corrector1;
            float corrector2;

            if (1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + SPUtility.TWO_EPSYLON) {
                corrector1 = 2.0f - squareModule;
                corrector2 = 2.0f * corrector1;
            }
            else {
                corrector1 = 1.0f / squareModule;
                corrector2 = 2.0f / squareModule;
            }

            float s0x1 = this.s0 * this.x1;
            float s0x2 = this.s0 * this.x2;
            float s0x3 = this.s0 * this.x3;
            float x1x2 = this.x1 * this.x2;
            float x1x3 = this.x1 * this.x3;
            float x2x3 = this.x2 * this.x3;

            matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
            matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
            matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));

            matrix.r1c2 = corrector2 * (x1x2 + s0x3);
            matrix.r2c3 = corrector2 * (x2x3 + s0x1);
            matrix.r3c1 = corrector2 * (x1x3 + s0x2);

            matrix.r2c1 = corrector2 * (x1x2 - s0x3);
            matrix.r3c2 = corrector2 * (x2x3 - s0x1);
            matrix.r1c3 = corrector2 * (x1x3 - s0x2);
        }

        public static void Add(in SPQuaternion quaternion1, in SPQuaternion quaternion2, out SPQuaternion sum)
        {
            sum.s0 = quaternion1.s0 + quaternion2.s0;
            sum.x1 = quaternion1.x1 + quaternion2.x1;
            sum.x2 = quaternion1.x2 + quaternion2.x2;
            sum.x3 = quaternion1.x3 + quaternion2.x3;
        }

        public static void Subtract(in SPQuaternion minuend, in SPQuaternion subtrahend, out SPQuaternion difference)
        {
            difference.s0 = minuend.s0 - subtrahend.s0;
            difference.x1 = minuend.x1 - subtrahend.x1;
            difference.x2 = minuend.x2 - subtrahend.x2;
            difference.x3 = minuend.x3 - subtrahend.x3;
        }

        public static void Multiply(in SPQuaternion left, in SPQuaternion right, out SPQuaternion product)
        {
            float s0 = (left.s0 * right.s0 - left.x1 * right.x1) - (left.x2 * right.x2 + left.x3 * right.x3);
            float x1 = (left.x1 * right.s0 + left.s0 * right.x1) - (left.x3 * right.x2 - left.x2 * right.x3);
            float x2 = (left.x2 * right.s0 + left.s0 * right.x2) - (left.x1 * right.x3 - left.x3 * right.x1);
            float x3 = (left.x3 * right.s0 + left.s0 * right.x3) - (left.x2 * right.x1 - left.x1 * right.x2);

            product.s0 = s0;
            product.x1 = x1;
            product.x2 = x2;
            product.x3 = x3;
        }
    }
}