Universal Framework for Science and Engineering - Part 2: Regression

using System;
using System.Collections.Generic;
using System.Text;


using Vector3D;

namespace Motion6D
{
    public class ReferenceFrame : IPosition, IOrientation
    {

        #region Fields

        /// <summary>
        /// Orientation quaternion
        /// </summary>
        protected double[] quaternion = new double[] { 1, 0, 0, 0 };

        /// <summary>
        /// Absolute position
        /// </summary>
        protected double[] position = new double[] { 0, 0, 0 };

        /// <summary>
        /// Orientation matrix
        /// </summary>
        protected double[,] matrix = new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };

        /// <summary>
        /// Auxiliary array
        /// </summary>
        protected double[,] qq = new double[4, 4];

        /// <summary>
        /// Auxiliary array
        /// </summary>
        protected double[] p = new double[3];

        /// <summary>
        /// Parent frame
        /// </summary>
        protected IReferenceFrame parent;

        /// <summary>
        /// Parameters
        /// </summary>
        protected object parameters;

        #endregion

        #region Ctor

        /// <summary>
        /// Constructor
        /// </summary>
        public ReferenceFrame()
        {
        }

        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="b">Auxiliary</param>
        private ReferenceFrame(bool b)
        {
        }

        #endregion

        #region IPosition Members

        /// <summary>
        /// Absolute position
        /// </summary>
        public double[] Position
        {
            get { return position; }
        }

        /// <summary>
        /// Parent frame
        /// </summary>
        public IReferenceFrame Parent
        {
            get
            {
                return parent;
            }
            set
            {
                parent = value;
            }
        }

        /// <summary>
        /// Position parameters
        /// </summary>
        public virtual object Parameters
        {
            get
            {
                return parameters;
            }
            set
            {
                parameters = value;
            }
        }

 
        /// <summary>
        /// Parent frame
        /// </summary>
        /// <param name="position">Position</param>
        /// <returns>Parent frame</returns>
        static public ReferenceFrame GetParentFrame(IPosition position)
        {
            if (position.Parent == null)
            {
                return Motion6DFrame.Base;
            }
            return position.Parent.Own;
        }

        /// <summary>
        /// Updates itself
        /// </summary>
        public virtual void Update()
        {
            ReferenceFrame p = ParentFrame;
            position = p.Position;
            quaternion = p.quaternion;
            matrix = p.matrix;
        }

        #endregion

        #region IOrientation Members

        /// <summary>
        /// Orientation quaternion
        /// </summary>
        public double[] Quaternion
        {
            get { return quaternion; }
        }

        /// <summary>
        /// Orientation matrix
        /// </summary>
        public double[,] Matrix
        {
            get { return matrix; }
        }


        #endregion

        #region Specific Members

        /// <summary>
        /// Gets relative frame
        /// </summary>
        /// <param name="baseFrame">Base frame</param>
        /// <param name="relativeFrame">Relative frame</param>
        /// <param name="result">Result frame</param>
        /// <param name="diff">Difference between coordinates</param>
        static public void GetRelative(ReferenceFrame baseFrame, ReferenceFrame relativeFrame, 
            ReferenceFrame result, double[] diff)
        {
            V3DOperations.QuaternionInvertMultiply(relativeFrame.quaternion, baseFrame.quaternion, result.quaternion);
            result.SetMatrix();
            for (int i = 0; i < 3; i++)
            {
                diff[i] = relativeFrame.position[i] - baseFrame.position[i];
            }
            double[,] m = baseFrame.Matrix;
            double[] p = result.position;
            for (int i = 0; i < 3; i++)
            {
                p[i] = 0;
                for (int j = 0; j < 3; j++)
                {
                    p[i] += m[j, i] * diff[j];
                }
            }

        }

