/fpTransform/fpTransform/Gm.cs
C# | 3637 lines | 2865 code | 431 blank | 341 comment | 347 complexity | c67a18e165c5b38af6f8921336fd8caa MD5 | raw file
Possible License(s): BSD-3-Clause
- #define L_ARM
- using System;
- using System.Data.SqlTypes;
- using System.Collections.Generic;
- using System.Collections;
- using Microsoft.SqlServer.Server;
- namespace geometry
- {
- public static class Gm
- {
- public enum DecartPosition
- {
- Origin = 0,
- At1 = 1,
- At2 = 2,
- At3 = 3,
- At4 = 4,
- AtX_PlusY = 5,
- AtX_MinusY = 6,
- AtY_PlusX = 7,
- AtY_MinusX = 8
- }
- #region Common
- public static double Accuracy { get { return 0.0000001; } }
- public static uint MaxDigits { get { return 7; } }
- public static bool ApproximatelyEqual(double d1, double d2, double tol)
- {
- return (Math.Abs(d1 - d2) < tol);
- }
- public static bool ApproximatelyEqual(double d1, double d2)
- {
- return (Math.Abs(d1 - d2) < Accuracy);
- }
- public static bool ApproximatelyEqual(float d1, float d2, float tol)
- {
- return (Math.Abs(d1 - d2) < tol);
- }
- public static bool ApproximatelyEqual(float d1, float d2)
- {
- return (Math.Abs(d1 - d2) < Accuracy);
- }
- public static bool Factor(uint digits, out double factor1, out double factor2)
- {
- factor1 = 1;
- factor2 = 1;
-
- if (digits > MaxDigits)
- throw new ArgumentException("Argument exception: digits");
- switch (digits)
- {
- case 0:
- factor1 = factor2 = 0;
- return true;
- case 1:
- factor1 = 0.1;
- factor2 = 10;
- break;
- case 2:
- factor1 = 0.01;
- factor2 = 100;
- break;
- case 3:
- factor1 = 0.001;
- factor2 = 1000;
- break;
- case 4:
- factor1 = 0.0001;
- factor2 = 10000;
- break;
- case 5:
- factor1 = 0.00001;
- factor2 = 100000;
- break;
- default:
- for (int i = 0; i < digits; i++)
- {
- factor1 *= 0.1;
- factor2 *= 10;
- }
- break;
- }
- return false;
- }
- public static bool Factor(uint digits, out float factor1, out float factor2)
- {
- factor1 = 1;
- factor2 = 1;
- if (digits > MaxDigits)
- throw new ArgumentException("Argument exception: digits");
- switch (digits)
- {
- case 0:
- factor1 = factor2 = 0f;
- return true;
- case 1:
- factor1 = 0.1f;
- factor2 = 10f;
- break;
- case 2:
- factor1 = 0.01f;
- factor2 = 100f;
- break;
- case 3:
- factor1 = 0.001f;
- factor2 = 1000f;
- break;
- case 4:
- factor1 = 0.0001f;
- factor2 = 10000f;
- break;
- case 5:
- factor1 = 0.00001f;
- factor2 = 100000f;
- break;
- default:
- for (int i = 0; i < digits; i++)
- {
- factor1 *= 0.1f;
- factor2 *= 10f;
- }
- break;
- }
- return false;
- }
- public static double DegToRad(float angle) { return angle * Math.PI / 180; }
- public static double DegToRad(double angle) { return angle * Math.PI / 180; }
- public static double RadToDeg(float angle) { return angle * 180 / Math.PI; }
- public static double RadToDeg(double angle) { return angle * 180/ Math.PI; }
- public static double RoundTo(float value, uint digits)
- {
- float factor1 = default(float);
- float factor2 = factor1;
- float ret;
- Factor(digits, out factor1, out factor2);
- if (Factor(digits, out factor1, out factor2))
- ret = (int)value;
- else
- ret = ((int)(value * factor2)) * factor1;
- return ret;
- }
- public static double RoundTo(double value, uint digits)
- {
- double factor1 = default(double);
- double factor2 = factor1;
- double ret;
- Factor(digits, out factor1, out factor2);
- if (Factor(digits, out factor1, out factor2))
- ret = (int)value;
- else
- ret = ((int)(value * factor2)) * factor1;
- return ret;
- }
- public static Vector RoundTo(Vector value, uint digits)
- {
- double factor1 = default(double);
- double factor2 = factor1;
- Vector ret = new Vector();
- if (Factor(digits, out factor1, out factor2))
- {
- ret.x = (int)value.x;
- ret.y = (int)value.y;
- ret.z = (int)value.z;
- }
- else
- {
- ret.x = ((int)(value.x * factor2)) * factor1;
- ret.y = ((int)(value.y * factor2)) * factor1;
- ret.z = ((int)(value.z * factor2)) * factor1;
- }
- return ret;
- }
- public static Point RoundTo(Point value, uint digits)
- {
- double factor1 = default(double);
- double factor2 = factor1;
- Point ret = new Point();
- Factor(digits, out factor1, out factor2);
- if (Factor(digits, out factor1, out factor2))
- {
- ret.x = (int)value.x;
- ret.y = (int)value.y;
- ret.z = (int)value.z;
- }
- else
- {
- ret.x = ((int)(value.x * factor2)) * factor1;
- ret.y = ((int)(value.y * factor2)) * factor1;
- ret.z = ((int)(value.z * factor2)) * factor1;
- }
- return ret;
- }
- public static Quaterion RoundTo(Quaterion value, uint digits)
- {
- double factor1 = default(double);
- double factor2 = factor1;
- Quaterion ret = new Quaterion();
- Factor(digits, out factor1, out factor2);
- if (Factor(digits, out factor1, out factor2))
- {
- ret.Vector = new Vector((int)value.Vector.x, (int)value.Vector.y, (int)value.Vector.z);
- ret.W = (int)value.W;
- }
- else
- {
- ret.Vector = new Vector(((int)(value.Vector.x * factor2)) * factor1, ((int)(value.Vector.y * factor2)) * factor1, ((int)(value.Vector.z * factor2)) * factor1);
- ret.W = ((int)(value.W * factor2)) * factor1;
- }
- return ret;
- }
- public static Matrix3 RoundTo(Matrix3 value, uint digits)
- {
- double factor1 = default(double);
- double factor2 = factor1;
- double[] data = value.Data;
- //Matrix3 ret = new Matrix3();
- Factor(digits, out factor1, out factor2);
- if (Factor(digits, out factor1, out factor2))
- {
- for (int i = 0; i < data.Length; i++)
- data[i] = (int)data[i];
- }
- else
- {
- for (int i = 0; i < data.Length; i++)
- data[i] = ((int)(data[i] * factor2)) * factor1;
- }
- return new Matrix3(data);
- }
- public static Transform RoundTo(Transform value, uint digits)
- {
- double factor1 = default(double);
- double factor2 = factor1;
- double[] data = value.Data;
- Transform ret = new Transform();
- Factor(digits, out factor1, out factor2);
- if (Factor(digits, out factor1, out factor2))
- {
- for (int i = 0; i < data.Length; i++)
- data[i] = (int)data[i];
- }
- else
- {
- for (int i = 0; i < data.Length; i++)
- data[i] = ((int)(data[i] * factor2)) * factor1;
- }
- return new Transform(data);
- }
- /// <summary>
- /// Split double array to vectors
- /// </summary>
- /// <param name="values"></param>
- /// <param name="vDim"></param>
- /// <param name="useRound"></param>
- /// <param name="digits"></param>
- /// <returns></returns>
- public static Vector[] SplitToVectors(double[] values, byte vDim, bool useRound, uint digits)
- {
- int vCCount = values.Length % vDim;
- if (vDim < 2 | vDim > 3)
- throw new ArgumentException("Gm.SplitToVectors: vDim must be 2 or 3");
- if (vCCount > 0 & vCCount < vDim - 1)
- throw new ArgumentException("Gm.SplitToVectors: values");
- Vector[] ret = new Vector[(int)(values.Length / vDim) + (vCCount > 0 ? 1 : 0)];
- for (int i = 0, counter = 0; i < values.Length & counter < ret.Length; i += vDim, counter++)
- {
- if (vDim > 1)
- {
- ret[counter].x = values[i];
- ret[counter].y = values[i + 1];
- }
- if (vDim > 2 && i + 2 < values.Length)
- ret[counter].z = values[i + 2];
- // round
- if (useRound)
- ret[counter].Round(digits);
- }
- return ret;
- }
- /// <summary>
- /// Split double array to points
- /// </summary>
- /// <param name="values"></param>
- /// <param name="vDim"></param>
- /// <param name="useRound"></param>
- /// <param name="digits"></param>
- /// <returns></returns>
- public static Point[] SplitToPoints(double[] values, byte vDim, bool useRound, uint digits)
- {
- int vCCount = values.Length % vDim;
- if (vDim < 2 | vDim > 4)
- throw new ArgumentException("Gm.SplitToPoints: vDim must be 2 or 3");
- if (vCCount > 0 & vCCount < vDim - 1)
- throw new ArgumentException("Gm.SplitToPoints: values");
- Point[] ret = new Point[(int)(values.Length / vDim) + (vCCount > 0 ? 1 : 0)];
- for (int i = 0, counter = 0; i < values.Length & counter < ret.Length; i += vDim, counter++)
- {
- if (vDim > 1)
- {
- ret[counter].x = values[i];
- ret[counter].y = values[i + 1];
- }
- if (vDim > 2 && i + 2 < values.Length)
- ret[counter].z = values[i + 2];
- // uses round
- if (useRound)
- ret[counter].Round(digits);
- }
- return ret;
- }
- /// <summary>
- /// Split double array to points
- /// </summary>
- /// <param name="values"></param>
- /// <param name="vDim"></param>
- /// <param name="useRound"></param>
- /// <param name="digits"></param>
- /// <returns></returns>
- public static Point[] SplitToPoints(float[] values, byte vDim, bool useRound, uint digits)
- {
- int vCCount = values.Length % vDim;
- if (vDim < 2 | vDim > 4)
- throw new ArgumentException("Gm.SplitToPoints: vDim must be 2 or 3");
- if (vCCount > 0 & vCCount < vDim - 1)
- throw new ArgumentException("Gm.SplitToPoints: values");
- Point[] ret = new Point[(int)(values.Length / vDim) + (vCCount > 0 ? 1 : 0)];
- for (int i = 0, counter = 0; i < values.Length & counter < ret.Length; i += vDim, counter++)
- {
- if (vDim > 1)
- {
- ret[counter].x = values[i];
- ret[counter].y = values[i + 1];
- }
- if (vDim > 2 && i + 2 < values.Length)
- ret[counter].z = values[i + 2];
- // uses round
- if (useRound)
- ret[counter].Round(digits);
- }
- return ret;
- }
- public static double GetPolarAngle02P(double x, double y)
- {
- return Math.Atan(y / (x * x + y * y));
- }
- public static double iLeftOrRight(Point p, Point p1, Point p2)
- {
- double result = (p1.x - p.x) * (p2.y - p.y) - (p2.x - p.x) * (p1.y - p.y);
- if (Math.Abs(result) < Accuracy * Accuracy)
- result = 0;
- return result;
- }
- public static double SignTriangSquare(Point p1, Point p2, Point p3, bool isClockWice)
- {
- /*
- // p1, p2, p3
- -------
- x1 y1 1
- x2 y2 1
- x3 y3 1
- -------
- return m_matrix[0, 0] * m_matrix[1, 1] * m_matrix[2, 2] + m_matrix[0, 1] * m_matrix[1, 2] * m_matrix[2, 0] + m_matrix[0, 2] * m_matrix[1, 0] * m_matrix[2, 1]
- - m_matrix[0, 2] * m_matrix[1, 1] * m_matrix[2, 0] - m_matrix[0, 1] * m_matrix[1, 0] * m_matrix[2, 2] - m_matrix[0, 0] * m_matrix[1, 2] * m_matrix[2, 1];
- */
- double result = p1.x * p2.y + p1.y * p3.x + p2.x * p3.y - p2.y * p3.x - p1.y * p2.x - p1.x * p3.y;
- return isClockWice ? -result : result;
- }
- public static Point Mid(Point p1, Point p2)
- {
- double
- x = (p1.x + p2.x) / 2,
- y = (p1.y + p2.y) / 2,
- z = (p1.z + p2.z) / 2;
- return new Point(x, y, z);
- }
- public static bool DetectTriang(IEnumerable<Point> points,
- out Point p1, out Point p2, out Point p3)
- {
- ///Set points to default
- ///
- p1 = default(Point);
- p2 = default(Point);
- p3 = default(Point);
- /// Preparation
- ///
- IEnumerator<Point> iEnum = null;
- if (points == null)
- return false;
- if ((iEnum = points.GetEnumerator()) == null)
- return false;
- iEnum.Reset();
- ///Read first two points
- ///
- if (iEnum.MoveNext())
- p1 = iEnum.Current;
- else
- return false;
- if (iEnum.MoveNext())
- p2 = iEnum.Current;
- else
- return false;
- /// Search point
- ///
- while (iEnum.MoveNext())
- {
- if ( Math.Abs( iLeftOrRight(iEnum.Current, p1, p2)) > Accuracy)
- {
- p3 = iEnum.Current;
- return true;
- }
- }
- return false;
- }
- public static Point Centroid(IEnumerable<Point> points, out bool isDetect)
- {
- Point ret = default(Point),
- p1 = default(Point), p2 = default(Point), p3 = default(Point);
- isDetect = false;
-
- if (DetectTriang(points, out p1, out p2, out p3))
- {
- Point m1 = Mid(p2, p3);
- Point m2 = Mid(p1, p3);
- Point m3 = Mid(p1, p2);
- if (Intersect(p1, m1, p2, m2, out ret) < 1)
- {
- ret = default(Point);
- isDetect = false;
- }
- isDetect = true;
- }
- return ret;
- }
- public static void Sort(ref Point[] array, int minIndex, int maxIndex)
- {
- Dictionary<bool, List<Point>> convSegment = new Dictionary<bool, List<Point>>();
- /// up convex
- ///
- convSegment.Add(false, new List<Point>());
- /// down convex
- ///
- convSegment.Add(true, new List<Point>());
-
- for (int i = 0; i < array.Length; i++)
- {
- double res = Gm.iLeftOrRight(array[i], array[minIndex], array[maxIndex]);
- if (Math.Abs(res) < Accuracy)
- continue;
- if (res > 0)
- convSegment[false].Add(array[i]);
- else
- convSegment[true].Add(array[i]);
- }
- }
- public static bool Convex(ref Point[] tPoints)
- {
- int minIndex = 0, maxIndex = 0, tIndex = 0;
- double temp = 0;
- //double max = 0;
- //double min = 0;
- //if (tPoints.Length > 0)
- //{
- // min = tPoints[0].x;
- // max = tPoints[0].x;
- //}
- /// loock for min index and max index by x-componenta
- ///
- for (int i = 0; i < tPoints.Length; i++)
- {
- //if (min > tPoints[i].x)
- //{
- // min = tPoints[i].x;
- // minIndex = i;
- //}
- //if (max < tPoints[i].x)
- //{
- // max = tPoints[i].x;
- // maxIndex = i;
- //}
- System.Diagnostics.Debug.Print("x{0:F4}; y{1:F4}; z{2:F4};", new object[] { tPoints[i].x, tPoints[i].y, tPoints[i].z });
- if (tPoints[minIndex].x > tPoints[i].x)
- minIndex = i;
- if (tPoints[maxIndex].x < tPoints[i].x)
- maxIndex = i;
- }
- double tSquare = 0;
- /// Look for point with max triange square
- ///
- for (int i = 0; i < tPoints.Length; i++)
- {
- temp = Gm.TriangSquareGeron(tPoints[minIndex], tPoints[maxIndex], tPoints[i]);
- if (Math.Abs(tSquare)< Math.Abs(temp))
- {
- tSquare = temp;
- tIndex = i;
- }
- }
- bool isDetect = false;
- Point cPoint = Centroid(new Point[] { tPoints[minIndex], tPoints[maxIndex], tPoints[tIndex] }, out isDetect);
- /// projected face is line
- ///
- if (!isDetect)
- return false;
- /// projected face is polygon
- /// Sort points as polar angles
- ///
- Sort(ref tPoints, minIndex, maxIndex);
- return true;
- }
- public static void JoinConvex(ref List<Point> resultConvex, ref List<Point> convex1, ref List<Point> convex2)
- {
- }
- public static double Length2(Point p1, Point p2)
- {
- return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
- }
- public static double TriangSquareGeron(Point p1, Point p2, Point p3)
- {
- double a = Math.Sqrt(Length2(p1, p2));
- double b = Math.Sqrt(Length2(p2, p3));
- double c = Math.Sqrt(Length2(p1, p3));
- double p = 0.5 * (a + b + c);
-
- return p * Math.Sqrt((p - a) * (p - b) * (p - c));
- }
- public static double TriangSquare(Point p1, Point p2, Point p3)
- {
- return 0.5 * Math.Abs(SignTriangSquare(p1, p2, p3, false));
- }
- public static int Intersect(Point a1, Point a2, Point b1, Point b2, out Point intersection)
- {
- intersection = default(Point);
- double
- d = (a1.x - a2.x) * (b2.y - b1.y) - (a1.y - a2.y) * (b2.x - b1.x),
- da = (a1.x - b1.x) * (b2.y - b1.y) - (a1.y - b1.y) * (b2.x - b1.x),
- db = (a1.x - a2.x) * (a1.y - b1.y) - (a1.y - a2.y) * (a1.x - b1.x);
- if (Math.Abs(d) < Accuracy)
- return 0;
- double
- ta = da / d,
- tb = db / d;
- if (0 <= ta & ta <= 1 & 0 <= tb & tb <= 1)
- {
- intersection = new Point(a1.x + ta * (a2.x - a1.x), a1.y + ta * (a2.y - a1.y), 0);
- return 1;
- }
- return -1;
- }
- #endregion
- #region Vector definition
- //[System.Diagnostics.DebuggerDisplay("{format(4)}", Name = "Vector")]
- [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
- public struct Vector : INullable, IEquatable<Vector>
- {
- public double x;
- public double y;
- public double z;
- private bool m_null;
- private string format(byte dig)
- {
- Vector v = RoundTo(this,dig);
- return "(" + v.x.ToString() + "; " + v.y.ToString() + "; " + v.z.ToString() + "), Length:" + v.Length().ToString();
- }
- public Vector(double x, double y, double z)
- : this()
- {
- this.x = x;
- this.y = y;
- this.z = z;
- }
- public Vector(double[] points)
- : this()
- {
- if (points != null && points.Length >= 2)
- {
- this.x = points[0];
- this.y = points[1];
- if (points.Length > 2)
- this.z = points[2];
- }
- else
- throw new ArgumentOutOfRangeException("Array size have to be equal or greater than 3 elements");
- }
- public Vector(Vector v)
- : this()
- {
- this.x = v.x;
- this.y = v.y;
- this.z = v.z;
- }
- public void SetCoords(double x, double y, double z)
- {
- this.x = x;
- this.y = y;
- this.z = z;
- }
- public void SetCoords(Vector v)
- {
- this.x = v.x;
- this.y = v.y;
- this.z = v.z;
- }
- public void SetCoords(double[] points)
- {
- if (points != null && points.Length >= 2)
- {
- this.x = points[0];
- this.y = points[1];
- if (points.Length >= 3)
- this.z = points[2];
- }
- else
- throw new ArgumentOutOfRangeException("Array size have to be equal or greater than 3 elements");
- }
-
- public void Normalize()
- {
- double dist = Length2();
- if (dist > 0)
- {
- x /= dist;
- y /= dist;
- z /= dist;
- }
- }
-
- public Vector GetNormalized()
- {
- Vector v = new Vector(x, y, z);
- double dist = v.Length();
- if (dist > 0)
- {
- dist = 1 / dist;
- v.x *= dist;
- v.y *= dist;
- v.z *= dist;
- }
- return v;
- }
- /// <summary>
- /// Creates parallel vector, at point
- /// </summary>
- /// <param name="point"></param>
- /// <returns></returns>
- public Vector GetParallel(Point point)
- {
- Vector v = this;
- v.x += point.x;
- v.y += point.y;
- v.z += point.z;
- return v;
- }
- /// <summary>
- /// Creates parallel vector, at point
- /// </summary>
- /// <param name="point"></param>
- /// <returns></returns>
- public Vector GetParallel(double distance)
- {
- Vector normal = GetPerpVector()*distance;
- Vector v = normal.GetPerpVector();
- if (this.GetNormalized() != v)
- v.Inverce();
- return v;
- }
- /// <summary>
- /// Returns perpendicular vector
- /// </summary>
- /// <returns></returns>
- public Vector GetPerpVector()
- {
- if ((AngleTo(new Vector(0, 1, 0)) < Accuracy) || (AngleTo(new Vector(0, -1, 0)) < Accuracy))
- {
- return new Vector(y, 0, 0).GetNormalized();
- }
- return new Vector(z, 0, -x).GetNormalized();
- }
- public void Inverce() { x = -x; y = -y; z = -z; }
- /// <summary>
- /// Returns opposite vector
- /// </summary>
- /// <returns></returns>
- public Vector GetInverse()
- {
- return new Vector(-x, -y, -z);
- }
- public double Length()
- {
- return Math.Sqrt(x * x + y * y + z * z);
- }
- public double Length2()
- {
- return x * x + y * y + z * z;
- }
- public Vector Rotate(Vector dir, double ang)
- {
- if (ang == 0)
- return this;
- double c, s;
- double a;
- a = ang * Math.PI / 180;
- c = Math.Cos(a);
- s = Math.Sin(a);
- if (Double.IsNaN(c))
- c = 0;
- if (Double.IsNaN(s))
- s = 0;
- double nx, ny, nz;
- nx = x * (c + (1 - c) * (dir.x * dir.x)) + y * ((1 - c) * dir.y * dir.x + s * dir.z) + z * ((1 - c) * dir.z * dir.x - s * dir.y);
- ny = x * ((1 - c) * dir.y * dir.x - s * dir.z) + y * (c + (1 - c) * (dir.y * dir.y)) + z * ((1 - c) * dir.z * dir.y + s * dir.x);
- nz = x * ((1 - c) * dir.z * dir.x + s * dir.y) + y * ((1 - c) * dir.z * dir.y - s * dir.x) + z * (c + (1 - c) * (dir.z * dir.z));
- x = nx;
- y = ny;
- z = nz;
- return this;
- }
- public void Round(uint digits)
- {
- double factor1 = default(double);
- double factor2 = factor1;
- Factor(digits, out factor1, out factor2);
- if (Factor(digits, out factor1, out factor2))
- {
- x = (int)x;
- y = (int)y;
- z = (int)z;
- }
- else
- {
- x = ((int)(x * factor2)) * factor1;
- y = ((int)(y * factor2)) * factor1;
- z = ((int)(z * factor2)) * factor1;
- }
- }
- public double AngleTo(Vector vector)
- {
- if (vector.Length() == 0)
- return 0;
- double d = (this * vector) / (Length() * vector.Length());
- if (ApproximatelyEqual(d, 1, Accuracy))
- {
- d = 1;
- }
- else if (ApproximatelyEqual(d, -1, Accuracy))
- {
- d = -1;
- }
- return Math.Acos(d);
- }
- public double AngleToNonACos(Vector vector)
- {
- if (vector.Length() == 0)
- return 0;
- double d = (this * vector) / (Length() * vector.Length());
- if (ApproximatelyEqual(d, 1, Accuracy))
- {
- d = 1;
- }
- else if (ApproximatelyEqual(d, -1, Accuracy))
- {
- d = -1;
- }
- return d;
- }
- public bool IsParallelTo(Vector vector)
- {
- double angle = AngleTo(vector);
- return (ApproximatelyEqual(angle, 0, Accuracy) || ApproximatelyEqual(angle, Math.PI, Accuracy));
- }
- public bool IsPerpendicularTo(Vector vector)
- {
- return (ApproximatelyEqual(AngleTo(vector), Math.PI / 2));
- }
-
- /// <summary>
- /// Return rotated vector at transform data
- /// </summary>
- /// <param name="transform">transform matrix</param>
- /// <returns>rotated vector</returns>
- public Vector this[Transform transform]
- {
- get { return transform.TransformVector(this); }
- }
- #region INullable Members
- public bool IsNull { get { return m_null; } }//ApproximatelyEqual(x, 0) && ApproximatelyEqual(y, 0) && ApproximatelyEqual(z, 0);
- #endregion
- #region IEquatable<Vector> Members
- public override int GetHashCode()
- {
- return (int)Math.Sqrt(x * x * y * y * z * z);
- }
- public override bool Equals(object o)
- {
- if (o is Vector)
- {
- Vector v = (Vector)o;
- return !v.IsNull && Equals(v);
- }
- return false;
- }
- public bool Equals(Vector v)
- {
- return this == v;
- }
- public bool Equals(Vector vec, int tol)
- {
- return
- ApproximatelyEqual(vec.x, x, tol)
- &&
- ApproximatelyEqual(vec.y, y, tol)
- &&
- ApproximatelyEqual(vec.z, z, tol);
- }
- #endregion
- #region operators
- public static Vector operator +(Vector v1, Vector v2)
- {
- return new Vector(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
- }
- public static Vector operator -(Vector v1, Vector v2)
- {
- return new Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
- }
- public static Vector operator *(Vector v1, double val)
- {
- return new Vector(v1.x * val, v1.y * val, v1.z * val);
- }
- public static Vector operator /(Vector v1, double val)
- {
- return new Vector(v1.x / val, v1.y / val, v1.z / val);
- }
- public static Vector operator ^(Vector v1, Vector v2)
- {
- #if L_ARM
- return new Vector(v2.y * v1.z - v2.z * v1.y, v2.z * v1.x - v2.x * v1.z, v2.x * v1.y - v2.y * v1.x);
- #else
- return new Vector(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
- #endif
- }
- public static double operator *(Vector v1, Vector v2)
- {
- return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
- }
- public static bool operator ==(Vector v1, Vector v2)
- {
- return
- ApproximatelyEqual(v1.x, v2.x)
- &&
- ApproximatelyEqual(v1.y, v2.y)
- &&
- ApproximatelyEqual(v1.z, v2.z);
- }
- public static bool operator !=(Vector v1, Vector v2)
- {
- return !(v1 == v2);
- }
- public static implicit operator double[](Vector v1)
- {
- double[] arr = new double[3];
- arr[0] = v1.x;
- arr[1] = v1.y;
- arr[2] = v1.z;
- return arr;
- }
- public static implicit operator Vector(double[] points)
- {
- Vector v = new Vector();
- if (points != null && points.Length >= 2)
- {
- v.x = points[0];
- v.y = points[1];
- v.z = points[2];
- }
- else
- throw new ArgumentOutOfRangeException("Array size have to be equal or greater than 3 elements");
- return v;
- }
- #endregion
- #region static members
- public static Vector Null
- {
- get
- {
- Vector v = new Vector();
- v.m_null = true;
- return v;
- }
- }
- /// <summary>
- /// Calculate angle between vectors v1 and v2. All vectors mut be vectors with unit length
- /// </summary>
- /// <param name="axis">required to define sign of the angle</param>
- /// <param name="v1">first vector</param>
- /// <param name="v2">second vector</param>
- /// <returns>return signed angle value in radians</returns>
- public static double GetAngleZBetweenVectors(Vector axis, Vector v1, Vector v2)
- {
- Vector tmp = (v1 ^ v2);
- double cosAngle = v2 * v1;
- double angle = Math.Acos(cosAngle);
- if (Double.IsNaN(angle))
- angle = 0;
- if (tmp * axis < 0) //cos 90+ is negative
- angle = -angle;
- return angle;
- }
- #endregion
- }
- #endregion
- #region Point definition
- //[System.Diagnostics.DebuggerDisplay("{format(4)}", Name = "Point")]
- [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
- public struct Point : IEquatable<Point>, INullable, IBinarySerialize
- {
- public double x, y, z;
- private bool m_null;
- private string format(byte dig)
- {
- Point p = RoundTo(this, dig);
- return "(" + p.x.ToString() + "; " + p.y.ToString() + "; " + p.z.ToString() + ")";
- }
- public Point(double x, double y, double z)
- {
- m_null = false;
- this.x = x;
- this.y = y;
- this.z = z;
- }
- public Point(double x, double y) : this(x, y, 0) { }
- public void Translate(Vector vector)
- {
- x += vector.x;
- y += vector.y;
- z += vector.z;
- }
- public Point GetTranslated(Vector vector)
- {
- return this[vector];
- }
- public Point GetTransformed(Matrix3 matrix)
- {
- throw new NotImplementedException();
- }
- public void Transform(Matrix3 matrix)
- {
- throw new NotImplementedException();
- }
- public override int GetHashCode()
- {
- return base.GetHashCode();
- }
- public void Round(uint digits)
- {
- double factor1 = default(double);
- double factor2 = factor1;
- if (Factor(digits, out factor1, out factor2))
- {
- x = (int)x;
- y = (int)y;
- z = (int)z;
- }
- else
- {
- x = ((int)(x * factor2)) * factor1;
- y = ((int)(y * factor2)) * factor1;
- z = ((int)(z * factor2)) * factor1;
- }
- }
- public void MirrorAtOrigin()
- {
- x = -x;
- y = -y;
- z = -z;
- }
- public double DistanceTo(Point point)
- {
- return this[point].Length();
- }
- #region IEquatable<Point> Members
-
- public override bool Equals(object obj)
- {
- if (obj is Point)
- {
- return Equals((Point)obj);
- }
- return false;
- }
- public bool Equals(Point point)
- {
- return this == point;
- }
- #endregion
- public bool IsOrigin { get { return ApproximatelyEqual(x, 0) && ApproximatelyEqual(y, 0) && ApproximatelyEqual(z, 0); } }
-
- #region INullable Members
- public bool IsNull { get { return m_null; } }
- #endregion
-
- /// <summary>
- /// Returns non normailzed vector from this point and another
- /// </summary>
- /// <param name="end">Another point</param>
- /// <returns></returns>
- public Vector this[Point end]
- {
- get { return new Vector(end.x - x, end.y - y, end.z - z); }
- }
- /// <summary>
- /// Returns point into new Transformation
- /// </summary>
- /// <param name="matrix">Transform matrix</param>
- /// <returns></returns>
- public Point this[Transform transform]
- {
- get
- {
- return transform.TransformPoint(this);
- }
- }
- /// <summary>
- /// Returns translated point at direction <paramref name="vactor"/>
- /// </summary>
- /// <param name="vector">translate direction</param>
- /// <returns></returns>
- public Point this[Vector vector] { get { return this + vector; } }
- #region operators
- public static bool operator ==(Point p1, object p2)
- {
- if (p2 == default(object))
- return false;
- return p1 == (Point)p2;
- }
- public static bool operator !=(Point p1, object p2)
- {
- if (p2 == default(object))
- return true;
- return p1 != (Point)p2;
- }
- public static bool operator ==(Point p1, Point p2)
- {
- return
- Math.Abs(p1.x - p2.x) < Accuracy
- &&
- Math.Abs(p1.y - p2.y) < Accuracy
- &&
- Math.Abs(p1.z - p2.z) < Accuracy;
- }
- public static bool operator !=(Point p1, Point p2)
- {
- return !(p1 == p2);
- }
- public static Point operator +(Point point, Vector vector)
- {
- return new Point(point.x + vector.x, point.y + vector.y, point.z + vector.z);
- }
- public static Point operator -(Point point, Vector vector)
- {
- return new Point(point.x - vector.x, point.y - vector.y, point.z - vector.z);
- }
- public static Vector operator -(Point point1, Point point2)
- {
- return new Vector(point1.x - point2.x, point1.y - point2.y, point1.z - point2.z);
- }
- public static Vector operator +(Point point1, Point point2)
- {
- return new Vector(point1.x + point2.x, point1.y + point2.y, point1.z + point2.z);
- }
- #endregion
- #region static members
- public static Point Null
- {
- get
- {
- Point p = new Point();
- p.m_null = true;
- return p;
- }
- }
- #endregion
- #region IBinarySerialize Members
- public void Read(System.IO.BinaryReader r)
- {
- m_null = r.ReadBoolean();
- x = r.ReadDouble();
- y = r.ReadDouble();
- z = r.ReadDouble();
- }
- public void Write(System.IO.BinaryWriter w)
- {
- w.Write(m_null);
- w.Write(x);
- w.Write(y);
- w.Write(z);
- }
- #endregion
- }
- #endregion
- #region Plane definition
- //[System.Diagnostics.DebuggerDisplay("{format(4)}", Name = "Plane")]
- [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
- public struct Plane :INullable
- {
- private double[,] m_matr;
- private bool m_null;
- private string format(byte dig)
- {
- //Plane p = RoundTo(this, dig);
- //"(" + p.x.ToString() + "; " + p.y.ToString() + "; " + p.z.ToString() + ")";
- return string.Empty;
- }
- public Plane(Vector normal, Point point)
- : this()
- {
- m_matr = new double[3, 3];
- Vector perp1 = normal.GetPerpVector();
- Vector perp2 = perp1 ^ normal;
- Point p1 = point, p2 = point + perp1, p3 = point + perp2;
- m_matr[0, 0] = p1.x;
- m_matr[0, 1] = p1.y;
- m_matr[0, 2] = p1.z;
- m_matr[1, 0] = p2.x - p1.x;
- m_matr[1, 1] = p2.y - p1.y;
- m_matr[1, 2] = p2.z - p1.z;
- m_matr[2, 0] = p3.x - p1.x;
- m_matr[2, 1] = p3.y - p1.y;
- m_matr[2, 2] = p3.z - p1.z;
- }
- public Plane(Point p1, Point p2, Point p3)
- : this()
- {
- m_matr = new double[3, 3];
- m_matr[0, 0] = p1.x;
- m_matr[0, 1] = p1.y;
- m_matr[0, 2] = p1.z;
- m_matr[1, 0] = p2.x - p1.x;
- m_matr[1, 1] = p2.y - p1.y;
- m_matr[1, 2] = p2.z - p1.z;
- m_matr[2, 0] = p3.x - p1.x;
- m_matr[2, 1] = p3.y - p1.y;
- m_matr[2, 2] = p3.z - p1.z;
- }
- public Plane(Vector v1, Vector v2, Point p): this()
- {
- m_matr = new double[3, 3];
- Point p1 = p, p2 = p + v1, p3 = p + v2;
- m_matr[0, 0] = p1.x;
- m_matr[0, 1] = p1.y;
- m_matr[0, 2] = p1.z;
- m_matr[1, 0] = p2.x - p1.x;
- m_matr[1, 1] = p2.y - p1.y;
- m_matr[1, 2] = p2.z - p1.z;
- m_matr[2, 0] = p3.x - p1.x;
- m_matr[2, 1] = p3.y - p1.y;
- m_matr[2, 2] = p3.z - p1.z;
- }
- public bool Contains(Vector vector)
- {
- return Contains(new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2])[vector]);
- }
- public bool Contains(Point point)
- {
- double a = point.x - m_matr[0, 0];
- double b = point.y - m_matr[0, 1];
- double c = point.z - m_matr[0, 2];
- double det = a * m_matr[1, 1] * m_matr[2, 2];
- det += b * m_matr[1, 2] * m_matr[2, 0];
- det += c * m_matr[1, 0] * m_matr[2, 1];
- det -= c * m_matr[1, 1] * m_matr[2, 0];
- det -= b * m_matr[1, 0] * m_matr[2, 2];
- det -= a * m_matr[1, 2] * m_matr[2, 1];
- return ApproximatelyEqual(det, 0.0);
- }
- public double DistanceTo(Point point)
- {
- double[] x = new double[3];
- double[] y = new double[3];
- double[] z = new double[3];
- for (int i = 0; i < 3; ++i)
- {
- x[i] = m_matr[i, 0];
- y[i] = m_matr[i, 1];
- z[i] = m_matr[i, 2];
- }
- double A = y[1] * z[2] - z[1] * y[2];
- double B = x[2] * z[1] - x[1] * z[2];
- double C = x[1] * y[2] - x[2] * y[1];
- double D = x[0] * (z[1] * y[2] - y[1] * z[2]);
- D += x[1] * (z[2] * y[0] - y[2] * z[0]);
- D += x[2] * (y[1] * z[0] - z[1] * y[0]);
- double retval = (A * point.x) + (B * point.y) + (C * point.z) + D;
- retval /= Math.Sqrt((A * A) + (B * B) + (C * C));
- return Math.Abs(retval);
- }
- public double DistanceTo(Plane plane)
- {
- return DistanceTo(new Point(plane.m_matr[0, 0], plane.m_matr[0, 1], m_matr[0, 2]));
- }
- public Point[] points
- {
- get
- {
- Point[] retval = new Point[3];
- retval[0] = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]);
- Vector vec = new Vector(m_matr[1, 0], m_matr[1, 1], m_matr[1, 2]);
- retval[1] = retval[0] + vec;
- vec = new Vector(m_matr[2, 0], m_matr[2, 1], m_matr[2, 2]);
- retval[2] = retval[0] + vec;
- return retval;
- }
- }
- private Point intersect(Point a, Point b)
- {
- //sometimes (like in th bug #2307) we have very large inaccuracy
- //so we need to square our angle to prevent the error
- double angle = this.AngleTo(a[b]);
- if (ApproximatelyEqual(Math.Pow(angle, 2), 0.0))
- return Point.Null;
- Point p = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]);
- Vector l = new Vector(m_matr[1, 0], m_matr[1, 1], m_matr[1, 2]);
- Vector m = new Vector(m_matr[2, 0], m_matr[2, 1], m_matr[2, 2]);
- Vector val = l ^ m;
- Point q = a + ((b - a) * (((p - a) * val) / ((b - a) * val)));
- return q;
- }
- public Point[] Intersect(Plane plane)
- {
- Point[] ret = new Point[2];
- Point[] pts = points;
- List<Point[]> lines1 = new List<Point[]>();
- lines1.Add(new Point[] { pts[0], pts[1]});
- lines1.Add(new Point[] { pts[1], pts[2] });
- lines1.Add(new Point[] { pts[2], pts[0] });
- int k = 0;
- for (int i = 0; i < lines1.Count; ++i)
- if (!plane.IsParallel(lines1[i][1][lines1[i][0]]) && k != 2)
- {
- ret[k] = plane.intersect(lines1[i][0], lines1[i][1]);
- k++;
- }
- if (k != 2)
- return new Point[0];
- return ret;
- }
- public Vector ParallelVector() { return (Normal ^ new Vector(0, 0, 1)).GetNormalized(); }
- public Vector Normal
- {
- get
- {
- double[] x = new double[3];
- double[] y = new double[3];
- double[] z = new double[3];
- for (int i = 0; i < 3; ++i)
- {
- x[i] = m_matr[i, 0];
- y[i] = m_matr[i, 1];
- z[i] = m_matr[i, 2];
- }
- double A = y[1] * z[2] - z[1] * y[2];
- double B = x[2] * z[1] - x[1] * z[2];
- double C = x[1] * y[2] - x[2] * y[1];
- return (new Vector(A, B, C).GetNormalized());
- }
- }
-
- public bool IsParallel(Plane p)
- {
- return Normal.IsParallelTo(p.Normal);
- }
- public bool IsParallel(Vector vector)
- {
- double angle = 0.0;
- angle = Normal.AngleTo(vector);
- return ApproximatelyEqual(angle, Math.PI) || ApproximatelyEqual(angle, 0) ;
- }
- public double AngleTo(Vector vector)
- {
- double angle = Normal.AngleTo(vector);
- angle = (angle < (Math.PI / 2)) ? (Math.PI / 2 - angle) : angle - (Math.PI / 2);
- return angle;
- }
-
- #region INullable Members
- public bool IsNull { get { return m_null; } }
- #endregion
-
- #region Projection
- public Point Projection(Point point)
- {
- double dist = DistanceTo(point);
- Vector norm = Normal.GetNormalized();
- Point pr = point - (norm * dist);
- if (!ApproximatelyEqual(DistanceTo(pr), 0))
- pr = point + (norm * dist);
- return pr;
- }
- public Vector Projection(Vector vector)
- {
- Point p1 = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]);
- Point p2 = Projection(p1[vector]);
- return p1[p2];
- }
- #endregion
- #region Shift
- public void Shift(Vector direction)
- {
- Point
- p1 = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]),
- p2 = new Point(m_matr[1, 0] + p1.x, m_matr[1, 1] + p1.y, m_matr[1, 2] + p1.z),
- p3 = new Point(m_matr[2, 0] + p1.x, m_matr[2, 1] + p1.y, m_matr[2, 2] + p1.z);
- p1 = p1 + direction;
- p2 = p2 + direction;
- p3 = p3 + direction;
- m_matr[0, 0] = p1.x;
- m_matr[0, 1] = p1.y;
- m_matr[0, 2] = p1.z;
- m_matr[1, 0] = p2.x - p1.x;
- m_matr[1, 1] = p2.y - p1.y;
- m_matr[1, 2] = p2.z - p1.z;
- 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();
- }
- }
-
- }
- }