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(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 T norm()
        {
            return subNorm(elements.Length);
        }
        public T subNorm(int subLength)
        {
            T result = (dynamic)0;
            for (int i = 0; i < subLength; i++)
            {
                result += (dynamic)elements[i] * elements[i];
            }
            return (T)Math.Sqrt((dynamic)result);
        }
        public T distance(Vector<T> vector)
        {
            return (vector - this).norm();
        }
        public T sum()
        {
            T result = (dynamic)0;
            for (int i = 0; i < this.length(); i++)
            {
                result += (dynamic)elements[0];
            }
            return result;
        }

        public T subDistance(Vector<T> vector, int subLength) {
            return (vector - this).subNorm(subLength);
        }


        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> 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)
        {

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

            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;

            return distance;

        }
    }
}