/fpTransform/fpTransform/Module/TransformModule/Gm.cs

Large files files are truncated, but you can click here to view the full file

#define L_ARM

using System;
using System.Data.SqlTypes;
using System.Collections.Generic;
using System.Collections;
using Microsoft.SqlServer.Server;

namespace geometry
{
    public static class Gm
    {
        public enum DecartPosition
        {
            Origin = 0,
            At1 = 1,
            At2 = 2,
            At3 = 3,
            At4 = 4,
            AtX_PlusY = 5,
            AtX_MinusY = 6,
            AtY_PlusX = 7,
            AtY_MinusX = 8
        }

        #region Common

        public static double Accuracy { get { return 0.0000001; } }
        public static uint MaxDigits { get { return 7; } }
        public static bool ApproximatelyEqual(double d1, double d2, double tol)
        {
            return (Math.Abs(d1 - d2) < tol);
        }
        public static bool ApproximatelyEqual(double d1, double d2)
        {
            return (Math.Abs(d1 - d2) < Accuracy);
        }

        public static bool ApproximatelyEqual(float d1, float d2, float tol)
        {
            return (Math.Abs(d1 - d2) < tol);
        }
        public static bool ApproximatelyEqual(float d1, float d2)
        {
            return (Math.Abs(d1 - d2) < Accuracy);
        }
        public static bool Factor(uint digits, out double factor1, out double factor2) 
        {
            factor1 = 1;
            factor2 = 1;
            
            if (digits > MaxDigits)
                throw new ArgumentException("Argument exception: digits");

            switch (digits)
            {
                case 0:
                    factor1 = factor2 = 0;
                    return true;
                case 1:
                    factor1 = 0.1;
                    factor2 = 10;
                    break;
                case 2:
                    factor1 = 0.01;
                    factor2 = 100;
                    break;
                case 3:
                    factor1 = 0.001;
                    factor2 = 1000;
                    break;
                case 4:
                    factor1 = 0.0001;
                    factor2 = 10000;
                    break;
                case 5:
                    factor1 = 0.00001;
                    factor2 = 100000;
                    break;
                default:
                    for (int i = 0; i < digits; i++)
                    {
                        factor1 *= 0.1;
                        factor2 *= 10;
                    }
                    break;
            }
            return false;
        }
        public static bool Factor(uint digits, out float factor1, out float factor2)
        {
            factor1 = 1;
            factor2 = 1;

            if (digits > MaxDigits)
                throw new ArgumentException("Argument exception: digits");

            switch (digits)
            {
                case 0:
                    factor1 = factor2 = 0f;
                    return true;
                case 1:
                    factor1 = 0.1f;
                    factor2 = 10f;
                    break;
                case 2:
                    factor1 = 0.01f;
                    factor2 = 100f;
                    break;
                case 3:
                    factor1 = 0.001f;
                    factor2 = 1000f;
                    break;
                case 4:
                    factor1 = 0.0001f;
                    factor2 = 10000f;
                    break;
                case 5:
                    factor1 = 0.00001f;
                    factor2 = 100000f;
                    break;
                default:
                    for (int i = 0; i < digits; i++)
                    {
                        factor1 *= 0.1f;
                        factor2 *= 10f;
                    }
                    break;
            }
            return false;
        }
        public static double DegToRad(float angle) { return angle * Math.PI / 180; }
        public static double DegToRad(double angle) { return angle * Math.PI / 180; }
        public static double RadToDeg(float angle) { return angle * 180 / Math.PI; }
        public static double RadToDeg(double angle) { return angle * 180/ Math.PI; }
        public static double RoundTo(float value, uint digits)
        {
            float factor1 = default(float);
            float factor2 = factor1;
            float ret;
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
                ret = (int)value;
            else
                ret = ((int)(value * factor2)) * factor1;

            return ret;
        }
        public static double RoundTo(double value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            double ret;
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
                ret = (int)value;
            else
                ret = ((int)(value * factor2)) * factor1;

            return ret;
        }

        public static Vector RoundTo(Vector value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            Vector ret = new Vector();
            if (Factor(digits, out factor1, out factor2))
            {
                ret.x = (int)value.x;
                ret.y = (int)value.y;
                ret.z = (int)value.z;
            }
            else
            {
                ret.x = ((int)(value.x * factor2)) * factor1;
                ret.y = ((int)(value.y * factor2)) * factor1;
                ret.z = ((int)(value.z * factor2)) * factor1;
            }
            return ret;
        }
        public static Point RoundTo(Point value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            Point ret = new Point();
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
            {
                ret.x = (int)value.x;
                ret.y = (int)value.y;
                ret.z = (int)value.z;
            }
            else
            {
                ret.x = ((int)(value.x * factor2)) * factor1;
                ret.y = ((int)(value.y * factor2)) * factor1;
                ret.z = ((int)(value.z * factor2)) * factor1;
            }
            return ret;
        }
        public static Quaterion RoundTo(Quaterion value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            Quaterion ret = new Quaterion();
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
            {
                ret.Vector = new Vector((int)value.Vector.x, (int)value.Vector.y, (int)value.Vector.z);
                ret.W = (int)value.W;
            }
            else
            {
                ret.Vector = new Vector(((int)(value.Vector.x * factor2)) * factor1, ((int)(value.Vector.y * factor2)) * factor1, ((int)(value.Vector.z * factor2)) * factor1);
                ret.W = ((int)(value.W * factor2)) * factor1;
            }
            return ret;
        }
        public static Matrix3 RoundTo(Matrix3 value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            double[] data = value.Data;

            //Matrix3 ret = new Matrix3();
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
            {                
                for (int i = 0; i < data.Length; i++)
                    data[i] = (int)data[i];
            }
            else
            {
                for (int i = 0; i < data.Length; i++)
                    data[i] = ((int)(data[i] * factor2)) * factor1;                
            }
            return new Matrix3(data);
        }
        public static Transform RoundTo(Transform value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            double[] data = value.Data;

            Transform ret = new Transform();
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
            {
                for (int i = 0; i < data.Length; i++)
                    data[i] = (int)data[i];
            }
            else
            {
                for (int i = 0; i < data.Length; i++)
                    data[i] = ((int)(data[i] * factor2)) * factor1;
            }
            return new Transform(data);
        }

