Free cookie consent management tool by TermsFeed Policy Generator

source: branches/HeuristicLab.Analysis.AlgorithmBehavior/MIConvexHull/StarMath/StarMathLibrary.cs @ 9730

Last change on this file since 9730 was 9730, checked in by ascheibe, 11 years ago

#1886 added a library that calculates convex hulls

File size: 28.4 KB
Line 
1/*************************************************************************
2 *     This file & class is part of the StarMath Project
3 *     Copyright 2010, 2011 Matthew Ira Campbell, PhD.
4 *
5 *     StarMath is free software: you can redistribute it and/or modify
6 *     it under the terms of the GNU General Public License as published by
7 *     the Free Software Foundation, either version 3 of the License, or
8 *     (at your option) any later version.
9 * 
10 *     StarMath is distributed in the hope that it will be useful,
11 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
12 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 *     GNU General Public License for more details.
14 * 
15 *     You should have received a copy of the GNU General Public License
16 *     along with StarMath.  If not, see <http://www.gnu.org/licenses/>.
17 *     
18 *     Please find further details and contact information on StarMath
19 *     at http://starmath.codeplex.com/.
20 *************************************************************************/
21using System;
22using System.Collections.Generic;
23using System.Linq;
24
25
26namespace MIConvexHull
27{
28    public static class StarMath
29    {
30        #region Printing
31        private const int cellWidth = 10;
32        private const int numDecimals = 3;
33
34        /// <summary>
35        /// Makes the print string.
36        /// </summary>
37        /// <param name = "A">The A.</param>
38        public static string MakePrintString(double[,] A)
39        {
40            if (A == null) return "<null>";
41            var format = "{0:F" + numDecimals + "}";
42            var p = "";
43            for (var i = 0; i < A.GetLength(0); i++)
44            {
45                p += "| ";
46                for (var j = 0; j < A.GetLength(1); j++)
47                    p += formatCell(format, A[i, j]) + ",";
48                p = p.Remove(p.Length - 1);
49                p += " |\n";
50            }
51            p = p.Remove(p.Length - 1);
52            return p;
53        }
54
55        private static object formatCell(string format, double p)
56        {
57            var numStr = string.Format(format, p);
58            numStr = numStr.TrimEnd('0');
59            numStr = numStr.TrimEnd('.');
60            var padAmt = ((double)(cellWidth - numStr.Length)) / 2;
61            numStr = numStr.PadLeft((int)Math.Floor(padAmt + numStr.Length));
62            numStr = numStr.PadRight(cellWidth);
63            return numStr;
64        }
65        #endregion
66
67        #region Solve
68        /// <summary>
69        /// Solves the specified A matrix.
70        /// </summary>
71        /// <param name="A">The A.</param>
72        /// <param name="b">The b.</param>
73        /// <returns></returns>
74        public static double[] solve(double[,] A, IList<double> b)
75        {
76            var aLength = A.GetLength(0);
77            if (aLength != A.GetLength(1))
78                throw new Exception("Matrix, A, must be square.");
79            if (aLength != b.Count)
80                throw new Exception("Matrix, A, must be have the same number of rows as the vector, b.");
81
82            /****** need code to determine when to switch between *****
83             ****** this analytical approach and the SOR approach *****/
84            return multiply(inverse(A, aLength), b, aLength, aLength);
85        }
86
87        public static void solveDestructiveInto(double[,] A, double[] b, double[] target)
88        {
89            var aLength = A.GetLength(0);
90            if (aLength != A.GetLength(1))
91                throw new Exception("Matrix, A, must be square.");
92            if (aLength != b.Length)
93                throw new Exception("Matrix, A, must be have the same number of rows as the vector, b.");
94
95            /****** need code to determine when to switch between *****
96             ****** this analytical approach and the SOR approach *****/
97            inverseInPlace(A, aLength);
98            multiplyInto(A, b, aLength, aLength, target);
99        }
100
101        /// <summary>
102        /// does gaussian elimination.
103        /// </summary>
104        /// <param name="nDim"></param>
105        /// <param name="pfMatr"></param>
106        /// <param name="pfVect"></param>
107        /// <param name="pfSolution"></param>
108        public static void gaussElimination(int nDim, double[][] pfMatr, double[] pfVect, double[] pfSolution)
109        {
110            double fMaxElem;
111            double fAcc;
112
113            int i, j, k, m;
114           
115            for (k = 0; k < (nDim - 1); k++) // base row of matrix
116            {
117                var rowK = pfMatr[k];
118
119                // search of line with max element
120                fMaxElem = Math.Abs(rowK[k]);
121                m = k;
122                for (i = k + 1; i < nDim; i++)
123                {
124                    if (fMaxElem < Math.Abs(pfMatr[i][k]))
125                    {
126                        fMaxElem = pfMatr[i][k];
127                        m = i;
128                    }
129                }
130
131                // permutation of base line (index k) and max element line(index m)               
132                if (m != k)
133                {
134                    var rowM = pfMatr[m];
135                    for (i = k; i < nDim; i++)
136                    {                       
137                        fAcc = rowK[i];
138                        rowK[i] = rowM[i];
139                        rowM[i] = fAcc;
140                    }
141                    fAcc = pfVect[k];
142                    pfVect[k] = pfVect[m];
143                    pfVect[m] = fAcc;
144                }
145
146                //if( pfMatr[k*nDim + k] == 0.0) return 1; // needs improvement !!!
147
148                // triangulation of matrix with coefficients
149                for (j = (k + 1); j < nDim; j++) // current row of matrix
150                {
151                    var rowJ = pfMatr[j];
152                    fAcc = -rowJ[k] / rowK[k];
153                    for (i = k; i < nDim; i++)
154                    {
155                        rowJ[i] = rowJ[i] + fAcc * rowK[i];
156                    }
157                    pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation
158                }
159            }
160
161            for (k = (nDim - 1); k >= 0; k--)
162            {
163                var rowK = pfMatr[k];
164                pfSolution[k] = pfVect[k];
165                for (i = (k + 1); i < nDim; i++)
166                {
167                    pfSolution[k] -= (rowK[i] * pfSolution[i]);
168                }
169                pfSolution[k] = pfSolution[k] / rowK[k];
170            }
171        }
172
173        #endregion
174
175        #region inverse transpose
176
177        /// <summary>
178        /// Inverses the matrix A only if the diagonal is all non-zero.
179        /// A[i,i] != 0.0
180        /// </summary>
181        /// <param name = "A">The matrix to invert. This matrix is unchanged by this function.</param>
182        /// <param name="length">The length of the number of rows/columns in the square matrix, A.</param>
183        /// <returns>The inverted matrix, A^-1.</returns>
184        public static double[,] inverse(double[,] A, int length)
185        {
186            if (length == 1) return new[,] { { 1 / A[0, 0] } };
187            return inverseWithLUResult(LUDecomposition(A, length), length);
188        }
189
190        public static void inverseInPlace(double[,] A, int length)
191        {
192            if (length == 1)
193            {
194                A[0, 0] = 1 / A[0, 0];
195                return;
196            }
197            LUDecompositionInPlace(A, length);
198            inverseWithLUResult(A, length);
199        }
200
201
202        /// <summary>
203        /// Returns the LU decomposition of A in a new matrix.
204        /// </summary>
205        /// <param name = "A">The matrix to invert. This matrix is unchanged by this function.</param>
206        /// <param name="length">The length of the number of rows/columns in the square matrix, A.</param>
207        /// <returns>A matrix of equal size to A that combines the L and U. Here the diagonals belongs to L and the U's diagonal elements are all 1.</returns>
208        public static double[,] LUDecomposition(double[,] A, int length)
209        {
210            var B = (double[,])A.Clone();
211            // normalize row 0
212            for (var i = 1; i < length; i++) B[0, i] /= B[0, 0];
213
214            for (var i = 1; i < length; i++)
215            {
216                for (var j = i; j < length; j++)
217                {
218                    // do a column of L
219                    var sum = 0.0;
220                    for (var k = 0; k < i; k++)
221                        sum += B[j, k] * B[k, i];
222                    B[j, i] -= sum;
223                }
224                if (i == length - 1) continue;
225                for (var j = i + 1; j < length; j++)
226                {
227                    // do a row of U
228                    var sum = 0.0;
229                    for (var k = 0; k < i; k++)
230                        sum += B[i, k] * B[k, j];
231                    B[i, j] =
232                        (B[i, j] - sum) / B[i, i];
233                }
234            }
235            return B;
236        }
237
238        public static void LUDecompositionInPlace(double[,] A, int length)
239        {
240            var B = A;
241            // normalize row 0
242            for (var i = 1; i < length; i++) B[0, i] /= B[0, 0];
243
244            for (var i = 1; i < length; i++)
245            {
246                for (var j = i; j < length; j++)
247                {
248                    // do a column of L
249                    var sum = 0.0;
250                    for (var k = 0; k < i; k++)
251                        sum += B[j, k] * B[k, i];
252                    B[j, i] -= sum;
253                }
254                if (i == length - 1) continue;
255                for (var j = i + 1; j < length; j++)
256                {
257                    // do a row of U
258                    var sum = 0.0;
259                    for (var k = 0; k < i; k++)
260                        sum += B[i, k] * B[k, j];
261                    B[i, j] =
262                        (B[i, j] - sum) / B[i, i];
263                }
264            }
265        }
266
267        private static double[,] inverseWithLUResult(double[,] B, int length)
268        {
269            // this code is adapted from http://users.erols.com/mdinolfo/matrix.htm
270            // one constraint/caveat in this function is that the diagonal elts. cannot
271            // be zero.
272            // if the matrix is not square or is less than B 2x2,
273            // then this function won't work
274            #region invert L
275
276            for (var i = 0; i < length; i++)
277                for (var j = i; j < length; j++)
278                {
279                    var x = 1.0;
280                    if (i != j)
281                    {
282                        x = 0.0;
283                        for (var k = i; k < j; k++)
284                            x -= B[j, k] * B[k, i];
285                    }
286                    B[j, i] = x / B[j, j];
287                }
288
289            #endregion
290
291            #region invert U
292
293            for (var i = 0; i < length; i++)
294                for (var j = i; j < length; j++)
295                {
296                    if (i == j) continue;
297                    var sum = 0.0;
298                    for (var k = i; k < j; k++)
299                        sum += B[k, j] * ((i == k) ? 1.0 : B[i, k]);
300                    B[i, j] = -sum;
301                }
302
303            #endregion
304
305            #region final inversion
306
307            for (var i = 0; i < length; i++)
308                for (var j = 0; j < length; j++)
309                {
310                    var sum = 0.0;
311                    for (var k = ((i > j) ? i : j); k < length; k++)
312                        sum += ((j == k) ? 1.0 : B[j, k]) * B[k, i];
313                    B[j, i] = sum;
314                }
315
316            #endregion
317
318            return B;
319        }
320        /// <summary>
321        /// Returns the determinant of matrix, A.
322        /// </summary>
323        /// <param name = "A">The input argument matrix. This matrix is unchanged by this function.</param>
324        /// <exception cref = "Exception"></exception>
325        /// <returns>a single value representing the matrix's determinant.</returns>
326        public static double determinant(double[,] A)
327        {
328            if (A == null) throw new Exception("The matrix, A, is null.");
329            var length = A.GetLength(0);
330            if (length != A.GetLength(1))
331                throw new Exception("The determinant is only possible for square matrices.");
332            if (length == 0) return 0.0;
333            if (length == 1) return A[0, 0];
334            if (length == 2) return (A[0, 0] * A[1, 1]) - (A[0, 1] * A[1, 0]);
335            if (length == 3)
336                return (A[0, 0] * A[1, 1] * A[2, 2])
337                       + (A[0, 1] * A[1, 2] * A[2, 0])
338                       + (A[0, 2] * A[1, 0] * A[2, 1])
339                       - (A[0, 0] * A[1, 2] * A[2, 1])
340                       - (A[0, 1] * A[1, 0] * A[2, 2])
341                       - (A[0, 2] * A[1, 1] * A[2, 0]);
342            return determinantBig(A, length);
343        }
344
345        /// <summary>
346        /// Modifies the matrix during the computation if the dimension > 3.
347        /// </summary>
348        /// <param name="A"></param>
349        /// <returns></returns>
350        public static double determinantDestructive(double[,] A, int dimension)
351        {
352            if (A == null) throw new Exception("The matrix, A, is null.");
353            var length = dimension;
354            //if (length != A.GetLength(1))
355            //    throw new Exception("The determinant is only possible for square matrices.");
356            if (length == 0) return 0.0;
357            if (length == 1) return A[0, 0];
358            if (length == 2) return (A[0, 0] * A[1, 1]) - (A[0, 1] * A[1, 0]);
359            if (length == 3)
360                return (A[0, 0] * A[1, 1] * A[2, 2])
361                       + (A[0, 1] * A[1, 2] * A[2, 0])
362                       + (A[0, 2] * A[1, 0] * A[2, 1])
363                       - (A[0, 0] * A[1, 2] * A[2, 1])
364                       - (A[0, 1] * A[1, 0] * A[2, 2])
365                       - (A[0, 2] * A[1, 1] * A[2, 0]);
366            return determinantBigDestructive(A, length);
367        }
368
369        static double determinantBigDestructive(double[,] A, int length)
370        {
371            LUDecompositionInPlace(A, length);
372            var result = 1.0;
373            for (var i = 0; i < length; i++)
374                if (double.IsNaN(A[i, i]))
375                    return 0;
376                else result *= A[i, i];
377            return result;
378        }
379
380        /// <summary>
381        /// Returns the determinant of matrix, A. Only used internally for matrices larger than 3.
382        /// </summary>
383        /// <param name="A">The input argument matrix. This matrix is unchanged by this function.</param>
384        /// <param name="length">The length of the side of the square matrix.</param>
385        /// <returns>
386        /// a single value representing the matrix's determinant.
387        /// </returns>
388        public static double determinantBig(double[,] A, int length)
389        {
390            double[,] L, U;
391            LUDecomposition(A, out L, out U, length);
392            var result = 1.0;
393            for (var i = 0; i < length; i++)
394                if (double.IsNaN(L[i, i]))
395                    return 0;
396                else result *= L[i, i];
397            return result;
398        }
399        /// <summary>
400        /// Returns the LU decomposition of A in a new matrix.
401        /// </summary>
402        /// <param name = "A">The matrix to invert. This matrix is unchanged by this function.</param>
403        /// <param name = "L">The L matrix is output where the diagonal elements are included and not (necessarily) equal to one.</param>
404        /// <param name = "U">The U matrix is output where the diagonal elements are all equal to one.</param>
405        /// <param name="length">The length of the number of rows/columns in the square matrix, A.</param>
406        public static void LUDecomposition(double[,] A, out double[,] L, out double[,] U, int length)
407        {
408            L = LUDecomposition(A, length);
409            U = new double[length, length];
410            for (var i = 0; i < length; i++)
411            {
412                U[i, i] = 1.0;
413                for (var j = i + 1; j < length; j++)
414                {
415                    U[i, j] = L[i, j];
416                    L[i, j] = 0.0;
417                }
418            }
419        }
420
421        #endregion
422
423        #region norm
424        /// <summary>
425        /// Returns to 2-norm (square root of the sum of squares of all terms)
426        /// of the vector, x.
427        /// </summary>
428        /// <param name="x">The vector, x.</param>
429        /// <param name="dontDoSqrt">if set to <c>true</c> [don't take the square root].</param>
430        /// <returns>Scalar value of 2-norm.</returns>
431        public static double norm2(double[] x, int dim, Boolean dontDoSqrt = false)
432        {
433            double norm = 0;
434            for (int i = 0; i < dim; i++)
435            {
436                var t = x[i];
437                norm += t * t;
438            }
439            return dontDoSqrt ? norm : Math.Sqrt(norm);
440        }
441        /// <summary>
442        /// Returns to normalized vector (has lenght or 2-norm of 1))
443        /// of the vector, x.
444        /// </summary>
445        /// <param name="x">The vector, x.</param>
446        /// <param name="length">The length of the vector.</param>
447        /// <returns>unit vector.</returns>
448        public static double[] normalize(double[] x, int length)
449        {
450            return divide(x, norm2(x, length), length);
451        }
452
453        public static void normalizeInPlace(double[] x, int length)
454        {
455            double f = 1.0 / norm2(x, length);
456            for (int i = 0; i < length; i++) x[i] *= f;
457        }
458        #endregion
459
460        #region make extract
461        /// <summary>
462        /// Gets a row of matrix, A.
463        /// </summary>
464        /// <param name = "rowIndex">The row index.</param>
465        /// <param name = "A">The matrix, A.</param>
466        /// <returns>A double array that contains the requested row</returns>
467        public static double[] GetRow(int rowIndex, double[,] A)
468        {
469            var numRows = A.GetLength(0);
470            var numCols = A.GetLength(1);
471            if ((rowIndex < 0) || (rowIndex >= numRows))
472                throw new Exception("MatrixMath Size Error: An index value of "
473                                    + rowIndex
474                                    + " for getRow is not in required range from 0 up to (but not including) "
475                                    + numRows + ".");
476            var v = new double[numCols];
477            for (var i = 0; i < numCols; i++)
478                v[i] = A[rowIndex, i];
479            return v;
480        }
481        /// <summary>
482        /// Sets/Replaces the given column of matrix A with the vector v.
483        /// </summary>
484        /// <param name = "colIndex">Index of the col.</param>
485        /// <param name = "A">The A.</param>
486        /// <param name = "v">The v.</param>
487        public static void SetColumn(int colIndex, double[,] A, IList<double> v)
488        {
489            var numRows = A.GetLength(0);
490            var numCols = A.GetLength(1);
491            if ((colIndex < 0) || (colIndex >= numCols))
492                throw new Exception("MatrixMath Size Error: An index value of "
493                                    + colIndex
494                                    + " for getColumn is not in required range from 0 up to (but not including) "
495                                    + numCols + ".");
496            for (var i = 0; i < numRows; i++)
497                A[i, colIndex] = v[i];
498        }
499        /// <summary>
500        /// Sets/Replaces the given row of matrix A with the vector v.
501        /// </summary>
502        /// <param name = "rowIndex">The index of the row, rowIndex.</param>
503        /// <param name = "A">The matrix, A.</param>
504        /// <param name = "v">The vector, v.</param>
505        public static void SetRow(int rowIndex, double[,] A, IList<double> v)
506        {
507            var numRows = A.GetLength(0);
508            var numCols = A.GetLength(1);
509            if ((rowIndex < 0) || (rowIndex >= numRows))
510                throw new Exception("MatrixMath Size Error: An index value of "
511                                    + rowIndex
512                                    + " for getRow is not in required range from 0 up to (but not including) "
513                                    + numRows + ".");
514            for (var i = 0; i < numCols; i++)
515                A[rowIndex, i] = v[i];
516        }
517        /// <summary>
518        /// Makes the zero vector.
519        /// </summary>
520        /// <param name = "p">The p.</param>
521        /// <returns></returns>
522        public static double[] makeZeroVector(int p)
523        {
524            if (p <= 0) throw new Exception("The size, p, must be a positive integer.");
525            return new double[p];
526        }
527
528        #endregion
529
530        #region add subtract multiply
531        /// <summary>
532        /// The cross product of two double vectors, A and B, which are of length, 2.
533        /// In actuality, there is no cross-product for 2D. This is shorthand for 2D systems
534        /// that are really simplifications of 3D. The returned scalar is actually the value in
535        /// the third (read: z) direction.
536        /// </summary>
537        /// <param name = "A">1D double Array, A</param>
538        /// <param name = "B">1D double Array, B</param>
539        /// <returns></returns>
540        public static double crossProduct2(IList<double> A, IList<double> B)
541        {
542            if (((A.Count() == 2) && (B.Count() == 2))
543                || ((A.Count() == 3) && (B.Count() == 3) && A[2] == 0.0 && B[2] == 0.0))
544                return A[0] * B[1] - B[0] * A[1];
545            throw new Exception("This cross product \"shortcut\" is only used with 2D vectors to get the single value in the,"
546                                + "would be, Z-direction.");
547        }
548
549        /// <summary>
550        /// The dot product of the two 1D double vectors A and B
551        /// </summary>
552        /// <param name="A">1D double Array, A</param>
553        /// <param name="B">1D double Array, B</param>
554        /// <param name="length">The length of both vectors A and B. This is an optional argument, but if it is already known
555        /// - there is a slight speed advantage to providing it.</param>
556        /// <returns>
557        /// A double value that contains the dot product
558        /// </returns>
559        public static double dotProduct(IList<double> A, IList<double> B, int length)
560        {
561            var c = 0.0;
562            for (var i = 0; i != length; i++)
563                c += A[i] * B[i];
564            return c;
565        }
566        /// <summary>
567        /// The dot product of the one 1D int vector and one 1D double vector
568        /// </summary>
569        /// <param name="A">1D int Array, A</param>
570        /// <param name="B">1D double Array, B</param>
571        /// <param name="length">The length of both vectors A and B. This is an optional argument, but if it is already known
572        /// - there is a slight speed advantage to providing it.</param>
573        /// <returns>
574        /// A double value that contains the dot product
575        /// </returns>
576        public static double dotProduct(IList<int> A, IList<double> B, int length)
577        {
578            var c = 0.0;
579            for (var i = 0; i != length; i++)
580                c += A[i] * B[i];
581            return c;
582        }
583        /// <summary>
584        /// Subtracts one vector (B) from the other (A). C = A - B.
585        /// </summary>
586        /// <param name = "A">The minuend vector, A (1D double)</param>
587        /// <param name = "B">The subtrahend vector, B (1D double)</param>
588        /// <param name="length">The length of the vectors.</param>
589        /// <returns>Returns the difference vector, C (1D double)</returns>
590        public static double[] subtract(IList<double> A, IList<double> B, int length)
591        {
592            var c = new double[length];
593            for (var i = 0; i != length; i++)
594                c[i] = A[i] - B[i];
595            return c;
596        }
597        /// <summary>
598        /// Adds arrays A and B
599        /// </summary>
600        /// <param name = "A">1D double array 1</param>
601        /// <param name = "B">1D double array 2</param>
602        /// <param name="length">The length of the array.</param>
603        /// <returns>1D double array that contains sum of vectros A and B</returns>
604        public static double[] add(IList<double> A, IList<double> B, int length)
605        {
606            var c = new double[length];
607            for (var i = 0; i != length; i++)
608                c[i] = A[i] + B[i];
609            return c;
610        }
611        /// <summary>
612        /// Divides all elements of a 1D double array by the double value.
613        /// </summary>
614        /// <param name="B">The vector to be divided</param>
615        /// <param name="a">The double value to be divided by, the divisor.</param>
616        /// <param name="length">The length of the vector B. This is an optional argument, but if it is already known
617        /// - there is a slight speed advantage to providing it.</param>
618        /// <returns>
619        /// A 1D double array that contains the product
620        /// </returns>
621        public static double[] divide(IList<double> B, double a, int length)
622        { return multiply((1 / a), B, length); }
623        /// <summary>
624        /// Multiplies all elements of a 1D double array with the double value.
625        /// </summary>
626        /// <param name="a">The double value to be multiplied</param>
627        /// <param name="B">The double vector to be multiplied with</param>
628        /// <param name="length">The length of the vector B. This is an optional argument, but if it is already known
629        /// - there is a slight speed advantage to providing it.</param>
630        /// <returns>
631        /// A 1D double array that contains the product
632        /// </returns>
633        public static double[] multiply(double a, IList<double> B, int length)
634        {
635            // scale vector B by the amount of scalar a
636            var c = new double[length];
637            for (var i = 0; i != length; i++)
638                c[i] = a * B[i];
639            return c;
640        }
641
642        /// <summary>
643        /// Product of a matrix and a vector (2D double and 1D double)
644        /// </summary>
645        /// <param name = "A">2D double Array</param>
646        /// <param name = "B">1D double array - column vector (1 element row)</param>
647        /// <param name="numRows">The number of rows.</param>
648        /// <param name="numCols">The number of columns.</param>
649        /// <returns>A 1D double array that is the product of the two matrices A and B</returns>
650        public static double[] multiply(double[,] A, IList<double> B, int numRows, int numCols)
651        {
652            var C = new double[numRows];
653
654            for (var i = 0; i != numRows; i++)
655            {
656                C[i] = 0.0;
657                for (var j = 0; j != numCols; j++)
658                    C[i] += A[i, j] * B[j];
659            }
660            return C;
661        }
662
663        public static void multiplyInto(double[,] A, double[] B, int numRows, int numCols, double[] target)
664        {
665            var C = target;
666
667            for (var i = 0; i != numRows; i++)
668            {
669                C[i] = 0.0;
670                for (var j = 0; j != numCols; j++)
671                    C[i] += A[i, j] * B[j];
672            }
673        }
674        /// <summary>
675        /// The cross product of two double vectors, A and B, which are of length, 3.
676        /// This is equivalent to calling crossProduct, but a slight speed advantage
677        /// may exist in skipping directly to this sub-function.
678        /// </summary>
679        /// <param name = "A">1D double Array, A</param>
680        /// <param name = "B">1D double Array, B</param>
681        /// <returns></returns>
682        public static double[] crossProduct3(IList<double> A, IList<double> B)
683        {
684            return new[]
685                       {
686                           A[1]*B[2] - B[1]*A[2],
687                           A[2]*B[0] - B[2]*A[0],
688                           A[0]*B[1] - B[0]*A[1]
689                       };
690        }
691
692        #endregion
693
694
695        public static double subtractAndDot(double[] n, double[] l, double[] r, int dim)
696        {
697            double acc = 0;
698            for (int i = 0; i < dim; i++)
699            {
700                double t = l[i] - r[i];
701                acc += n[i] * t;
702            }
703
704            return acc;
705        }
706    }
707}
Note: See TracBrowser for help on using the repository browser.