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