        /// <summary>
        /// Split double array to vectors
        /// </summary>
        /// <param name="values"></param>
        /// <param name="vDim"></param>
        /// <param name="useRound"></param>
        /// <param name="digits"></param>
        /// <returns></returns>
        public static Vector[] SplitToVectors(double[] values, byte vDim, bool useRound, uint digits)
        {
            int vCCount = values.Length % vDim;

            if (vDim < 2 | vDim > 3)
                throw new ArgumentException("Gm.SplitToVectors: vDim must be 2 or 3");

            if (vCCount > 0 & vCCount < vDim - 1)
                throw new ArgumentException("Gm.SplitToVectors: values");

            Vector[] ret = new Vector[(int)(values.Length / vDim) + (vCCount > 0 ? 1 : 0)];

            for (int i = 0, counter = 0; i < values.Length & counter < ret.Length; i += vDim, counter++)
            {
                if (vDim > 1)
                {
                    ret[counter].x = values[i];
                    ret[counter].y = values[i + 1];
                }
                if (vDim > 2 && i + 2 < values.Length)
                    ret[counter].z = values[i + 2];

                // round
                if (useRound)
                    ret[counter].Round(digits);
            }
            return ret;
        }
        /// <summary>
        /// Split double array to points
        /// </summary>
        /// <param name="values"></param>
        /// <param name="vDim"></param>
        /// <param name="useRound"></param>
        /// <param name="digits"></param>
        /// <returns></returns>
        public static Point[] SplitToPoints(double[] values, byte vDim, bool useRound, uint digits)
        {
            int vCCount = values.Length % vDim;

            if (vDim < 2 | vDim > 4)
                throw new ArgumentException("Gm.SplitToPoints: vDim must be 2 or 3");

            if (vCCount > 0 & vCCount < vDim - 1)
                throw new ArgumentException("Gm.SplitToPoints: values");

            Point[] ret = new Point[(int)(values.Length / vDim) + (vCCount > 0 ? 1 : 0)];

            for (int i = 0, counter = 0; i < values.Length & counter < ret.Length; i += vDim, counter++)
            {
                if (vDim > 1)
                {
                    ret[counter].x = values[i];
                    ret[counter].y = values[i + 1];
                }
                if (vDim > 2 && i + 2 < values.Length)
                    ret[counter].z = values[i + 2];
                // uses round
                if (useRound)
                    ret[counter].Round(digits);
            }
            return ret;
        }
        /// <summary>
        /// Split double array to points
        /// </summary>
        /// <param name="values"></param>
        /// <param name="vDim"></param>
        /// <param name="useRound"></param>
        /// <param name="digits"></param>
        /// <returns></returns>
        public static Point[] SplitToPoints(float[] values, byte vDim, bool useRound, uint digits)
        {
            int vCCount = values.Length % vDim;

            if (vDim < 2 | vDim > 4)
                throw new ArgumentException("Gm.SplitToPoints: vDim must be 2 or 3");

            if (vCCount > 0 & vCCount < vDim - 1)
                throw new ArgumentException("Gm.SplitToPoints: values");

            Point[] ret = new Point[(int)(values.Length / vDim) + (vCCount > 0 ? 1 : 0)];

            for (int i = 0, counter = 0; i < values.Length & counter < ret.Length; i += vDim, counter++)
            {
                if (vDim > 1)
                {
                    ret[counter].x = values[i];
                    ret[counter].y = values[i + 1];
                }
                if (vDim > 2 && i + 2 < values.Length)
                    ret[counter].z = values[i + 2];
                // uses round
                if (useRound)
                    ret[counter].Round(digits);
            }
            return ret;
        }

        public static double GetPolarAngle02P(double x, double y)
        {
            return Math.Atan(y / (x * x + y * y));
        }

        public static double iLeftOrRight(Point p, Point p1, Point p2)
        {
            double result = (p1.x - p.x) * (p2.y - p.y) - (p2.x - p.x) * (p1.y - p.y);

            if (Math.Abs(result) < Accuracy * Accuracy)
                result = 0;

            return result;
        }

