bbiwarg-original/bbiwarg/Utility/Vector.cs

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);
        }
    }
}