-added distancePointPlane, pointwiseMultiply and the sum of a Vector

bbiwarg/Utility/Vector.cs

@@ -120,6 +120,15 @@ namespace bbiwarg.Utility
         {
             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);
@@ -170,5 +179,41 @@ namespace bbiwarg.Utility
 
             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;
+
+        }
     }
 }