        public static double SignTriangSquare(Point p1, Point p2, Point p3, bool isClockWice)
        {
            /*
             // p1, p2, p3
             -------
             x1 y1 1
             x2 y2 1
             x3 y3 1
             -------
            return m_matrix[0, 0] * m_matrix[1, 1] * m_matrix[2, 2] + m_matrix[0, 1] * m_matrix[1, 2] * m_matrix[2, 0] + m_matrix[0, 2] * m_matrix[1, 0] * m_matrix[2, 1]
  - m_matrix[0, 2] * m_matrix[1, 1] * m_matrix[2, 0] - m_matrix[0, 1] * m_matrix[1, 0] * m_matrix[2, 2] - m_matrix[0, 0] * m_matrix[1, 2] * m_matrix[2, 1];
             */
            double result = p1.x * p2.y + p1.y * p3.x + p2.x * p3.y - p2.y * p3.x - p1.y * p2.x - p1.x * p3.y;

            return isClockWice ? -result : result;

        }

        public static Point Mid(Point p1, Point p2)
        {
            double
                x = (p1.x + p2.x) / 2,
                y = (p1.y + p2.y) / 2,
                z = (p1.z + p2.z) / 2;

            return new Point(x, y, z);
        }

        public static bool DetectTriang(IEnumerable<Point> points,
            out Point p1, out Point p2, out Point p3)
        {
            ///Set points to default
            ///
            p1 = default(Point);
            p2 = default(Point);
            p3 = default(Point);
            /// Preparation
            /// 
            IEnumerator<Point> iEnum = null;
            if (points == null)
                return false;
            if ((iEnum = points.GetEnumerator()) == null)
                return false;
            iEnum.Reset();
            ///Read first two points
            ///
            if (iEnum.MoveNext())
                p1 = iEnum.Current;
            else
                return false;
            if (iEnum.MoveNext())
                p2 = iEnum.Current;
            else
                return false;
            /// Search point
            /// 
            while (iEnum.MoveNext())
            {
                if ( Math.Abs( iLeftOrRight(iEnum.Current, p1, p2)) > Accuracy)
                {
                    p3 = iEnum.Current;
                    return true;
                }
            }
            return false;
        }

        public static Point Centroid(IEnumerable<Point> points, out bool isDetect)
        {
            Point ret = default(Point),
                p1 = default(Point), p2 = default(Point), p3 = default(Point);
            isDetect = false;
            
            if (DetectTriang(points, out p1, out p2, out p3))
            {
                Point m1 = Mid(p2, p3);
                Point m2 = Mid(p1, p3);
                Point m3 = Mid(p1, p2);

                if (Intersect(p1, m1, p2, m2, out ret) < 1)
                {
                    ret = default(Point);
                    isDetect = false;
                }
                isDetect = true;
            }
            return ret;
        }

        public static void Sort(ref Point[] array, int minIndex, int maxIndex)
        {
            Dictionary<bool, List<Point>> convSegment = new Dictionary<bool, List<Point>>();
            /// up convex
            /// 
            convSegment.Add(false, new List<Point>());
            /// down convex
            /// 
            convSegment.Add(true, new List<Point>());
            
            for (int i = 0; i < array.Length; i++)
            {
                double res = Gm.iLeftOrRight(array[i], array[minIndex], array[maxIndex]);
                if (Math.Abs(res) < Accuracy)
                    continue;
                if (res > 0)
                    convSegment[false].Add(array[i]);
                else
                    convSegment[true].Add(array[i]);
            }
        }

        public static bool Convex(ref Point[] tPoints)
        {
            int minIndex = 0, maxIndex = 0, tIndex = 0;
            double temp = 0;
            //double max = 0;
            //double min = 0;
            //if (tPoints.Length > 0)
            //{
            //    min = tPoints[0].x;
            //    max = tPoints[0].x;
            //}
            /// loock for min index and max index by x-componenta
            /// 
            for (int i = 0; i < tPoints.Length; i++)
            {
                //if (min > tPoints[i].x)
                //{
                //    min = tPoints[i].x;
                //    minIndex = i;
                //}
                //if (max < tPoints[i].x)
                //{
                //    max = tPoints[i].x;
                //    maxIndex = i;
                //}
                System.Diagnostics.Debug.Print("x{0:F4}; y{1:F4}; z{2:F4};", new object[] { tPoints[i].x, tPoints[i].y, tPoints[i].z });
                if (tPoints[minIndex].x > tPoints[i].x)
                    minIndex = i;
                if (tPoints[maxIndex].x < tPoints[i].x)
                    maxIndex = i;
            }
            double tSquare = 0;
            /// Look for point with max triange square
            /// 
            for (int i = 0; i < tPoints.Length; i++)
            {
                temp = Gm.TriangSquareGeron(tPoints[minIndex], tPoints[maxIndex], tPoints[i]);
                if (Math.Abs(tSquare)< Math.Abs(temp))
                {
                    tSquare = temp;
                    tIndex = i;
                }
            }
            bool isDetect = false;
            Point cPoint = Centroid(new Point[] { tPoints[minIndex], tPoints[maxIndex], tPoints[tIndex] }, out isDetect);
            /// projected face is line
            /// 
            if (!isDetect)
                return false;
            /// projected face is polygon
            /// Sort points as polar angles
            /// 
            Sort(ref tPoints, minIndex, maxIndex);

            return true;
        }

