/fpFormat/FormatModule/Gm.cs

#define L_ARM

using System;
using System.Data.SqlTypes;
using System.Collections.Generic;
using System.Collections;
using Microsoft.SqlServer.Server;
using System.Runtime.Serialization;
using SoftServe.TaskElementary.FormatModule;

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")]
        [Serializable]
        [DataContract]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Point : IEquatable<Point>, INullable, IBinarySerialize
        {
            [DataMember]
            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)
                {
                    var point = (Point)obj;
                    if (this.x.Equals(point.x) && this.y.Equals(point.y) && this.z.Equals(point.z))
                        return true;
                }
                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;

                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 void Shift(double distance)
            {
                Shift(Normal * distance);
            }
            public Plane GetShifted(Vector direction)
            {
                Plane plane = this;
                plane.Shift(direction);
                return plane;
            }
            public Plane GetShifted(double distance)
            {
                Plane plane = this;
                plane.Shift(distance);
                return plane;
            }
            #endregion

            public DecartPosition DetectDecartPart(Vector v)
            {
                if (
                    ApproximatelyEqual(v.x, 0)
                  & ApproximatelyEqual(v.y, 0)
                  & ApproximatelyEqual(v.z, 0))
                    return DecartPosition.Origin;

                if (v.x > 0 & v.y > 0)
                    return DecartPosition.At1;
                if (v.x > 0 & v.y < 0)
                    return DecartPosition.At2;
                if (v.x < 0 & v.y < 0)
                    return DecartPosition.At3;
                if (v.x < 0 & v.y > 0)
                    return DecartPosition.At4;

                if (v.x > 0 & v.y == 0)
                    return DecartPosition.AtY_PlusX;
                if (v.x < 0 & v.y == 0)
                    return DecartPosition.AtY_MinusX;

                if (v.x == 0 & v.y > 0)
                    return DecartPosition.AtX_PlusY;
                if (v.x == 0 & v.y < 0)
                    return DecartPosition.AtX_MinusY;
                
                throw new InvalidOperationException();
            }
            public DecartPosition DetectDecartPart(Point p)
            {
                if (
                    ApproximatelyEqual(p.x, 0)
                  & ApproximatelyEqual(p.y, 0)
                  & ApproximatelyEqual(p.z, 0))
                    return DecartPosition.Origin;

                if (p.x > 0 & p.y > 0)
                    return DecartPosition.At1;
                if (p.x > 0 & p.y < 0)
                    return DecartPosition.At2;
                if (p.x < 0 & p.y < 0)
                    return DecartPosition.At3;
                if (p.x < 0 & p.y > 0)
                    return DecartPosition.At4;

                if (p.x > 0 & p.y == 0)
                    return DecartPosition.AtY_PlusX;
                if (p.x < 0 & p.y == 0)
                    return DecartPosition.AtY_MinusX;

                if (p.x == 0 & p.y > 0)
                    return DecartPosition.AtX_PlusY;
                if (p.x == 0 & p.y < 0)
                    return DecartPosition.AtX_MinusY;

                throw new InvalidOperationException();
            }

            #region static members
            public static Plane Null
            {
                get
                {
                    Plane p = new Plane();
                    p.m_null = true;
                    return p;
                }
            }
            #endregion

        }
        #endregion

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


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

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

            public Quaterion(double x, double y, double z) : this(x, y, z, 0) { }
            /// <summary>
            /// 
            /// </summary>
            /// <param name="axis">Rotation axis</param>
            /// <param name="angle">In radians</param>
            public Quaterion(Vector axis, double angle) : this(axis.x, axis.y, axis.z, angle) { }
            /// <summary>
            /// Length of the quaterion
            /// </summary>
            public double Norma
            {
                get { return vector.Length2() + w*w; }
            }

            public Vector Vector
            {
                get { return vector; }
                set { vector = value; }
            }

            public double W
            {
                get { return w; }
                set { w = value; }
            }
            public double Magnitude { get { return Math.Sqrt(Norma); } }

            public Quaterion Conjugate
            {
                get
                {
                    Quaterion q = this;
                    q.vector = vector.GetInverse();
                    return q;
                }
            }

            public Quaterion Negate
            {
                get { return new Quaterion(-vector.x, -vector.y, -vector.z, -w); }
            }

            public double Angle { get { return Math.Acos(w) * 2.0; } }

            public void FromAngleAxis(double Angle, Vector axis)	// set the Quaternion by Angle-axis
            {
                double x = axis.x;
                double y = axis.y;
                double z = axis.z;

                // required: Normalize the axis

                double i_length = 1.0 / Math.Sqrt(x * x + y * y + z * z);

                x = x * i_length;
                y = y * i_length;
                z = z * i_length;

                // now make a clQuaternionernion out of it
                double Half = DegToRad(Angle * 0.5);

                w = Math.Cos(Half);//this used to be w/o deg to rad.

                double sin_theta_over_two = Math.Sin(Half);
                x = x * sin_theta_over_two;
                y = y * sin_theta_over_two;
                z = z * sin_theta_over_two;

                vector = new Vector(x, y, z);
            }

            public void FromAngleAxis2(double AngleRadians, Vector axis)
            {
                double s;
                s = Math.Sin(AngleRadians * 0.5f);
                w = Math.Cos(AngleRadians * 0.5f);
                vector.x = axis.x * s;
                vector.y = axis.y * s;
                vector.z = axis.z * s;
            }

            public void Normalize()
            {
                double mag = Norma;
                if (mag> 0)
                {
                    double factor = 1.0 / Math.Sqrt(mag);

                    vector.x *= factor;
                    vector.y *= factor;
                    vector.z *= factor;
                    w *= factor;
                }
            }

            public void Slerp(double t, Quaterion left, Quaterion q2)
            {
                double quatEpsilon = 1.0e-8;

                vector = left.vector;
                w = left.w;

                double cosine =
                    vector.x * q2.vector.x +
                    vector.y * q2.vector.y +
                    vector.z * q2.vector.z +
                    w * q2.w;		//this is left.dot(right)

                double sign = 1.0;
                if (cosine < 0)
                {
                    cosine = -cosine;
                    sign = -1.0;
                }

                double Sin = 1.0 - cosine * cosine;

                if (Sin >= quatEpsilon * quatEpsilon)
                {
                    Sin = Math.Sqrt(Sin);
                    double _angle = Math.Atan2(Sin, cosine);
                    double i_sin_angle = 1.0 / Sin;



                    double lower_weight = Math.Sin(_angle * (1.0 - t)) * i_sin_angle;
                    double upper_weight = Math.Sin(_angle * t) * i_sin_angle * sign;

                    w = (w * (lower_weight)) + (q2.w * (upper_weight));
                    vector.x = (vector.x * (lower_weight)) + (q2.vector.x * (upper_weight));
                    vector.y = (vector.y * (lower_weight)) + (q2.vector.y * (upper_weight));
                    vector.z = (vector.z * (lower_weight)) + (q2.vector.z * (upper_weight));
                }
            }
            /// <summary>
            /// returns axis and angle of rotation of quaternion
            /// </summary>
            /// <param name="angle"></param>
            /// <param name="axis"></param>
            public void GetAngleAxis(out double angle, ref Vector axis)
            {                
                angle = Math.Acos(w) * 2.0;
                angle = RadToDeg(angle);
                double sa = Math.Sqrt(1.0 - w * w);
                if (sa != 0)
                {
                    sa = 1 / sa;
                    axis.x = vector.x * sa;
                    axis.y = vector.y * sa;
                    axis.z = vector.z * sa;
                }
                else
                {
                    axis.x = 1.0;
                    axis.y = 0.0;
                    axis.z = 0.0;
                }
            }

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

            public Vector Rotate(Vector v)
            {
                Vector qv = vector;
                return (v * (w * w - 0.5) + (qv ^ v) * w + qv * (qv * v)) * 2;
            }
            public Vector InvRotate(Vector v)
            {
                Vector qv = vector;
                return (v * (w * w - 0.5) - (qv ^ v) * w + qv * (qv * v)) * 2;
            }

            public Vector Transform(Vector vec, Vector position) 
            {
                return Rotate(vec) + position;
            }
            public Vector InvTransform(Vector vec, Vector position)
            {
                return InvRotate(vec - position);
            }


            //public void Multiply(

            #region static members

            public static Quaterion operator +(Quaterion q1, Quaterion q2) 
            {
                return new Quaterion(q1.vector.x + q2.vector.x, q1.vector.y + q2.vector.y, q1.vector.z + q2.vector.z, q1.w + q2.w);
            }
            
            public static Quaterion operator -(Quaterion q1, Quaterion q2)
            {
                return new Quaterion(q1.vector.x - q2.vector.x, q1.vector.y - q2.vector.y, q1.vector.z - q2.vector.z, q1.w - q2.w);
            }

            public static Quaterion operator ^(Quaterion q1, Quaterion q2)
            {
                double a, b, c, d;

                a = q1.w * q2.w - q1.vector.x * q2.vector.x - q1.vector.y * q2.vector.y - q1.vector.z * q2.vector.z;
                b = q1.w * q2.vector.x + q2.w * q1.vector.x + q1.vector.y * q2.vector.z - q2.vector.y * q1.vector.z;
                c = q1.w * q2.vector.y + q2.w * q1.vector.y + q1.vector.z * q2.vector.x - q2.vector.z * q1.vector.x;
                d = q1.w * q2.vector.z + q2.w * q1.vector.z + q1.vector.x * q2.vector.y - q2.vector.x * q1.vector.y;
                return new Quaterion(b, c, d, a);
            }

            public static Quaterion operator ^(Quaterion q1, Vector q2)
            {
                double a, b, c, d;

                a = -q1.vector.x * q2.x - q1.vector.y * q2.y - q1.vector.z * q2.z;
                b = q1.w * q2.x + q1.vector.y * q2.z - q2.y * q1.vector.z;
                c = q1.w * q2.y + q1.vector.z * q2.x - q2.z * q1.vector.x;
                d = q1.w * q2.z + q1.vector.x * q2.y - q2.x * q1.vector.y;
                return new Quaterion(b, c, d, a);
            }

            public static double operator *(Quaterion q1, Quaterion q2)
            {
                return q1.vector.x * q2.vector.x + q1.vector.y * q2.vector.y + q1.vector.z * q2.vector.z + q1.w * q2.w;
            }
           
            public static Quaterion Null
            {
                get
                {
                    Quaterion q = new Quaterion();
                    q.m_null = true;
                    return q;
                }
            }

            public static Quaterion Identity { get { return new Quaterion(0, 0, 0, 1); } }

            #endregion

            #region INullable Members

            public bool IsNull
            {
                get { return m_null || vector.IsNull; }
            }

            #endregion
        }
        #endregion

        #region Matrix definition
       // [System.Diagnostics.DebuggerDisplay("{format(8)}", Name = "Matrix3")]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Matrix3: INullable, IEquatable<Matrix3>
        {
            private double[,] m_matrix;
            private bool m_null;

            private string format(byte dig)
            {
                return string.Empty;
            }


            public Matrix3(double[,] data)
            {
                m_null = false;
                m_matrix = new double[3, 3]
               {
                   {data[0,0], data[0,1], data[0,2]},
                   {data[1,0], data[1,1], data[1,2]},
                   {data[2,0], data[2,1], data[2,2]}
               };
            }
            public Matrix3(double[] data)
            {
                m_null = false;
                m_matrix = new double[3, 3]
               {
                   {data[0], data[1], data[2]},
                   {data[3], data[4], data[5]},
                   {data[6], data[7], data[8]}
               };
            }
            public Matrix3(Matrix3 matrix) : this(matrix.m_matrix) { }
            public Matrix3(Quaterion q):this()
            {
                m_matrix = new double[3,3];
                double w = q.W;
                double x = q.Vector.x;
                double y = q.Vector.y;
                double z = q.Vector.z;

                m_matrix[0,0] = 1.0 - y * y * 2.0 - z * z * 2.0;
                m_matrix[0, 1] = x * y * 2.0 - w * z * 2.0;
                m_matrix[0, 2] = x * z * 2.0 + w * y * 2.0;

                m_matrix[1, 0] = x * y * 2.0 + w * z * 2.0;
                m_matrix[1, 1] = 1.0 - x * x * 2.0 - z * z * 2.0;
                m_matrix[1, 2] = y * z * 2.0 - w * x * 2.0;

                m_matrix[2, 0] = x * z * 2.0 - w * y * 2.0;
                m_matrix[2, 1] = y * z * 2.0 + w * x * 2.0;
                m_matrix[2, 2] = 1.0 - x * x * 2.0 - y * y * 2.0;
            }
            /// <summary>
            /// Creates matrix from Euler angles
            /// </summary>
            /// <param name="X"></param>
            /// <param name="Y"></param>
            /// <param name="Z"></param>
            public Matrix3(double X, double Y, double Z): this()
            {
                m_matrix = new double[3, 3];

                X = DegToRad(X);
                Y = DegToRad(Y);
                Z = DegToRad(Z);

                Matrix3 RotZ = new Matrix3(
                    new Vector(Math.Cos(Z), -Math.Sin(Z), 0),
                    new Vector(Math.Sin(Z), Math.Cos(Z), 0),
                    new Vector(0, 0, 1));

                Matrix3 RotY = new Matrix3(
                    new Vector(Math.Cos(Y), 0, Math.Sin(Y)),
                    new Vector(0, 1, 0),
                    new Vector(-Math.Sin(Y), 0, Math.Cos(Y)));

                Matrix3 RotX = new Matrix3(
                    new Vector(1, 0, 0),
                    new Vector(0, Math.Cos(X), -Math.Sin(X)),
                    new Vector(0, Math.Sin(X), Math.Cos(X)));

                m_matrix = (RotX * RotY * RotZ).m_matrix;
            }
            /// <summary>
            /// Creates matrix from rotation an axis
            /// </summary>
            /// <param name="XX">Rotation at X-Axis</param>
            /// <param name="YY">Rotation at Y-Axis</param>
            /// <param name="ZZ">Rotation at Z-Axis</param>
            public Matrix3(Vector XX, Vector YY, Vector ZZ): this()
            {
                m_matrix = new double[3, 3];
                m_matrix[0, 0] = XX.x;
                m_matrix[0, 1] = XX.y;
                m_matrix[0, 2] = XX.z;

                m_matrix[1, 0] = YY.x;
                m_matrix[1, 1] = YY.y;
                m_matrix[1, 2] = YY.z;

                m_matrix[2, 0] = ZZ.x;
                m_matrix[2, 1] = ZZ.y;
                m_matrix[2, 2] = ZZ.z;
            }

            public Vector Column_get(int column)
            {
                return new Vector(
                    m_matrix[0, column]
                   ,m_matrix[1, column]
                   ,m_matrix[2, column]
                   );
            }
            public void Column_set(int column, Vector value)
            {
                m_matrix[0, column] = value.x;
                m_matrix[1, column] = value.y;
                m_matrix[2, column] = value.z;
            }

            public Vector Row_get(int row)
            {
                return new Vector(
                    m_matrix[row, 0]
                   ,m_matrix[row, 1]
                   ,m_matrix[row, 2]);
            }
            public void Row_set(int row, Vector value)
            {

                m_matrix[row, 0] = value.x;
                m_matrix[row, 1] = value.y;
                m_matrix[row, 2] = value.z;
            }

            public double[] RowMajor_get()
            {
                double[] ret = new double[9];

                ret[0] = m_matrix[0, 0];
                ret[1] = m_matrix[0, 1];
                ret[2] = m_matrix[0, 2];

                ret[3] = m_matrix[1, 0];
                ret[4] = m_matrix[1, 1];
                ret[5] = m_matrix[1, 2];

                ret[6] = m_matrix[2, 0];
                ret[7] = m_matrix[2, 1];
                ret[8] = m_matrix[2, 2];

                return ret;
            }
            public void RowMajor_set(double[] ret)
            {
                m_matrix[0, 0] = ret[0];
                m_matrix[0, 1] = ret[1];
                m_matrix[0, 2] = ret[2];

                m_matrix[1, 0] = ret[3];
                m_matrix[1, 1] = ret[4];
                m_matrix[1, 2] = ret[5];

                m_matrix[2, 0] = ret[6];
                m_matrix[2, 1] = ret[7];
                m_matrix[2, 2] = ret[8];
            }

            public double[] ColumnMajor_get() 
            {
                double[] ret = new double[9];

                ret[0] = m_matrix[0, 0];
                ret[1] = m_matrix[1, 0];
                ret[2] = m_matrix[2, 0];

                ret[3] = m_matrix[0, 1];
                ret[4] = m_matrix[1, 1];
                ret[5] = m_matrix[2, 1];

                ret[6] = m_matrix[0, 2];
                ret[7] = m_matrix[1, 2];
                ret[8] = m_matrix[2, 2];

                return ret;
            }
            public void ColumnMajor_set(double[] ret)
            {
                m_matrix[0, 0] = ret[0];
                m_matrix[1, 0] = ret[1];
                m_matrix[2, 0] = ret[2];

                m_matrix[0, 1] = ret[3];
                m_matrix[1, 1] = ret[4];
                m_matrix[2, 1] = ret[5];

                m_matrix[0, 2] = ret[6];
                m_matrix[1, 2] = ret[7];
                m_matrix[2, 2] = ret[8];
            }

            public double[] Diag_Main
            {
                get
                {
                    return new double[] { m_matrix[0, 0], m_matrix[1, 1], m_matrix[2, 2] };
                }
            }
            
            public double this[int row, int column]
            {
                get { return m_matrix[row, column]; }
                set { m_matrix[row, column] = value; }
            }

            /// <summary>
            /// Data of the matrix
            /// </summary>
            public double[] Data{
                get
                {
                    return new double[] {
                        m_matrix[0, 0], m_matrix[0, 1], m_matrix[0, 2]
                       ,m_matrix[1, 0], m_matrix[1, 1], m_matrix[1, 2]
                       ,m_matrix[2, 0], m_matrix[2, 1], m_matrix[2, 2]
                    };
                }
                set{
                   m_matrix[0, 0] = value[0]; m_matrix[0, 1] = value[1]; m_matrix[0, 2] = value[2];
                   m_matrix[1, 0] = value[3]; m_matrix[1, 1] = value[4]; m_matrix[1, 2] = value[5];
                   m_matrix[2, 0] = value[6]; m_matrix[2, 1] = value[7]; m_matrix[2, 2] = value[8];
                }
            }
            /// <summary>
            /// Returns the Determinant of this matrix
            /// </summary>
            public double Determinant
            {
                get
                {
                    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];
                }
            }
            /// <summary>
            /// Transorm this matrix to quaterion
            /// </summary>
            public Quaterion Quaterion
            {
                get
                {
                    Quaterion q = new Quaterion();
                    double tr, s;
                    double x, y, z;
                    tr = m_matrix[0, 0] + m_matrix[1, 1] + m_matrix[2, 2];
                    if (tr >= 0)
                    {
                        s = Math.Sqrt(tr + 1);
                        q.W = 0.5 * s;
                        s = 0.5 / s;
                        x = (this[2, 1] - this[1, 2]) * s;
                        y = (this[0, 2] - this[2, 0]) * s;
                        z = (this[1, 0] - this[0, 1]) * s;
                        q.Vector = new Vector(x, y, z);
                    }
                    else
                    {
                        int i = 0;
                        if (m_matrix[1, 1] > m_matrix[0, 0])
                            i = 1;
                        if (m_matrix[2, 2] > m_matrix[i, i])
                            i = 2;
                        switch (i)
                        {
                            case 0:
                                {
                                    s = Math.Sqrt((m_matrix[0, 0] - (m_matrix[1, 1] + m_matrix[2, 2])) + 1);
                                    x = 0.5 * s;
                                    s = 0.5 / s;
                                    y = (m_matrix[0, 1] + m_matrix[1, 0]) * s;
                                    z = (m_matrix[2, 0] + m_matrix[0, 2]) * s;
                                    q.W = (m_matrix[2, 1] - m_matrix[1, 2]) * s;
                                    q.Vector = new Vector(x, y, z);
                                    break;
                                }
                            case 1:
                                {
                                    s = Math.Sqrt((m_matrix[1, 1] - (m_matrix[2, 2] + m_matrix[0, 0])) + 1);
                                    y = 0.5 * s;
                                    s = 0.5 / s;
                                    z = (this[1, 2] + this[2, 1]) * s;
                                    x = (this[0, 1] + this[1, 0]) * s;
                                    q.W = (this[0, 2] - this[2, 0]) * s;
                                    break;
                                }
                            case 2:
                                {
                                    s = Math.Sqrt((m_matrix[2, 2] - (m_matrix[0, 0] + m_matrix[1, 1])) + 1);
                                    z = 0.5 * s;
                                    s = 0.5 / s;
                                    x = (this[2, 0] + this[0, 2]) * s;
                                    y = (this[1, 2] + this[2, 1]) * s;
                                    q.W = (this[1, 0] - this[0, 1]) * s;
                                    break;
                                }
                        }
                    }
                    return q;
                }
            }
            /// <summary>
            /// Get opposite matrix of this matrix
            /// </summary>
            public Matrix3 Inverse
            {
                get
                {
                    double d = Determinant;

                    if (ApproximatelyEqual(d, 0.0))
                    {                        
                        return Null;
                    }

                    d = 1.0 / d;
                    
                    double AlgExt11, AlgExt12, AlgExt13, AlgExt21, AlgExt22, AlgExt23, AlgExt31, AlgExt32, AlgExt33;
                    AlgExt11 = m_matrix[1, 1] * m_matrix[2, 2] - m_matrix[1, 2] * m_matrix[2, 1];
                    AlgExt21 = m_matrix[0, 2] * m_matrix[2, 1] - m_matrix[0, 1] * m_matrix[2, 2];
                    AlgExt31 = m_matrix[0, 1] * m_matrix[1, 2] - m_matrix[0, 2] * m_matrix[1, 1];
                    AlgExt12 = m_matrix[1, 2] * m_matrix[2, 0] - m_matrix[1, 0] * m_matrix[2, 2];
                    AlgExt22 = m_matrix[0, 0] * m_matrix[2, 2] - m_matrix[0, 2] * m_matrix[2, 0];
                    AlgExt32 = m_matrix[0, 2] * m_matrix[1, 0] - m_matrix[0, 0] * m_matrix[1, 2];
                    AlgExt13 = m_matrix[1, 0] * m_matrix[2, 1] - m_matrix[1, 1] * m_matrix[2, 0];
                    AlgExt23 = m_matrix[0, 1] * m_matrix[2, 0] - m_matrix[0, 0] * m_matrix[2, 1];
                    AlgExt33 = m_matrix[0, 0] * m_matrix[1, 1] - m_matrix[0, 1] * m_matrix[1, 0];

                    Matrix3 dest = new Matrix3();
                    dest.m_matrix = new double[,]{
                        {AlgExt11 * d, AlgExt21 * d, AlgExt31 * d},
                        {AlgExt12 * d, AlgExt22 * d, AlgExt32 * d},
                        {AlgExt13 * d, AlgExt23 * d, AlgExt33 * d},
                    };

                    ////Matrix3 dest = new Matrix3(new double[] { 
                    ////    AlgExt11 * d, AlgExt21 * d, AlgExt31 * d,
                    ////    AlgExt12 * d, AlgExt22 * d, AlgExt32 * d,
                    ////    AlgExt13 * d, AlgExt23 * d, AlgExt33 * d
                    ////});
                    
                    return dest;
                }
            }
            /// <summary>
            /// true if this matrix is identity
            /// </summary>
            public bool IsIdentity
            {
                get
                {
                    for (int i = 0; i < 3; i++)
                        for (int j = 0; j < 3; j++)
                            if (i != j)
                            { if (!ApproximatelyEqual(m_matrix[i, j], 0)) return false; }
                            else
                            { if (!ApproximatelyEqual(m_matrix[i, j], 1)) return false; }
                    
                    return true;
                }
            }
            /// <summary>
            /// True if this matrix is nullable
            /// </summary>
            public bool IsZero
            {
                get
                {
                    for (int i = 0; i < 3; i++)
                        for (int j = 0; j < 3; j++)
                            if (!ApproximatelyEqual(m_matrix[i, j], 0)) return false;
                    return true;
                }
            }

            public Vector XAxis { get { return new Vector(m_matrix[0, 0], m_matrix[0, 1], m_matrix[0, 2]); } }
            public Vector YAxis { get { return new Vector(m_matrix[1, 0], m_matrix[1, 1], m_matrix[1, 2]); } }
            public Vector ZAxis { get { return new Vector(m_matrix[2, 0], m_matrix[2, 1], m_matrix[2, 2]); } }

            /// <summary>
            /// Makes this matrix as nullable
            /// </summary>
            public void Zero()
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                        m_matrix[i, j] = 0;
            }
            /// <summary>
            /// Negate this matrix
            /// </summary>
            public void Negate()
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                        m_matrix[i, j] *= -1;
            }

            /// <summary>
            /// Rotates current matrix at X axis
            /// </summary>
            /// <param name="angle">Angle in radians</param>
            public void RotateXAxis(double angle)
            {              
                Matrix3 RotX = new Matrix3(
                    new Vector(1, 0, 0),
                    new Vector(0, Math.Cos(angle), -Math.Sin(angle)),
                    new Vector(0, Math.Sin(angle), Math.Cos(angle)));
                
                m_matrix = (this * RotX).m_matrix;
            }
            /// <summary>
            /// Rotates current matrix at Y axis
            /// </summary>
            /// <param name="angle">Angle in radians</param>
            public void RotateYAxis(double angle)
            {

                Matrix3 RotY = new Matrix3(
                    new Vector(Math.Cos(angle), 0, Math.Sin(angle)),
                    new Vector(0, 1, 0),
                    new Vector(-Math.Sin(angle), 0, Math.Cos(angle)));

                m_matrix = (this * RotY).m_matrix;
            }
            /// <summary>
            /// Rotates current matrix at Z axis
            /// </summary>
            /// <param name="angle">Angle in radians</param>
            public void RotateZAxis(double angle)
            {
                Matrix3 RotZ = new Matrix3(
                                 new Vector(Math.Cos(angle), -Math.Sin(angle), 0),
                                 new Vector(Math.Sin(angle), Math.Cos(angle), 0),
                                 new Vector(0, 0, 1));
                
                this.Multiply(RotZ);

                //m_matrix = (this * RotZ).m_matrix;
            }
            /// <summary>
            /// Transpose this matrix
            /// </summary>
            public void Transpose()
            {
                for (int i = 0; i < 3; i++)
                {
                    for (int j = 0; j < 3; j++)
                    {
                        double temp = m_matrix[i, j];
                        m_matrix[i, j] = m_matrix[j, i];
                        m_matrix[j, i] = temp;
                    }
                }
            }
            /// <summary>
            /// Returns rotated vector by this matrix
            /// </summary>
            /// <param name="vec">vector</param>
            /// <returns></returns>
            public Vector Multiply(Vector vec)
            {
                double x, y, z;
                
                x = m_matrix[0, 0] * vec.x + m_matrix[1, 0] * vec.y + m_matrix[2, 0] * vec.z;
                y = m_matrix[0, 1] * vec.x + m_matrix[1, 1] * vec.y + m_matrix[2, 1] * vec.z;
                z = m_matrix[0, 2] * vec.x + m_matrix[1, 2] * vec.y + m_matrix[2, 2] * vec.z;
                
                return new Vector(x, y, z);
            }            
            /// <summary>
            /// Returns rotated point by this matrix
            /// </summary>
            /// <param name="vec">point</param>
            /// <returns></returns>
            public Point Multiply(Point vec)
            {
                double x, y, z;

                x = m_matrix[0, 0] * vec.x + m_matrix[1, 0] * vec.y + m_matrix[2, 0] * vec.z;
                y = m_matrix[0, 1] * vec.x + m_matrix[1, 1] * vec.y + m_matrix[2, 1] * vec.z;
                z = m_matrix[0, 2] * vec.x + m_matrix[1, 2] * vec.y + m_matrix[2, 2] * vec.z;

                return new Point(x, y, z);
            }
            
            /// <summary>
            /// Returns rotated vector by this matrix (transposed)
            /// </summary>
            /// <param name="vec">vector</param>
            /// <returns></returns>
            public Vector MultiplyByTranspose(Vector vec)
            {
                double x, y, z;

                x = m_matrix[0, 0] * vec.x + m_matrix[0, 1] * vec.y + m_matrix[0, 2] * vec.z;
                y = m_matrix[1, 0] * vec.x + m_matrix[1, 1] * vec.y + m_matrix[1, 2] * vec.z;
                z = m_matrix[2, 0] * vec.x + m_matrix[2, 1] * vec.y + m_matrix[2, 2] * vec.z;

                return new Vector(x, y, z);
            }
            /// <summary>
            /// Adds matrix to current
            /// </summary>
            /// <param name="matrix"></param>
            public void Add(Matrix3 matrix)
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        m_matrix[i, j] += matrix.m_matrix[i, j];
                    }
            }
            /// <summary>
            /// Substract matrix from current
            /// </summary>
            /// <param name="matrix"></param>
            public void Substract(Matrix3 matrix)
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        m_matrix[i, j] -= matrix.m_matrix[i, j];
                    }
            }
            /// <summary>
            /// Multiplies matrix do double value
            /// </summary>
            /// <param name="value"></param>
            public void Multiply(double value)
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        m_matrix[i, j] *= value;
                    }
            }
            /// <summary>
            /// Multiply matrix to current
            /// </summary>
            /// <param name="matrix"></param>
            public void Multiply(Matrix3 matrix)
            {
                double a, b, c, d, e, f, g, h, i;
                
                //note: temps needed so that x.multiply(x,y) works OK.
                a = m_matrix[0, 0] * matrix.m_matrix[0, 0] + m_matrix[0, 1] * matrix.m_matrix[1, 0] + m_matrix[0, 2] * matrix.m_matrix[2, 0];
                b = m_matrix[0, 0] * matrix.m_matrix[0, 1] + m_matrix[0, 1] * matrix.m_matrix[1, 1] + m_matrix[0, 2] * matrix.m_matrix[2, 1];
                c = m_matrix[0, 0] * matrix.m_matrix[0, 2] + m_matrix[0, 1] * matrix.m_matrix[1, 2] + m_matrix[0, 2] * matrix.m_matrix[2, 2];

                d = m_matrix[1, 0] * matrix.m_matrix[0, 0] + m_matrix[1, 1] * matrix.m_matrix[1, 0] + m_matrix[1, 2] * matrix.m_matrix[2, 0];
                e = m_matrix[1, 0] * matrix.m_matrix[0, 1] + m_matrix[1, 1] * matrix.m_matrix[1, 1] + m_matrix[1, 2] * matrix.m_matrix[2, 1];
                f = m_matrix[1, 0] * matrix.m_matrix[0, 2] + m_matrix[1, 1] * matrix.m_matrix[1, 2] + m_matrix[1, 2] * matrix.m_matrix[2, 2];

                g = m_matrix[2, 0] * matrix.m_matrix[0, 0] + m_matrix[2, 1] * matrix.m_matrix[1, 0] + m_matrix[2, 2] * matrix.m_matrix[2, 0];
                h = m_matrix[2, 0] * matrix.m_matrix[0, 1] + m_matrix[2, 1] * matrix.m_matrix[1, 1] + m_matrix[2, 2] * matrix.m_matrix[2, 1];
                i = m_matrix[2, 0] * matrix.m_matrix[0, 2] + m_matrix[2, 1] * matrix.m_matrix[1, 2] + m_matrix[2, 2] * matrix.m_matrix[2, 2];

                // fill data
                m_matrix[0, 0] = a;
                m_matrix[0, 1] = b;
                m_matrix[0, 2] = c;

                m_matrix[1, 0] = d;
                m_matrix[1, 1] = e;
                m_matrix[1, 2] = f;

                m_matrix[2, 0] = g;
                m_matrix[2, 1] = h;
                m_matrix[2, 2] = i;
            }

            public void Round(uint digits)
            {
                double factor1 = default(double);
                double factor2 = factor1;
                if (Factor(digits, out factor1, out factor2))
                {
                    for (int i = 0; i < 3; i++)
                    {
                        for (int j = 0; j < 3; j++)
                        {
                            m_matrix[i, j] = (int)m_matrix[i, j];
                        }
                    }
                }
                else
                {
                    for (int i = 0; i < 3; i++)
                    {
                        for (int j = 0; j < 3; j++)
                        {
                            m_matrix[i, j] = ((int)(m_matrix[i, j] * factor2)) * factor1;
                        }
                    }
                }
            }

            #region operators

            public static bool operator ==(Matrix3 m1, Matrix3 m2)
            {
                if (m1 == default(Matrix3) | m1 == default(Matrix3))
                {
                    return false;
                }
                if (m1.m_null | m2.m_null)
                    return false;       
                
                if (System.Object.ReferenceEquals(m1, m2))
                {
                    return true;
                }


                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                        if (!ApproximatelyEqual(m1.m_matrix[i, j], m2.m_matrix[i, j]))
                            return false;

                return true;
            }
            public static Matrix3 operator *(Matrix3 m1, Matrix3 m2)
            {
                double a, b, c, d, e, f, g, h, i;

                //note: temps needed so that x.multiply(x,y) works OK.
                a = m1.m_matrix[0, 0] * m2.m_matrix[0, 0] + m1.m_matrix[0, 1] * m2.m_matrix[1, 0] + m1.m_matrix[0, 2] * m2.m_matrix[2, 0];
                b = m1.m_matrix[0, 0] * m2.m_matrix[0, 1] + m1.m_matrix[0, 1] * m2.m_matrix[1, 1] + m1.m_matrix[0, 2] * m2.m_matrix[2, 1];
                c = m1.m_matrix[0, 0] * m2.m_matrix[0, 2] + m1.m_matrix[0, 1] * m2.m_matrix[1, 2] + m1.m_matrix[0, 2] * m2.m_matrix[2, 2];

                d = m1.m_matrix[1, 0] * m2.m_matrix[0, 0] + m1.m_matrix[1, 1] * m2.m_matrix[1, 0] + m1.m_matrix[1, 2] * m2.m_matrix[2, 0];
                e = m1.m_matrix[1, 0] * m2.m_matrix[0, 1] + m1.m_matrix[1, 1] * m2.m_matrix[1, 1] + m1.m_matrix[1, 2] * m2.m_matrix[2, 1];
                f = m1.m_matrix[1, 0] * m2.m_matrix[0, 2] + m1.m_matrix[1, 1] * m2.m_matrix[1, 2] + m1.m_matrix[1, 2] * m2.m_matrix[2, 2];

                g = m1.m_matrix[2, 0] * m2.m_matrix[0, 0] + m1.m_matrix[2, 1] * m2.m_matrix[1, 0] + m1.m_matrix[2, 2] * m2.m_matrix[2, 0];
                h = m1.m_matrix[2, 0] * m2.m_matrix[0, 1] + m1.m_matrix[2, 1] * m2.m_matrix[1, 1] + m1.m_matrix[2, 2] * m2.m_matrix[2, 1];
                i = m1.m_matrix[2, 0] * m2.m_matrix[0, 2] + m1.m_matrix[2, 1] * m2.m_matrix[1, 2] + m1.m_matrix[2, 2] * m2.m_matrix[2, 2];

                // fill data
                Matrix3 ret = new Matrix3();
                ret.m_matrix = new double[3, 3];

                ret.m_matrix[0, 0] = a;
                ret.m_matrix[0, 1] = b;
                ret.m_matrix[0, 2] = c;

                ret.m_matrix[1, 0] = d;
                ret.m_matrix[1, 1] = e;
                ret.m_matrix[1, 2] = f;

                ret.m_matrix[2, 0] = g;
                ret.m_matrix[2, 1] = h;
                ret.m_matrix[2, 2] = i;

                return ret;
            }
            public static Matrix3 operator *(Matrix3 m1, double s)
            {
                Matrix3 ret = new Matrix3();
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        ret.m_matrix[i, j] = m1.m_matrix[i, j] * s;
                    }
                return ret;
            }
            public static Matrix3 operator -(Matrix3 m1, Matrix3 m2)
            {
                Matrix3 ret = new Matrix3();
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        ret.m_matrix[i, j] = m1.m_matrix[i, j] - m2.m_matrix[i, j];
                    }
                return ret;
            }
            public static Matrix3 operator +(Matrix3 m1, Matrix3 m2)
            {
                Matrix3 ret = new Matrix3();
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        ret.m_matrix[i, j] = m1.m_matrix[i, j] + m2.m_matrix[i, j];
                    }
                return ret;
            }
            public static bool operator !=(Matrix3 m1, Matrix3 m2)
            {
                return !(m1 == m2);
            }

            #endregion

            #region static memebers

            public static Matrix3 Null
            {
                get
                {
                    Matrix3 m = new Matrix3();
                    m.m_null = true;
                    m.m_matrix = new double[,]{ 
                       {0, 0, 0},
                       {0, 0, 0},
                       {0, 0, 0}
                   };
                    return m;
                }
            }
            public static Matrix3 Identity
            {
                get
                {
                    Matrix3 m = new Matrix3();
                    m.m_matrix = new double[,]{ 
                       {1, 0, 0},
                       {0, 1, 0},
                       {0, 0, 1}
                    };
                    return m;
                }
            }

            
            #endregion

            #region INullable Members

            public bool IsNull
            {
                get { return  m_matrix == null | m_null; }
            }

            #endregion

            #region IEquatable<Matrix3> Members

            public bool Equals(Matrix3 matrix)
            {                           
                return this == matrix;
            }

            public override bool Equals(object obj)
            {
                if (obj is Matrix3)
                    return base.Equals((Matrix3)obj);
                else return false;
            }            
            
            public override int GetHashCode()
            {
                return base.GetHashCode();
            }

            #endregion

            public Matrix3 Clone()
            {
                Matrix3 matrix = new Matrix3();
                matrix.m_matrix = new double[3, 3];
                matrix.Data = Data;

                return matrix;
            }
        }
        #endregion

        #region Transform definition
        //[System.Diagnostics.DebuggerDisplay("{format(8)}", Name = "Transform")]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Transform : INullable, IEquatable<Transform>
        {
            private Matrix3 rotation;
            private Vector translation;
            private double scale;
            private bool m_null;

            private string format(byte dig)
            {
                Transform t = RoundTo(this, dig);
                return string.Empty;
            }

            public Transform(double[] data): this()
            {
                if (rotation.IsNull)
                    rotation = Matrix3.Identity;

                rotation.RowMajor_set(data);
                translation = new Vector(data[9], data[10], data[11]);
                scale = data[12];
            }
            
            public double[] Data
            {
                get {
                    double[] value = new double[16];
                    //Rotation data
                    double[] rot = rotation.RowMajor_get();                    
                    for (int i = 0; i < rot.Length; i++)
                        value[i] = rot[i];
                    //Translation data
                    value[9] = translation.x;
                    value[10] = translation.y;
                    value[11] = translation.z;
                    //scale date
                    value[12] = scale; //scaling
                    
                    return value;
                }
                set
                {
                    //set Rotation data
                    rotation.RowMajor_set(value);
                    //set Translation data
                    translation.x = value[9];
                    translation.y = value[10];
                    translation.z = value[11];
                    //scale date
                    scale = value[12];
                }
            }

            public double Scale { get { return scale; } 
                //set { scale = value; } 
            }

            public Matrix3 Rotation
            {
                get { return rotation; }
                set { rotation = value; }
            }

            public Vector Translation
            {
                get { return translation; }
                set { translation = value; }
            }

            public Transform Inverse
            {
                get
                {
                    if (m_null)
                        return Null;
                   
                    Transform t = Transform.Identity;
                    t.Rotation.RowMajor_set(rotation.Inverse.RowMajor_get());
                    t.translation = rotation.Inverse.Multiply(translation * -1.0);
                    return t;
                }
            }

            public void Multiply(Transform t)
            {
                translation = t.rotation.Multiply(translation) + t.translation;
                rotation.Multiply(t.rotation);
            }

            public Point TransformPoint(Point point)
            {
                return rotation.Multiply(point) + translation;
            }

            public Vector TransformVector(Vector vector)
            {
                return rotation.Multiply(vector);
            }
            public void RotateXAxis(double angle) { rotation.RotateXAxis(angle); }
            public void RotateYAxis(double angle) { rotation.RotateYAxis(angle); }
            public void RotateZAxis(double angle) { rotation.RotateZAxis(angle); }

            #region INullable Members

            public bool IsNull
            {
                get { return m_null || rotation.IsNull || translation.IsNull; }
            }

            #endregion

            #region IEquatable<Transform> Members

            public bool Equals(Transform other)
            {
                throw new Exception("The method or operation is not implemented.");
            }

            #endregion

            #region static members
            public static Transform Null
            {
                get
                {
                    Transform t = new Transform();
                    t.rotation = Matrix3.Null;
                    t.translation = Vector.Null;
                    t.m_null = true;
                    return t;
                }
            }
            /// <summary>
            /// Returns identity transformation
            /// </summary>
            public static Transform Identity
            {
                get {
                    Transform t = new Transform();
                    t.rotation = Matrix3.Identity;
                    t.translation = new Vector();
                    t.scale = 1.0;
                    return t;
                }
            }
            #endregion

            public Transform Clone()
            {
                return new Transform(Data);
            }
        }
        #endregion

        #region Basis definition
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Basis : INullable
        {
            private Point point;
            private Transform transform;
            private Plane xyPlane;
            private Plane xzPlane;
            private Plane yzPlane;
            private bool m_null;

            public Point Point { get { return point; } }
            public Vector XAxis { get { return transform.Rotation.XAxis; } }
            public Vector YAxis { get { return transform.Rotation.YAxis; } }
            public Vector ZAxis { get { return transform.Rotation.ZAxis; } }
            public Plane XYPlane { get { return xyPlane; } }
            public Plane YZPlane { get { return yzPlane; } }
            public Plane XZPlane { get { return xzPlane; } }
            public Transform Transform { get { return transform; } }

            public Basis(Point point, Vector normal)
                : this()
            {
                this.point = point;
                Vector x = normal.GetPerpVector();
                Vector y = x ^ normal;
                Vector z = normal;
                x.Inverce();
                transform = new Transform();
                transform.Rotation = new Matrix3(x, y, z);
                transform.Translation = new Point()[point];
                xyPlane = new Plane(x, y, point);
                xzPlane = new Plane(x, z, point);
                yzPlane = new Plane(y, z, point);
            }

            public Basis(Point point, Vector X, Vector Y, Vector Z)
                : this()
            {
                this.point = point;
                transform = new Transform();
                transform.Rotation = new Matrix3(X, Y, Z);
                transform.Translation = new Point()[point];
                xyPlane = new Plane(X, Y, point);
                xzPlane = new Plane(X, Z, point);
                yzPlane = new Plane(Y, Z, point);
            }

            public Basis(Point point, double alpha, double beta, double gamma)
                : this()
            {
                this.point = point;
                transform = new Transform();
                transform.Rotation = new Matrix3(alpha, beta, gamma);
                transform.Translation = new Point()[point];
                xyPlane = new Plane(transform.Rotation.XAxis, transform.Rotation.YAxis, point);
                xzPlane = new Plane(transform.Rotation.XAxis, transform.Rotation.ZAxis, point);
                yzPlane = new Plane(transform.Rotation.YAxis, transform.Rotation.ZAxis, point);
            }

            #region INullable Members

            public bool IsNull
            {
                get { return m_null | transform.IsNull; }
            }

            #endregion

            public static Basis STOCK
            {
                get
                {
                    Basis u = new Basis();
                    u.transform = new Transform();
                    Vector x = new Vector(1, 0, 0);
                    Vector y = new Vector(0, 1, 0);
                    Vector z = new Vector(0, 0, 1);
                    u.transform.Rotation = Matrix3.Identity;
                    u.xyPlane = new Plane(x, y, new Point());
                    u.xzPlane = new Plane(x, z, new Point());
                    u.yzPlane = new Plane(y, z, new Point());
                    return u;
                }
            }

            public static Basis Null
            {
                get
                {
                    Basis u = new Basis();
                    u.m_null = true;
                    u.transform = Transform.Null;
                    u.xyPlane = Plane.Null;
                    u.xzPlane = Plane.Null;
                    u.yzPlane = Plane.Null;

                    return u;
                }
            }

            public Vector ToSTOCK(Vector vector)
            {
                return vector[transform.Inverse];
            }
            public Point ToSTOCK(Point point)
            {
                return point[transform.Inverse];
            }
            public Vector ToUCS(Vector vector)
            {
                return vector[transform];
            }
            public Point ToUCS(Point point)
            {
                return point[transform];
            }

            internal Basis Clone()
            {
                return new Basis(point, XAxis, YAxis, ZAxis);
            }
        }
        #endregion

        #region Beam definition
        public struct Beam : IEquatable<Beam>, INullable
        {
            private Vector _direction;
            private Point _point;
            private bool m_null;

            #region IEquatable<Beam> Members
            public override bool Equals(object obj)
            {
                return base.Equals(obj);
            }
            public bool Equals(Beam point)
            {
                return this == point;
            }

            public override int GetHashCode()
            {
                return _direction.GetHashCode() ^ _point.GetHashCode() ^ m_null.GetHashCode();
            }
            #endregion

            #region operators
            public static bool operator ==(Beam b1, object b2)
            {
                if (b2 == default(object))
                    return false;
                return b1 == (Beam)b2;
            }
            public static bool operator !=(Beam b1, object b2)
            {
                if (b2 == default(object))
                    return true;
                return b1 != (Beam)b2;
            }
            public static bool operator ==(Beam b1, Beam b2)
            {
                return
                   b1.Point == b2.Point
                  &&
                   b1.Direction == b2.Direction;
            }
            public static bool operator !=(Beam b1, Beam b2)
            {
                return !(b1 == b2);
            }
            #endregion

           
            public Vector Direction { get { return _direction; } }
            public Point Point { get { return _point; } }

            public Beam(Point point, Vector direction)
                : this()
            {
                m_null = false;
                _point = point;
                _direction = direction;
            }

            #region static members
            public static Beam Null
            {
                get
                {
                    Beam p = new Beam();                    
                    p._point = Point.Null;
                    p.m_null = true;
                    return p;
                }
            }
            public static bool IsSame(Beam beam1, Beam beam2)
            {
                if (beam2.Direction.IsParallelTo(beam1.Direction))
                {
                    if (beam2.Point - beam1.Point == beam1.Direction)
                        return true;
                    if (beam1.Point - beam2.Point == beam1.Direction)
                        return true;
                }
                return true;
            }
            public static bool Intersect(Beam beam1, Beam beam2, out Point point)
            {
                point = default(Point);
                if (!IsSame(beam1, beam2))
                {
                    if (beam1.Point == beam2.Point)
                    {
                        point = beam2.Point;
                        return true;
                    }
                    double ortLength = 0.0001;
                    Vector A = new Vector(beam1.Point.x, beam1.Point.y, beam1.Point.z);
                    Vector B = beam1.Point - beam2.Point[beam1.Direction.GetNormalized() * ortLength];

                    Vector C = new Vector(beam2.Point.x, beam2.Point.y, beam2.Point.z);
                    Vector D = beam2.Point - beam2.Point[beam2.Direction.GetNormalized() * ortLength];

                    double s;
                    if (!ApproximatelyEqual((D.x * B.y) - (D.y * B.x), 0))
                    {

                        s = (C.y * B.x) - (A.y * B.x) - (C.x * B.y) + (A.x * B.y);
                        if (ApproximatelyEqual(s, 0))
                            s = 0;

                        if (ApproximatelyEqual((D.x * B.y) - (D.y * B.x), 0))
                            s = 0;
                        else
                            s /= (D.x * B.y) - (D.y * B.x);

                        s = 1 - s;

                        C = C + (D * (1 - s));
                        point = new Point(C.x, C.y, C.z);
                    }
                    else
                        if (!ApproximatelyEqual((D.x * B.z) - (D.z * B.x), 0))
                        {
                            s = (C.z * B.x) - (A.z * B.x) - (C.x * B.z) + (A.x * B.z);
                            if (ApproximatelyEqual(s, 0))
                                s = 0;

                            if (ApproximatelyEqual((D.x * B.z) - (D.z * B.x), 0))
                                s = 0;
                            else
                                s /= (D.x * B.z) - (D.z * B.x);

                            s = 1 - s;
                            C = C + (D * (1 - s));
                            point = new Point(C.x, C.y, C.z);
                        }
                        else
                            if (!ApproximatelyEqual((D.y * B.z) - (D.z * B.y), 0))
                            {
                                s = (C.z * B.y) - (A.z * B.y) - (C.y * B.z) + (A.y * B.z);
                                if (ApproximatelyEqual(s, 0))
                                    s = 0;

                                if (ApproximatelyEqual((D.y * B.z) - (D.z * B.y), 0))
                                    s = 0;
                                else
                                    s /= (D.y * B.z) - (D.z * B.y);

                                s = 1 - s;

                                C = C + (D * (1 - s));
                                point = new Point(C.x, C.y, C.z);
                            }

                    return true;
                }
                else return false;
            }            
            #endregion

            #region INullable Members
            public bool IsNull { get { return m_null || _point.IsNull || _direction.IsNull; } }
            #endregion
        }
        #endregion

        #region Curve definition

        public struct Curve
        {
            private int _cType;
            
            public int CType
            {
                get { return _cType; }
                set { _cType = value; }
            }

        }
        #endregion


        public class Link<T>
        {
            private T _value = default(T);
            private Link<T> _Next = null;
            private Link<T> _Prev = null;
            /// <summary>
            /// Возвращает звено, в котором содержиться елемент
            /// </summary>
            /// <param name="element">Элемент</param>
            /// <returns></returns>
            private Link<T> _chain(T element) 
            {
                if (element == null)
                    return null;

                Link<T> _p = _Prev;
                Link<T> _n = _Next;
                
                if (element.Equals(_value))
                    return this;
                
                while (!(_p ==null | _n == null))
                {
                    if (_p != null)
                    {
                        if (element.Equals(_p._value))
                        {
                            return _p;
                        }
                        _p = _p._Prev;
                    }
                    if (_n != null)
                    {
                        if (element.Equals(_n._value))
                        {
                            return _n;
                        }
                        _n = _n._Next;
                    }
                }
                return null;
            }
            /// <summary>
            /// Исключает текущее звено из цепи
            /// </summary>
            public void Exclude()
            {
                if (!IsBound)
                {
                    _Next._Prev = _Prev._Next;
                    return;
                }
                if (IsLBound) 
                {
                    _Next._Prev = null;
                    return;
                }
                if (IsRBound)
                {
                    _Prev._Next = null;
                    return;
                }
                this._Prev = null;
                this._Next = null;
            }
            /// <summary>
            /// Добавляет элемент в последовательность
            /// </summary>
            /// <param name="element">Добавляемый элемент</param>
            /// <param name="after">В зависимости от значения, перед или после текущего элемента цепи</param>
            public void Include(T element, bool after) 
            {
                Link<T> el = new Link<T>();
                el._value = element;

                if (after) 
                {
                    if (IsRBound)
                    {
                        _Next = el;
                        el._Prev = this;
                    }
                    if (IsLBound)
                    {
                        el._Prev = _Next;
                        _Next = el;
                        el._Prev = this;
                    }
                }
            }
            /// <summary>
            /// Перемещает текущее звено после заданного или перед ним
            /// </summary>
            /// <param name="chain">Заданное звено</param>
            public void MoveTo(Link<T> chain, bool after) {  }
            /// <summary>
            /// True, если звено имеет неопределенное значение
            /// </summary>
            public bool IsNull { get { return _value != null ? _value.Equals(default(T)) : true; } }
            /// <summary>
            /// Значение
            /// </summary>
            public T Value { get { return _value; } }
            /// <summary>
            /// Определяет звено, в котором находится элемент
            /// </summary>
            /// <param name="element"></param>
            /// <returns></returns>
            public Link<T> Chain(T element) { return _chain(element); }
            public bool IsRBound { get { return _Next == null; } }
            public bool IsLBound { get { return _Prev == null; } }
            public bool IsBound { get { return IsRBound || IsLBound; } }
        }

        public class FlatPolygon
        {
            private double triangSquareGeron2(Point p1, Point p2, Point p3)
            {
                double a = Length2(p1, p2);
                double b = Length2(p2, p3);
                double c = Length2(p1, p3);
                double p = 0.5 * (a + b + c);

                if ((p - a < 0) | (p - b < 0) | (p - c < 0))
                    return double.NaN;

                return p * (p - a) * (p - b) * (p - c);
            }
            
            private void getExtremumIndexs(ref Point[] points, byte dwFlag, out int minIndex, out int maxIndex)
            {
                minIndex = -1; 
                maxIndex = -1;

                if (dwFlag < 1 | dwFlag > 3 | points.Length  <1)
                    return;               
                
                double minVal = default(double);
                double maxVal = default(double);
                /// set initial values
                /// 
                minIndex = 0;
                maxIndex = 0;
                /// set x component
                /// 
                if (dwFlag == 1)
                {
                        minVal = points[0].x;
                        maxVal = points[0].x;
                }
                /// set y component
                /// 
                if (dwFlag == 2)
                {
                    minVal = points[0].y;
                    maxVal = points[0].y;
                }
                /// set z component
                /// 
                if (dwFlag == 2)
                {
                    minVal = points[0].z;
                    maxVal = points[0].z;
                }
                /// scan throug all poins
                /// 
                for (int i = 1; i < points.Length; i++)
                {
                    /// test x component
                    /// 
                    if (dwFlag == 1)
                    {
                        if (minVal > points[i].x)
                        {
                            minIndex = i;
                            minVal = points[i].x;
                        }
                        if (maxVal < points[i].x)
                        {
                            maxIndex = i;
                            maxVal = points[i].x;
                        }
                        continue;
                    }
                    /// test y component
                    /// 
                    if (dwFlag == 2)
                    {
                        if (minVal > points[i].y)
                        {
                            minIndex = i;
                            minVal = points[i].y;
                        }
                        if (maxVal < points[i].y)
                        {
                            maxIndex = i;
                            maxVal = points[i].y;
                        }
                        continue;
                    }
                    /// test z component
                    /// 
                    if (dwFlag == 3)
                    {
                        if (minVal > points[i].z)
                        {
                            minIndex = i;
                            minVal = points[i].z;
                        }
                        if (maxVal < points[i].z)
                        {
                            maxIndex = i;
                            maxVal = points[i].z;
                        }
                        continue;
                    }
                }
            }
            private bool getExtremumIndexs(IEnumerator<Point> points, byte dwFlag, out Point minPoint, out Point maxPoint)
            {
                minPoint = default(Point);
                maxPoint = default(Point);
                points.Reset();
                if (dwFlag < 1 | dwFlag > 3 | !points.MoveNext())
                    return false;

                double minVal = default(double);
                double maxVal = default(double);
                /// set x component
                /// 
                if (dwFlag == 1)
                {
                    minVal = points.Current.x;
                    maxVal = points.Current.x;
                    minPoint = points.Current;
                    maxPoint = points.Current;
                }
                /// set y component
                /// 
                if (dwFlag == 2)
                {
                    minVal = points.Current.y;
                    maxVal = points.Current.y;
                    minPoint = points.Current;
                    maxPoint = points.Current;
                }
                /// set z component
                /// 
                if (dwFlag == 2)
                {
                    minVal = points.Current.z;
                    maxVal = points.Current.z;
                    minPoint = points.Current;
                    maxPoint = points.Current;
                }
                int counter = 0;
                /// scan throug all poins
                /// 
                while (points.MoveNext())
                {
                    counter++;
                    /// test x component
                    /// 
                    if (dwFlag == 1)
                    {
                        if (minVal > points.Current.x)
                        {
                            minVal = points.Current.x;
                            minPoint = points.Current;
                        }
                        if (maxVal < points.Current.x)
                        {
                            maxPoint = points.Current; 
                            maxVal = points.Current.x;
                        }
                        continue;
                    }
                    /// test y component
                    /// 
                    if (dwFlag == 2)
                    {
                        if (minVal > points.Current.y)
                        {
                            minVal = points.Current.y;
                            minPoint = points.Current; 
                        }
                        if (maxVal < points.Current.y)
                        {
                            maxPoint = points.Current; 
                            maxVal = points.Current.y;
                        }
                        continue;
                    }
                    /// test z component
                    /// 
                    if (dwFlag == 3)
                    {
                        if (minVal > points.Current.z)
                        {
                            minVal = points.Current.z;
                            minPoint = points.Current;
                        }
                        if (maxVal < points.Current.z)
                        {
                            maxPoint = points.Current; 
                            maxVal = points.Current.z;
                        }
                        continue;
                    }
                }
                return counter > 0;
            }
            
            /// <summary>
            /// returns true if set contains only 2 points l and r
            /// </summary>
            /// <param name="iEnum">point set</param>
            /// <param name="l"></param>
            /// <param name="r"></param>
            /// <returns></returns>
            private bool compareSetLR (IEnumerator<Point> iEnum, Point l, Point r)
            {
                if (iEnum == null)
                    throw new ArgumentNullException();

                iEnum.Reset();

                if (iEnum.MoveNext()) 
                {
                    if ((l != iEnum.Current) & r != iEnum.Current)
                        return false;
                }
                if (iEnum.MoveNext())
                {
                    if ((l != iEnum.Current) & r != iEnum.Current)
                        return false;
                }
                return !iEnum.MoveNext();
            }

            private Point[] qConvex(IEnumerable<Point> values, Point l, Point r) 
            {                
                if (compareSetLR(values.GetEnumerator(), l, r))
                    return new Point[] { l, r };
                else
                {
                    Point h = default(Point);
                    Dictionary<bool, IList<Point>> conv = new Dictionary<bool, IList<Point>>();
                    /// upper part
                    /// 
                    conv.Add(true, new List<Point>());
                    /// lower part
                    /// 
                    conv.Add(false, new List<Point>());
                    /// collect athother points (sort points on parts)
                    /// 

                    if (!getFarsetPoint(values, l, r, out h))
                        if (l[r].Length2() > Accuracy)
                            throw new InvalidOperationException("far point not found. values is line");
                        else
                        {
                            if (!getExtremumIndexs(values.GetEnumerator() as IEnumerator<Point>, 1, out l, out r))
                                throw new InvalidOperationException("far point not found. values is line");
                            if (!getFarsetPoint(values, l, r, out h))
                                throw new InvalidOperationException("far point not found. values is line");
                        }
                       // return new Point[0];

                    foreach (Point point in values)
                    {
                        bool isExists = false;
                        if (!(Gm.iLeftOrRight(point, l, h) < 0))
                        {
                            for (int i = 0; i < conv[true].Count; i++)
                            {
                                if (conv[true][i] == point)
                                {
                                    isExists = true;
                                    break;
                                }
                            }
                            if (!isExists)
                            conv[true].Add(point);
                            continue;
                        }
                        if (!(Gm.iLeftOrRight(point, h, r) > 0))
                        {                           
                            for (int i = 0; i < conv[false].Count; i++)
                            {
                                if (conv[false][i] == point)
                                {
                                    isExists = true;
                                    break;
                                }
                            }
                            if (!isExists)
                                conv[false].Add(point);
                            continue;
                        }
                        /// auxillary step 
                        /// ?
                        //double dRet = Gm.iLeftOrRight(point, l,r) ;
                        //if (Math.Abs( dRet ) < Accuracy)
                        //    continue;
                        //if (dRet > 0)
                        //    conv[true].Add(point);
                        //else
                        //    conv[false].Add(point);
                    }     
                    
                    List<Point> ret = new List<Point>();
                    if (conv[true].Count > 0)
                        ret.AddRange(qConvex(conv[true], l, h));
                    ///remove duplicat-point
                    ///
                    for (int i = 0; i < conv[false].Count; i++)
                    {
                        if (conv[false][i] == h)
                        {
                            conv[false].RemoveAt(i);
                            break;
                        }
                    }
                    if (conv[false].Count > 0)
                        ret.AddRange(qConvex(conv[false], h, r));

                    return ret.ToArray();
                }
            }

            private bool getFarsetPoint(IEnumerable<Point> values, Point l, Point r, out Point tPoint)
            {
                tPoint = default(Point);
                ///get a point enumerator
                ///
                IEnumerator<Point> pointEnum = values.GetEnumerator();
                /// prepare values
                /// 
                pointEnum.Reset();
                if (!pointEnum.MoveNext())
                    return false;
                double max = Math.Abs( TriangSquareGeron(pointEnum.Current, l, r));
                /// add first point to set
                /// 
                List<Point> maxPts = new List<Point>();
                if (!double.IsNaN(max) && !(max < Accuracy))
                    maxPts.Add(pointEnum.Current);
                ///enum another points
                ///
                while (pointEnum.MoveNext())
                {
                    if (pointEnum.Current == l || pointEnum.Current == r)
                        continue;
                    double temp = RoundTo(TriangSquareGeron(pointEnum.Current, l, r), 5);
                    if (!(temp < max) & RoundTo(System.Math.Abs(temp - max), 5) > Accuracy)
                    {
                        /// clear list and add new value
                        if (temp > max)
                            maxPts.Clear();
                        max = temp;
                        maxPts.Add(pointEnum.Current);
                    }
                }
                if(maxPts.Count == 0)
                    return false;
                
                Vector v1 = new Vector(0,1,0);
                Vector v2 =  maxPts[0] - l;
                int minI = 0;
                double min = v1.AngleTo(v2);
                for (int i = 1; i < maxPts.Count; i++)
                {
                    v2 =  maxPts[i] - l;
                    double temp;
                    if (min > (temp = v1.AngleTo(v2)))
                    {
                        minI = i;
                        min = temp;
                    }
                }
                tPoint = maxPts[minI];

                return true;
            } 

            public void Convex(ref Point[] points, out IList<Point> result)
            {               
                result = null;
                try
                {
                    int maxX, minX;
                    getExtremumIndexs(ref points, 1, out minX, out maxX);
                    if (maxX > -1 & minX > -1)
                    {
                        Point L0 = points[minX];
                        Point R0 = new Point(L0.x, L0.y - Accuracy, L0.z);
                        result = new List<Point>();
                        ((List<Point>)result).AddRange(qConvex(points, L0, R0));
                        result.Remove(R0);
                    }
                }
                catch (InvalidOperationException) { result = null; }
            }
            
            public bool DetectRationalBox(bool AllignByXAsix, ref Point[] convex) 
            {
                throw new NotFiniteNumberException();
            }
        }
       
    }
}