123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279 |
- using System;
- using System.Collections.Generic;
- using System.Linq;
- using System.Text;
- using System.Threading.Tasks;
- namespace bbiwarg.Utility
- {
- class Vector<T>
- {
- private T[] elements;
- public T x { get { return elements[0]; } set { elements[0] = value; } }
- public T y { get { return elements[1]; } set { elements[1] = value; } }
- public T z { get { return elements[2]; } set { elements[2] = value; } }
- public Vector(T x, T y)
- {
- elements = new T[2];
- elements[0] = x;
- elements[1] = y;
- }
- public Vector(T x, T y, T z)
- {
- elements = new T[3];
- elements[0] = x;
- elements[1] = y;
- elements[2] = z;
- }
- public Vector(int numberOfElements)
- {
- this.elements = new T[numberOfElements];
- }
- public Vector(T[] elements)
- {
- this.elements = elements;
- }
- public Vector(Vector<T> vector)
- {
- this.elements = new T[vector.length()];
- for (int i = 0; i < elements.Length; i++)
- {
- elements[i] = vector[i];
- }
- }
- public Vector<T> copy()
- {
- Vector<T> newVector = new Vector<T>(this);
- return newVector;
- }
- public T this[int index]
- {
- get
- {
- return elements[index];
- }
- set
- {
- elements[index] = value;
- }
- }
- public int length()
- {
- return elements.Length;
- }
- public void add(Vector<T> summand)
- {
- for (int i = 0; i < elements.Length; i++)
- {
- elements[i] = (dynamic)elements[i] + summand[i];
- }
- }
- public void subtract(Vector<T> subtrahend)
- {
- for (int i = 0; i < elements.Length; i++)
- {
- elements[i] = (dynamic)elements[i] - subtrahend[i];
- }
- }
- public void multiply(T scalar)
- {
- for (int i = 0; i < elements.Length; i++)
- {
- elements[i] = (dynamic)scalar * elements[i];
- }
- }
- public float norm()
- {
- return subNorm(elements.Length);
- }
- public float subNorm(int subLength)
- {
- T result = (dynamic)0;
- for (int i = 0; i < subLength; i++)
- {
- result += (dynamic)elements[i] * elements[i];
- }
- return (float)Math.Sqrt((dynamic)result);
- }
- public float distance(Vector<T> vector)
- {
- return (vector - this).norm();
- }
- public float subDistance(Vector<T> vector, int subLength)
- {
- return (vector - this).subNorm(subLength);
- }
- public T sum()
- {
- T result = (dynamic)0;
- for (int i = 0; i < this.length(); i++)
- {
- result += (dynamic)elements[i];
- }
- return result;
- }
- public Vector<T> normalize()
- {
- float norm = this.norm();
- Vector<T> result = new Vector<T>(this);
- for (int i = 0; i < result.length(); i++)
- {
- result[i] = (result[i] / (dynamic)norm);
- }
- return result;
- }
- private static void checkLength(Vector<T> vector1, Vector<T> vector2)
- {
- if (vector1.length() != vector2.length())
- throw new ArgumentException("The vectors must have the same length");
- }
- public static Vector<T> operator +(Vector<T> summand1, Vector<T> summand2)
- {
- checkLength(summand1, summand2);
- Vector<T> result = new Vector<T>(summand1);
- result.add(summand2);
- return result;
- }
- public static Vector<T> operator -(Vector<T> minuend, Vector<T> subtrahend)
- {
- checkLength(minuend, subtrahend);
- Vector<T> result = new Vector<T>(minuend);
- result.subtract(subtrahend);
- return result;
- }
- public static T operator *(Vector<T> vector1, Vector<T> vector2)
- {
- checkLength(vector1, vector2);
- T result = (dynamic)0;
- for (int i = 0; i < vector1.length(); i++)
- {
- result += (dynamic)vector1[i] * vector2[i];
- }
- return result;
- }
- public static Vector<T> operator *(T factor, Vector<T> vector1)
- {
- Vector<T> result = new Vector<T>(vector1);
- result.multiply(factor);
- return result;
- }
- public static bool operator ==(Vector<T> vector1, Vector<T> vector2)
- {
- checkLength(vector1, vector2);
- bool result = true;
- for (int i = 0; i < vector1.length(); i++)
- {
- if (vector1[i] != (dynamic)vector2[i])
- {
- result = false;
- break;
- }
- }
- return result;
- }
- public static bool operator !=(Vector<T> vector1, Vector<T> vector2)
- {
- return !(vector1 == vector2);
- }
- public static Vector<T> crossProduct(Vector<T> vector1, Vector<T> vector2)
- {
- if (vector1.length() != 3 || vector2.length() != 3)
- throw new ArgumentException("The vectors' length should be 3");
- Vector<T> result = new Vector<T>(vector1.length());
- result[0] = (dynamic)vector1.y * vector2.z - (dynamic)vector1.z * vector2.y;
- result[1] = (dynamic)vector1.z * vector2.x - (dynamic)vector1.x * vector2.z;
- result[2] = (dynamic)vector1.x * vector2.y - (dynamic)vector1.y * vector2.x;
- return result;
- }
- public static Vector<T> pointwiseMultiply(Vector<T> vector1, Vector<T> vector2)
- {
- checkLength(vector1, vector2);
- Vector<T> result = new Vector<T>(vector1);
- for (int i = 0; i < vector1.length(); i++)
- {
- result[i] = (dynamic)result[i] * vector2[i];
- }
- return result;
- }
- public static float distancePointPlane(Vector<T> point, Vector<T> planeA, Vector<T> planeB, Vector<T> planeC)
- {
- //TODO - Diese funktion funktioniert nur mit T = float,
- //die normalisierte Normale einer Ebene macht nur mit floats sinn (werte zwischen 0 und 1)
- if (point.length() != 3 || planeA.length() != 3 || planeB.length() != 3 || planeC.length() != 3)
- throw new ArgumentException("The vectors' length should be 3");
- Vector<T> ab = planeB - planeA;
- Vector<T> ac = planeC - planeA;
- Vector<T> normal = crossProduct(ab, ac).normalize();
-
- Vector<T> temp = point - planeA;
- temp = pointwiseMultiply(temp, normal);
- temp = pointwiseMultiply(temp, normal);
- T sum = temp.sum();
- temp = pointwiseMultiply(normal, normal);
- T sumR = temp.sum();
- if (sumR == (dynamic)0)
- throw new ArgumentException("the points do not clamp an Plane");
- float distance = (sum * (dynamic)(-1)) / sumR;
- if (distance < 0)
- distance *= (float)(-1);
- return distance;
- }
- public static Vector<float> projectToLine(Vector<float> p, Vector<float> direction, Vector<float> pointOnLine)
- {
- float px = p.x, py = p.y, dx = direction.x, dy = direction.y, ox = pointOnLine.x, oy = pointOnLine.y;
- float diffx = px - ox;
- float diffy = py - oy;
- float diff_d = (diffx * dx + diffy * dy);
- float d_d = (dx * dx + dy * dy);
- float q = diff_d / d_d;
- float newX = ox + q * dx;
- float newY = oy + q * dy;
- return new Vector<float>(newX, newY);
- }
- }
- }
|