        public static void JoinConvex(ref List<Point> resultConvex, ref List<Point> convex1, ref List<Point> convex2) 
        {
        }

        public static double Length2(Point p1, Point p2)
        {
            return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
        }

        public static double TriangSquareGeron(Point p1, Point p2, Point p3)
        {
            double a = Math.Sqrt(Length2(p1, p2));
            double b = Math.Sqrt(Length2(p2, p3));
            double c = Math.Sqrt(Length2(p1, p3));
            double p = 0.5 * (a + b + c);
            
            return p * Math.Sqrt((p - a) * (p - b) * (p - c));
        }

        public static double TriangSquare(Point p1, Point p2, Point p3)
        {
            return 0.5 * Math.Abs(SignTriangSquare(p1, p2, p3, false));
        }

        public static int Intersect(Point a1, Point a2, Point b1, Point b2, out Point intersection)
        {
            intersection = default(Point);

            double
                d = (a1.x - a2.x) * (b2.y - b1.y) - (a1.y - a2.y) * (b2.x - b1.x),
                da = (a1.x - b1.x) * (b2.y - b1.y) - (a1.y - b1.y) * (b2.x - b1.x),
                db = (a1.x - a2.x) * (a1.y - b1.y) - (a1.y - a2.y) * (a1.x - b1.x);


            if (Math.Abs(d) < Accuracy)
                return 0;
            double
                ta = da / d,
                tb = db / d;
            if (0 <= ta & ta <= 1 & 0 <= tb & tb <= 1)
            {
                intersection = new Point(a1.x + ta * (a2.x - a1.x), a1.y + ta * (a2.y - a1.y), 0);
                return 1;
            }

            return -1;
        }

        #endregion

        #region Vector definition