        /// <summary>
        /// Gets relative frame
        /// </summary>
        /// <param name="baseFrame">Base frame</param>
        /// <param name="relativeFrame">Relative frame</param>
        /// <param name="result">Result frame</param>
        /// <param name="diff">Difference between coordinates</param>
        /// <param name="matrix4">The 4 * 4 perspective matrix</param>
        static public void GetRelative(ReferenceFrame baseFrame, ReferenceFrame relativeFrame,
             ReferenceFrame result, double[] diff, double[,] matrix4)
        {
            GetRelative(baseFrame, relativeFrame, result, diff);
            double[,] m = result.Matrix;
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    matrix4[i, j] = m[i, j];
                }
            }
            double[] p = result.position;
            for (int i = 0; i < 3; i++)
            {
                matrix4[3, i] = p[i];
                matrix4[i, 3] = 0;
            }
            matrix4[3, 3] = 1;
        }


        /// <summary>
        /// Gets relative frame
        /// </summary>
        /// <param name="baseFrame">Base frame</param>
        /// <param name="relativeFrame">Relative frame</param>
        /// <param name="result">Result frame</param>
        /// <param name="diff">Difference between coordinates</param>
        /// <param name="matrix4">The 4 * 4 perspective matrix</param>
        /// <param name="array16">One dimensional array that correspond to perspective matrix</param>
        static public void GetRelative(ReferenceFrame baseFrame, ReferenceFrame relativeFrame,
               ReferenceFrame result, double[] diff, double[,] matrix4, double[] array16)
        {
            GetRelative(baseFrame, relativeFrame, result, diff, matrix4);
            for (int i = 0; i < 4; i++)
            {
                int k = 4 * i;
                for (int j = 0; j < 4; j++)
                {
                    array16[k + j] = matrix4[i, j];
                }
            }
        }
        

        /// <summary>
        /// Calculates matrix for object view
        /// </summary>
        /// <param name="position">Relative position</param>
        /// <param name="rotation">Rotation angle</param>
        /// <returns>View matrix</returns>
        static public double[,] CalucateViewMatrix(double[] position, double rotation)
        {
            double[] r = position;
            double ap = 0;
            ap = r[0] * r[0] + r[2] * r[2];
            double a = ap + r[1] * r[1];
            ap = Math.Sqrt(ap);
            a = Math.Sqrt(a);
            double[] ez = { r[0] / a, r[1] / a, r[2] / a };
            double[] ex1 = null;
            if (ap < 0.00000000001)
            {
                ex1 = new double[] { 1.0, 0.0, 0.0 };
            }
            else
            {
                ex1 = new double[] { -r[2] / ap, 0, r[0] / ap };
            }
            double[] ey1 = {ez[1] * ex1[2] - ez[2] * ex1[1],
							   ez[2] * ex1[0] - ez[0] * ex1[2],
							   ez[0] * ex1[1] - ez[1] * ex1[0]};
            double[] ey = new double[3];
            double[] ex = new double[3];
            double alpha = rotation;
            alpha *= Math.PI / 180.0;
            alpha += Math.PI;
            double s = 0;
            double c = 1;
            double sD2 = Math.Sin(alpha / 2);
            double cD2 = Math.Cos(alpha / 2);
            double[] rc = { r[0], r[1] };
            r[0] = rc[0] * c - rc[1] * s;
            r[1] = rc[1] * s + rc[1] * c;
            ex = ex1;
            ey = ey1;
            double[][] m = { ex, ey, ez };
            double[] rr = { r[1], r[2], r[0] };

            double[,] mat = new double[4, 4];
            for (int i = 0; i < 3; i++)
            {
                mat[3, i] = r[i];
                for (int j = 0; j < 3; j++)
                {
                    mat[j, i] = m[j][i];
                }
            }

            double[][] temp = new double[3][];
            temp[0] = new double[] { c, s, 0 };
            temp[1] = new double[] { -s, c, 0 };
            temp[2] = new double[] { 0, 0, 1 };
            double[,] mh = new double[3, 3];
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    mh[i, j] = 0;
                    for (int k = 0; k < 3; k++)
                    {
                        for (int l = 0; l < 3; l++)
                        {
                            mh[i, j] += mat[i, k] * temp[k][l] * mat[j, l];
                        }
                    }
                }
            }
            double[,] mh1 = new double[3, 3];
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    mh1[i, j] = mat[i, j];
                }
            }
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    mat[i, j] = 0;
                    for (int k = 0; k < 3; k++)
                    {
                        mat[i, j] += mh[i, k] * mh1[k, j];
                    }
                }
            }


            for (int i = 0; i < 3; i++)
            {
                rr[i] = 0;
                for (int j = 0; j < 3; j++)
                {
                    rr[i] += mat[i, j] * mat[3, j];
                }
            }
            double[,] matr = new double[3, 3];
            double[] rp = new double[3];
            for (int i = 0; i < 3; i++)
            {
                rp[i] = mat[3, i];
                for (int j = 0; j < 3; j++)
                {
                    matr[i, j] = mat[j, i];
                }
            }
            double[,] qq = new double[4, 4];
            double[] e = Vector3D.V3DOperations.VectorNorm(rp);
            double[] q = new double[] { cD2, e[0] * sD2, e[1] * sD2, e[2] * sD2 };
            double[,] mq = new double[3, 3];
            Vector3D.V3DOperations.CalculateMatrix(q, mq, qq);
            double[,] mr = new double[3, 3];
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    mr[i, j] = 0;
                    for (int k = 0; k < 3; k++)
                    {
                        mr[i, j] += mq[i, k] * matr[k, j];
                    }
                }
            }
            return mr;

        }


        /// <summary>
        /// Sets state
        /// </summary>
        /// <param name="baseFrame">Base frame</param>
        /// <param name="relative">Relative frame</param>
        public virtual void Set(ReferenceFrame baseFrame, ReferenceFrame relative)
        {
            for (int i = 0; i < 3; i++)
            {
                position[i] = baseFrame.position[i];
                for (int j = 0; j < 3; j++)
                {
                    position[i] += baseFrame.matrix[i, j] * relative.position[j];
                }
            }
            V3DOperations.QuaternionMultiply(baseFrame.quaternion, relative.quaternion, quaternion);
            Norm();
            SetMatrix();
        }

        /// <summary>
        /// Sets matrix
        /// </summary>
        public void SetMatrix()
        {
            V3DOperations.CalculateMatrix(quaternion, matrix, qq);
        }

        /// <summary>
        /// Quaternion normalization
        /// </summary>
        public void Norm()
        {
            double a = 0;
            foreach (double x in quaternion)
            {
                a += x * x;
            }
            a = 1 / Math.Sqrt(a);
            for (int i = 0; i < 3; i++)
            {
                quaternion[i] *= a;
            }
        }

        /// <summary>
        /// Gets relative position
        /// </summary>
        /// <param name="position">The position</param>
        /// <param name="coordinates">Relative coordinates</param>
        public void GetPositon(IPosition position, double[] coordinates)
        {
            double[] p1 = this.position;
            double[] p2 = position.Position;
            for (int i = 0; i < 3; i++)
            {
                p[i] = p2[i] - p1[i];
            }
            for (int i = 0; i < 3; i++)
            {
                coordinates[i] = 0;
                for (int j = 0; j < 3; j++)
                {
                    coordinates[i] += matrix[i, j] * p[j];
                }
            }
        }


        /// <summary>
        /// Parent frame
        /// </summary>
        protected virtual ReferenceFrame ParentFrame
        {
            get
            {
                if (parent == null)
                {
                    return Motion6D.Motion6DFrame.Base;
                }
                return parent.Own;
            }
        }

        #endregion
    }
}