        //[System.Diagnostics.DebuggerDisplay("{format(4)}", Name = "Vector")]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Vector : INullable, IEquatable<Vector>
        {
            public double x;
            public double y;
            public double z;
            private bool m_null;

            private string format(byte dig)
            {
                Vector v = RoundTo(this,dig);
                return "(" + v.x.ToString() + "; " + v.y.ToString() + "; " + v.z.ToString() + "), Length:" + v.Length().ToString();
            }

            public Vector(double x, double y, double z)
                : this()
            {
                this.x = x;
                this.y = y;
                this.z = z;
            }

            public Vector(double[] points)
                : this()
            {
                if (points != null && points.Length >= 2)
                {
                    this.x = points[0];
                    this.y = points[1];
                    if (points.Length > 2)
                        this.z = points[2];
                }
                else
                    throw new ArgumentOutOfRangeException("Array size have to be equal or greater than 3 elements");
            }

            public Vector(Vector v)
                : this()
            {
                this.x = v.x;
                this.y = v.y;
                this.z = v.z;
            }

            public void SetCoords(double x, double y, double z)
            {
                this.x = x;
                this.y = y;
                this.z = z;
            }

            public void SetCoords(Vector v)
            {
                this.x = v.x;
                this.y = v.y;
                this.z = v.z;
            }

            public void SetCoords(double[] points)
            {
                if (points != null && points.Length >= 2)
                {
                    this.x = points[0];
                    this.y = points[1];

                    if (points.Length >= 3)
                        this.z = points[2];
                }
                else
                    throw new ArgumentOutOfRangeException("Array size have to be equal or greater than 3 elements");
            }
           
            public void Normalize()
            {
                double dist = Length2();
                if (dist > 0)
                {
                    x /= dist;
                    y /= dist;
                    z /= dist;
                }
            }
            
            public Vector GetNormalized()
            {
                Vector v = new Vector(x, y, z);
                double dist = v.Length();
                if (dist > 0)
                {
                    dist = 1 / dist;
                    v.x *= dist;
                    v.y *= dist;
                    v.z *= dist;
                }
                return v;
            }

            /// <summary>
            /// Creates parallel vector, at point
            /// </summary>
            /// <param name="point"></param>
            /// <returns></returns>
            public Vector GetParallel(Point point)
            {
                Vector v = this;
                v.x += point.x;
                v.y += point.y;
                v.z += point.z;
                return v;
            }
            /// <summary>
            /// Creates parallel vector, at point
            /// </summary>
            /// <param name="point"></param>
            /// <returns></returns>
            public Vector GetParallel(double distance)
            {
                Vector normal = GetPerpVector()*distance;
                Vector v = normal.GetPerpVector();
                if (this.GetNormalized() != v)
                    v.Inverce();
                return v;
            }

            /// <summary>
            /// Returns perpendicular vector
            /// </summary>
            /// <returns></returns>
            public Vector GetPerpVector()
            {
                if ((AngleTo(new Vector(0, 1, 0)) < Accuracy) || (AngleTo(new Vector(0, -1, 0)) < Accuracy))
                {
                    return new Vector(y, 0, 0).GetNormalized();
                }
                return new Vector(z, 0, -x).GetNormalized();
            }

            public void Inverce() { x = -x; y = -y; z = -z; }
            /// <summary>
            /// Returns opposite vector
            /// </summary>
            /// <returns></returns>
            public Vector GetInverse()
            {
                return new Vector(-x, -y, -z);
            }

            public double Length()
            {
                return Math.Sqrt(x * x + y * y + z * z);
            }

            public double Length2()
            {
                return x * x + y * y + z * z;
            }

            public Vector Rotate(Vector dir, double ang)
            {
                if (ang == 0)
                    return this;

                double c, s;
                double a;
                a = ang * Math.PI / 180;
                c = Math.Cos(a);
                s = Math.Sin(a);

                if (Double.IsNaN(c))
                    c = 0;
                if (Double.IsNaN(s))
                    s = 0;
                double nx, ny, nz;
                nx = x * (c + (1 - c) * (dir.x * dir.x)) + y * ((1 - c) * dir.y * dir.x + s * dir.z) + z * ((1 - c) * dir.z * dir.x - s * dir.y);
                ny = x * ((1 - c) * dir.y * dir.x - s * dir.z) + y * (c + (1 - c) * (dir.y * dir.y)) + z * ((1 - c) * dir.z * dir.y + s * dir.x);
                nz = x * ((1 - c) * dir.z * dir.x + s * dir.y) + y * ((1 - c) * dir.z * dir.y - s * dir.x) + z * (c + (1 - c) * (dir.z * dir.z));
                x = nx;
                y = ny;
                z = nz;
                return this;
            }           

            public void Round(uint digits)
            {
                double factor1 = default(double);
                double factor2 = factor1;                
                Factor(digits, out factor1, out factor2);
                if (Factor(digits, out factor1, out factor2))
                {
                    x = (int)x;
                    y = (int)y;
                    z = (int)z;
                }
                else
                {
                    x = ((int)(x * factor2)) * factor1;
                    y = ((int)(y * factor2)) * factor1;
                    z = ((int)(z * factor2)) * factor1;
                }               
            }

            public double AngleTo(Vector vector)
            {              
                if (vector.Length() == 0)
                    return 0;

                double d = (this * vector) / (Length() * vector.Length());
                if (ApproximatelyEqual(d, 1, Accuracy))
                {
                    d = 1;
                }
                else if (ApproximatelyEqual(d, -1, Accuracy))
                {
                    d = -1;
                }
                return Math.Acos(d);
            }

            public double AngleToNonACos(Vector vector)
            {
                if (vector.Length() == 0)
                    return 0;

                double d = (this * vector) / (Length() * vector.Length());
                if (ApproximatelyEqual(d, 1, Accuracy))
                {
                    d = 1;
                }
                else if (ApproximatelyEqual(d, -1, Accuracy))
                {
                    d = -1;
                }
                return d;
            }

            public bool IsParallelTo(Vector vector)
            {
                double angle = AngleTo(vector);
                return (ApproximatelyEqual(angle, 0, Accuracy) || ApproximatelyEqual(angle, Math.PI, Accuracy));
            }

            public bool IsPerpendicularTo(Vector vector)
            {
                return (ApproximatelyEqual(AngleTo(vector), Math.PI / 2));
            }
            
            /// <summary>
            /// Return rotated vector at transform data
            /// </summary>
            /// <param name="transform">transform matrix</param>
            /// <returns>rotated vector</returns>
            public Vector this[Transform transform]
            {
                get { return transform.TransformVector(this); }
            }	

            #region INullable Members

            public bool IsNull { get { return m_null; } }//ApproximatelyEqual(x, 0) && ApproximatelyEqual(y, 0) && ApproximatelyEqual(z, 0);

            #endregion

            #region IEquatable<Vector> Members

            public override int GetHashCode()
            {
                return (int)Math.Sqrt(x * x * y * y * z * z);
            }

            public override bool Equals(object o)
            {
                if (o is Vector)
                {
                    Vector v = (Vector)o;
                    return !v.IsNull && Equals(v);
                }
                return false;
            }

            public bool Equals(Vector v)
            {
                return this == v;
            }

            public bool Equals(Vector vec, int tol)
            {
                return
                    ApproximatelyEqual(vec.x, x, tol)
                  &&
                    ApproximatelyEqual(vec.y, y, tol)
                  &&
                    ApproximatelyEqual(vec.z, z, tol);
            }

            #endregion

            #region operators

            public static Vector operator +(Vector v1, Vector v2)
            {
                return new Vector(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
            }

            public static Vector operator -(Vector v1, Vector v2)
            {
                return new Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
            }

            public static Vector operator *(Vector v1, double val)
            {
                return new Vector(v1.x * val, v1.y * val, v1.z * val);
            }

            public static Vector operator /(Vector v1, double val)
            {
                return new Vector(v1.x / val, v1.y / val, v1.z / val);
            }

            public static Vector operator ^(Vector v1, Vector v2)
            {
#if L_ARM
                return new Vector(v2.y * v1.z - v2.z * v1.y, v2.z * v1.x - v2.x * v1.z, v2.x * v1.y - v2.y * v1.x);
#else
                return new Vector(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
#endif
            }

            public static double operator *(Vector v1, Vector v2)
            {
                return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
            }

            public static bool operator ==(Vector v1, Vector v2)
            {
                return
                    ApproximatelyEqual(v1.x, v2.x)
                  &&
                    ApproximatelyEqual(v1.y, v2.y)
                  &&
                    ApproximatelyEqual(v1.z, v2.z);
            }

            public static bool operator !=(Vector v1, Vector v2)
            {
                return !(v1 == v2);
            }

            public static implicit operator double[](Vector v1)
            {
                double[] arr = new double[3];
                arr[0] = v1.x;
                arr[1] = v1.y;
                arr[2] = v1.z;
                return arr;
            }

            public static implicit operator Vector(double[] points)
            {
                Vector v = new Vector();
                if (points != null && points.Length >= 2)
                {
                    v.x = points[0];
                    v.y = points[1];
                    v.z = points[2];
                }
                else
                    throw new ArgumentOutOfRangeException("Array size have to be equal or greater than 3 elements");

                return v;
            }

            #endregion

            #region static members
            public static Vector Null
            {
                get
                {
                    Vector v = new Vector();
                    v.m_null = true;
                    return v;
                }
            }
            /// <summary>
            /// Calculate angle between vectors v1 and v2. All vectors mut be vectors with unit length 
            /// </summary>
            /// <param name="axis">required to define sign of the angle</param>
            /// <param name="v1">first vector</param>
            /// <param name="v2">second vector</param>
            /// <returns>return signed angle value in radians</returns>
            public static double GetAngleZBetweenVectors(Vector axis, Vector v1, Vector v2)
            {
                Vector tmp = (v1 ^ v2);
                double cosAngle = v2 * v1;
                double angle = Math.Acos(cosAngle);
                if (Double.IsNaN(angle))
                    angle = 0;
                if (tmp * axis < 0) //cos 90+ is negative
                    angle = -angle;
                return angle;
            }
            #endregion
        }

        #endregion

        #region Point definition
        //[System.Diagnostics.DebuggerDisplay("{format(4)}", Name = "Point")]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Point : IEquatable<Point>, INullable, IBinarySerialize
        {
            public double x, y, z;
            private bool m_null;

            private string format(byte dig)
            {
                Point p = RoundTo(this, dig);
                return "(" + p.x.ToString() + "; " + p.y.ToString() + "; " + p.z.ToString() + ")";
            }

            public Point(double x, double y, double z)
            {
                m_null = false;
                this.x = x;
                this.y = y;
                this.z = z;
            }
            public Point(double x, double y) : this(x, y, 0) { }

            public void Translate(Vector vector)
            {
                x += vector.x;
                y += vector.y;
                z += vector.z;
            }

            public Point GetTranslated(Vector vector)
            {
                return this[vector];
            }

            public Point GetTransformed(Matrix3 matrix)
            {
                throw new NotImplementedException();
            }

            public void Transform(Matrix3 matrix)
            {
                throw new NotImplementedException();
            }

            public override int GetHashCode()
            {
                return base.GetHashCode();
            }            

            public void Round(uint digits)
            {
                double factor1 = default(double);
                double factor2 = factor1;               
                if (Factor(digits, out factor1, out factor2))
                {
                    x = (int)x;
                    y = (int)y;
                    z = (int)z;
                }
                else
                {
                    x = ((int)(x * factor2)) * factor1;
                    y = ((int)(y * factor2)) * factor1;
                    z = ((int)(z * factor2)) * factor1;
                }
            }            

            public void MirrorAtOrigin()
            {
                x = -x;
                y = -y;
                z = -z;
            }           

            public double DistanceTo(Point point)
            {
                return this[point].Length();
            }

            #region IEquatable<Point> Members
            
            public override bool Equals(object obj)
            {
                if (obj is Point)
                {
                    return Equals((Point)obj);
                }
                return false;
            }
            public bool Equals(Point point)
            {
                return this == point;
            }

            #endregion

            public bool IsOrigin { get { return ApproximatelyEqual(x, 0) && ApproximatelyEqual(y, 0) && ApproximatelyEqual(z, 0); } }
            
            #region INullable Members
            public bool IsNull { get { return m_null; } }
            #endregion
            
            /// <summary>
            /// Returns non normailzed vector from this point and another
            /// </summary>
            /// <param name="end">Another point</param>
            /// <returns></returns>
            public Vector this[Point end]
            {
                get { return new Vector(end.x - x, end.y - y, end.z - z); }
            }
            /// <summary>
            /// Returns point into new Transformation
            /// </summary>
            /// <param name="matrix">Transform matrix</param>
            /// <returns></returns>
            public Point this[Transform transform]
            {
                get
                {
                    return transform.TransformPoint(this);
                }
            }
            /// <summary>
            /// Returns translated point at direction <paramref name="vactor"/>
            /// </summary>
            /// <param name="vector">translate direction</param>
            /// <returns></returns>
            public Point this[Vector vector] { get { return this + vector; } }

            #region operators

            public static bool operator ==(Point p1, object p2)
            {
                if (p2 == default(object))
                    return false;
                return p1 == (Point)p2;
            }
            public static bool operator !=(Point p1, object p2)
            {
                if (p2 == default(object))
                    return true;
                return p1 != (Point)p2;
            }

            public static bool operator ==(Point p1, Point p2)
            {
                return
                   Math.Abs(p1.x - p2.x) < Accuracy
                  &&
                     Math.Abs(p1.y - p2.y) < Accuracy
                  &&
                    Math.Abs(p1.z - p2.z) < Accuracy;
            }
            public static bool operator !=(Point p1, Point p2)
            {
                return !(p1 == p2);
            }
            public static Point operator +(Point point, Vector vector)
            {
                return new Point(point.x + vector.x, point.y + vector.y, point.z + vector.z);
            }
            public static Point operator -(Point point, Vector vector)
            {
                return new Point(point.x - vector.x, point.y - vector.y, point.z - vector.z);
            }

            public static Vector operator -(Point point1, Point point2)
            {
                return new Vector(point1.x - point2.x, point1.y - point2.y, point1.z - point2.z);
            }
            public static Vector operator +(Point point1, Point point2)
            {
                return new Vector(point1.x + point2.x, point1.y + point2.y, point1.z + point2.z);
            }

            #endregion

            #region static members

            public static Point Null
            {
                get
                {
                    Point p = new Point();
                    p.m_null = true;
                    return p;
                }
            }

            #endregion

            #region IBinarySerialize Members

            public void Read(System.IO.BinaryReader r)
            {
                 m_null = r.ReadBoolean();
                 x = r.ReadDouble();
                 y = r.ReadDouble();
                 z = r.ReadDouble();
            }

            public void Write(System.IO.BinaryWriter w)
            {
                w.Write(m_null);
                w.Write(x);
                w.Write(y);
                w.Write(z);
            }

            #endregion
        }
        #endregion

        #region Plane definition
        //[System.Diagnostics.DebuggerDisplay("{format(4)}", Name = "Plane")]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Plane :INullable
        {
            private double[,] m_matr;
            private bool m_null;

            private string format(byte dig)
            {
                //Plane p = RoundTo(this, dig);
                //"(" + p.x.ToString() + "; " + p.y.ToString() + "; " + p.z.ToString() + ")";
                return string.Empty; 
            }            

            public Plane(Vector normal, Point point)
                : this()
            {
                m_matr = new double[3, 3];

                Vector perp1 = normal.GetPerpVector();
                Vector perp2 = perp1 ^ normal;

                Point p1 = point, p2 = point + perp1, p3 = point + perp2;

                m_matr[0, 0] = p1.x;
                m_matr[0, 1] = p1.y;
                m_matr[0, 2] = p1.z;

                m_matr[1, 0] = p2.x - p1.x;
                m_matr[1, 1] = p2.y - p1.y;
                m_matr[1, 2] = p2.z - p1.z;

                m_matr[2, 0] = p3.x - p1.x;
                m_matr[2, 1] = p3.y - p1.y;
                m_matr[2, 2] = p3.z - p1.z;

            }
            public Plane(Point p1, Point p2, Point p3)
                : this()
            {
                m_matr = new double[3, 3];

                m_matr[0, 0] = p1.x;
                m_matr[0, 1] = p1.y;
                m_matr[0, 2] = p1.z;

                m_matr[1, 0] = p2.x - p1.x;
                m_matr[1, 1] = p2.y - p1.y;
                m_matr[1, 2] = p2.z - p1.z;

                m_matr[2, 0] = p3.x - p1.x;
                m_matr[2, 1] = p3.y - p1.y;
                m_matr[2, 2] = p3.z - p1.z;
            }

            public Plane(Vector v1, Vector v2, Point p): this()
            {
                m_matr = new double[3, 3];
                Point p1 = p, p2 = p + v1, p3 = p + v2;

                m_matr[0, 0] = p1.x;
                m_matr[0, 1] = p1.y;
                m_matr[0, 2] = p1.z;

                m_matr[1, 0] = p2.x - p1.x;
                m_matr[1, 1] = p2.y - p1.y;
                m_matr[1, 2] = p2.z - p1.z;

                m_matr[2, 0] = p3.x - p1.x;
                m_matr[2, 1] = p3.y - p1.y;
                m_matr[2, 2] = p3.z - p1.z;
            }

            public bool Contains(Vector vector)
            {
                return Contains(new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2])[vector]);
            }

            public bool Contains(Point point)
            {
                double a = point.x - m_matr[0, 0];
                double b = point.y - m_matr[0, 1];
                double c = point.z - m_matr[0, 2];

                double det = a * m_matr[1, 1] * m_matr[2, 2];
                det += b * m_matr[1, 2] * m_matr[2, 0];
                det += c * m_matr[1, 0] * m_matr[2, 1];
                det -= c * m_matr[1, 1] * m_matr[2, 0];
                det -= b * m_matr[1, 0] * m_matr[2, 2];
                det -= a * m_matr[1, 2] * m_matr[2, 1];

                return ApproximatelyEqual(det, 0.0);
            }

            public double DistanceTo(Point point)
            {
                double[] x = new double[3];
                double[] y = new double[3];
                double[] z = new double[3];

                for (int i = 0; i < 3; ++i)
                {
                    x[i] = m_matr[i, 0];
                    y[i] = m_matr[i, 1];
                    z[i] = m_matr[i, 2];
                }

                double A = y[1] * z[2] - z[1] * y[2];
                double B = x[2] * z[1] - x[1] * z[2];
                double C = x[1] * y[2] - x[2] * y[1];

                double D = x[0] * (z[1] * y[2] - y[1] * z[2]);
                D += x[1] * (z[2] * y[0] - y[2] * z[0]);
                D += x[2] * (y[1] * z[0] - z[1] * y[0]);

                double retval = (A * point.x) + (B * point.y) + (C * point.z) + D;
                retval /= Math.Sqrt((A * A) + (B * B) + (C * C));

                return Math.Abs(retval);
            }

            public double DistanceTo(Plane plane)
            {
                return DistanceTo(new Point(plane.m_matr[0, 0], plane.m_matr[0, 1], m_matr[0, 2]));
            }

            public Point[] points
            {
                get
                {
                    Point[] retval = new Point[3];
                    retval[0] = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]);

                    Vector vec = new Vector(m_matr[1, 0], m_matr[1, 1], m_matr[1, 2]);
                    retval[1] = retval[0] + vec;

                    vec = new Vector(m_matr[2, 0], m_matr[2, 1], m_matr[2, 2]);
                    retval[2] = retval[0] + vec;

                    return retval;
                }
            }

            private Point intersect(Point a, Point b)
            {
                //sometimes (like in th bug #2307) we have very large inaccuracy
                //so we need to square our angle to prevent the error 
                double angle = this.AngleTo(a[b]);
                if (ApproximatelyEqual(Math.Pow(angle, 2), 0.0))
                    return Point.Null;

                Point p = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]);
                Vector l = new Vector(m_matr[1, 0], m_matr[1, 1], m_matr[1, 2]);
                Vector m = new Vector(m_matr[2, 0], m_matr[2, 1], m_matr[2, 2]);
                Vector val = l ^ m;
                Point q = a + ((b - a) * (((p - a) * val) / ((b - a) * val)));
                return q;
            }

            public Point[] Intersect(Plane plane)
            {
                Point[] ret = new Point[2];
                Point[] pts = points;
                List<Point[]> lines1 = new List<Point[]>();
                lines1.Add(new Point[] { pts[0], pts[1]});
                lines1.Add(new Point[] { pts[1], pts[2] });
                lines1.Add(new Point[] { pts[2], pts[0] });

                int k = 0;
                for (int i = 0; i < lines1.Count; ++i)
                    if (!plane.IsParallel(lines1[i][1][lines1[i][0]]) && k != 2)
                    {
                        ret[k] = plane.intersect(lines1[i][0], lines1[i][1]);
                        k++;
                    }

                if (k != 2)
                    return new Point[0];

                return ret;
            }

            public Vector ParallelVector() { return (Normal ^ new Vector(0, 0, 1)).GetNormalized(); }

            public Vector Normal
            {
                get
                {
                    double[] x = new double[3];
                    double[] y = new double[3];
                    double[] z = new double[3];

                    for (int i = 0; i < 3; ++i)
                    {
                        x[i] = m_matr[i, 0];
                        y[i] = m_matr[i, 1];
                        z[i] = m_matr[i, 2];
                    }

                    double A = y[1] * z[2] - z[1] * y[2];
                    double B = x[2] * z[1] - x[1] * z[2];
                    double C = x[1] * y[2] - x[2] * y[1];

                    return (new Vector(A, B, C).GetNormalized());
                }
            }
            
            public bool IsParallel(Plane p)
            {
                return Normal.IsParallelTo(p.Normal);
            }

            public bool IsParallel(Vector vector)
            {
                double angle = 0.0;
                angle = Normal.AngleTo(vector);
                return ApproximatelyEqual(angle, Math.PI) || ApproximatelyEqual(angle, 0) ;
            }

            public double AngleTo(Vector vector)
            {
                double angle = Normal.AngleTo(vector);
                angle = (angle < (Math.PI / 2)) ? (Math.PI / 2 - angle) : angle - (Math.PI / 2);
                return angle;
            }
            
            #region INullable Members

            public bool IsNull { get { return m_null; } }

            #endregion
            
            #region Projection
            public Point Projection(Point point)
            {
                double dist = DistanceTo(point);
                Vector norm = Normal.GetNormalized();
                Point pr = point - (norm * dist);
                if (!ApproximatelyEqual(DistanceTo(pr), 0))
                    pr = point + (norm * dist);
                return pr;
            }
            public Vector Projection(Vector vector)
            {
                Point p1 = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]);
                Point p2 = Projection(p1[vector]);
                return p1[p2];
            }
            #endregion

            #region Shift
            public void Shift(Vector direction)
            {
                Point
                    p1 = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]),
                    p2 = new Point(m_matr[1, 0] + p1.x, m_matr[1, 1] + p1.y, m_matr[1, 2] + p1.z),
                    p3 = new Point(m_matr[2, 0] + p1.x, m_matr[2, 1] + p1.y, m_matr[2, 2] + p1.z);

                p1 = p1 + direction;
                p2 = p2 + direction;
                p3 = p3 + direction;

                m_matr[0, 0] = p1.x;
                m_matr[0, 1] = p1.y;
                m_matr[0, 2] = p1.z;

                m_matr[1, 0] = p2.x - p1.x;
                m_matr[1, 1] = p2.y - p1.y;
                m_matr[1, 2] = p2.z - p1.z;

  …