1 /*=========================================================================
3 Program: Visualization Toolkit
4 Module: $RCSfile: vtkMath.h,v $
6 Date: $Date: 2003/01/30 17:41:20 $
7 Version: $Revision: 1.83 $
9 Copyright (c) 1993-2002 Ken Martin, Will Schroeder, Bill Lorensen
11 See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
13 This software is distributed WITHOUT ANY WARRANTY; without even
14 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
15 PURPOSE. See the above copyright notice for more information.
17 =========================================================================*/
18 // .NAME vtkMath - performs common math operations
19 // .SECTION Description
20 // vtkMath is provides methods to perform common math operations. These
21 // include providing constants such as Pi; conversion from degrees to
22 // radians; vector operations such as dot and cross products and vector
23 // norm; matrix determinant for 2x2 and 3x3 matrices; and random
29 #include "vtkObject.h"
31 class VTK_COMMON_EXPORT vtkMath : public vtkObject
34 static vtkMath *New();
35 vtkTypeRevisionMacro(vtkMath,vtkObject);
39 static float Pi() {return 3.14159265358979f;};
40 static float DegreesToRadians() {return 0.017453292f;};
41 static float RadiansToDegrees() {return 57.2957795131f;};
44 // Useful constants. (double-precision version)
45 static double DoubleDegreesToRadians() {return 0.017453292519943295;};
46 static double DoublePi() {return 3.1415926535897932384626;};
47 static double DoubleRadiansToDegrees() {return 57.29577951308232;};
50 // Rounds a float to the nearest integer.
51 static int Round(float f) {
52 return static_cast<int>(f + (f >= 0 ? 0.5 : -0.5)); }
53 static int Round(double f) {
54 return static_cast<int>(f + (f >= 0 ? 0.5 : -0.5)); }
57 // Dot product of two 3-vectors (float version).
58 static float Dot(const float x[3], const float y[3]) {
59 return (x[0]*y[0] + x[1]*y[1] + x[2]*y[2]);};
62 // Dot product of two 3-vectors (double-precision version).
63 static double Dot(const double x[3], const double y[3]) {
64 return (x[0]*y[0] + x[1]*y[1] + x[2]*y[2]);};
67 // Cross product of two 3-vectors. Result vector in z[3].
68 static void Cross(const float x[3], const float y[3], float z[3]);
71 // Cross product of two 3-vectors. Result vector in z[3]. (double-precision
73 static void Cross(const double x[3], const double y[3], double z[3]);
76 // Compute the norm of n-vector.
77 static float Norm(const float* x, int n);
80 // Compute the norm of 3-vector.
81 static float Norm(const float x[3]) {
82 return static_cast<float> (sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]));};
85 // Compute the norm of 3-vector (double-precision version).
86 static double Norm(const double x[3]) {
87 return sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);};
90 // Normalize (in place) a 3-vector. Returns norm of vector.
91 static float Normalize(float x[3]);
94 // Normalize (in place) a 3-vector. Returns norm of vector
95 // (double-precision version).
96 static double Normalize(double x[3]);
99 // Given a unit vector x, find two unit vectors y and z such that
100 // x cross y = z (i.e. the vectors are perpendicular to each other).
101 // There is an infinite number of such vectors, specify an angle theta
102 // to choose one set. If you want only one perpendicular vector,
103 // specify NULL for z.
104 static void Perpendiculars(const double x[3], double y[3], double z[3],
106 static void Perpendiculars(const float x[3], float y[3], float z[3],
110 // Compute distance squared between two points.
111 static float Distance2BetweenPoints(const float x[3], const float y[3]);
114 // Compute distance squared between two points (double precision version).
115 static double Distance2BetweenPoints(const double x[3], const double y[3]);
118 // Dot product of two 2-vectors. The third (z) component is ignored.
119 static float Dot2D(const float x[3], const float y[3]) {
120 return (x[0]*y[0] + x[1]*y[1]);};
123 // Dot product of two 2-vectors. The third (z) component is
124 // ignored (double-precision version).
125 static double Dot2D(const double x[3], const double y[3]) {
126 return (x[0]*y[0] + x[1]*y[1]);};
129 // Compute the norm of a 2-vector. Ignores z-component.
130 static float Norm2D(const float x[3]) {
131 return static_cast<float> (sqrt(x[0]*x[0] + x[1]*x[1]));};
134 // Compute the norm of a 2-vector. Ignores z-component
135 // (double-precision version).
136 static double Norm2D(const double x[3]) {
137 return sqrt(x[0]*x[0] + x[1]*x[1]);};
140 // Normalize (in place) a 2-vector. Returns norm of vector. Ignores
142 static float Normalize2D(float x[3]);
145 // Normalize (in place) a 2-vector. Returns norm of vector. Ignores
146 // z-component (double-precision version).
147 static double Normalize2D(double x[3]);
150 // Compute determinant of 2x2 matrix. Two columns of matrix are input.
151 static float Determinant2x2(const float c1[2], const float c2[2]) {
152 return (c1[0]*c2[1] - c2[0]*c1[1]);};
155 // Calculate the determinant of a 2x2 matrix: | a b | | c d |
156 static double Determinant2x2(double a, double b, double c, double d) {
157 return (a * d - b * c);};
160 // LU Factorization of a 3x3 matrix. The diagonal elements are the
161 // multiplicative inverse of those in the standard LU factorization.
162 static void LUFactor3x3(float A[3][3], int index[3]);
163 static void LUFactor3x3(double A[3][3], int index[3]);
166 // LU back substitution for a 3x3 matrix. The diagonal elements are the
167 // multiplicative inverse of those in the standard LU factorization.
168 static void LUSolve3x3(const float A[3][3], const int index[3],
170 static void LUSolve3x3(const double A[3][3], const int index[3],
174 // Solve Ay = x for y and place the result in y. The matrix A is
175 // destroyed in the process.
176 static void LinearSolve3x3(const float A[3][3], const float x[3],
178 static void LinearSolve3x3(const double A[3][3], const double x[3],
182 // Multiply a vector by a 3x3 matrix. The result is placed in out.
183 static void Multiply3x3(const float A[3][3], const float in[3],
185 static void Multiply3x3(const double A[3][3], const double in[3],
189 // Mutltiply one 3x3 matrix by another according to C = AB.
190 static void Multiply3x3(const float A[3][3], const float B[3][3],
192 static void Multiply3x3(const double A[3][3], const double B[3][3],
196 // Transpose a 3x3 matrix.
197 static void Transpose3x3(const float A[3][3], float AT[3][3]);
198 static void Transpose3x3(const double A[3][3], double AT[3][3]);
201 // Invert a 3x3 matrix.
202 static void Invert3x3(const float A[3][3], float AI[3][3]);
203 static void Invert3x3(const double A[3][3], double AI[3][3]);
206 // Set A to the identity matrix.
207 static void Identity3x3(float A[3][3]);
208 static void Identity3x3(double A[3][3]);
211 // Return the determinant of a 3x3 matrix.
212 static double Determinant3x3(float A[3][3]);
213 static double Determinant3x3(double A[3][3]);
216 // Compute determinant of 3x3 matrix. Three columns of matrix are input.
217 static float Determinant3x3(const float c1[3],
222 // Calculate the determinant of a 3x3 matrix in the form:
226 static double Determinant3x3(double a1, double a2, double a3,
227 double b1, double b2, double b3,
228 double c1, double c2, double c3);
231 // Convert a quaternion to a 3x3 rotation matrix. The quaternion
232 // does not have to be normalized beforehand.
233 static void QuaternionToMatrix3x3(const float quat[4], float A[3][3]);
234 static void QuaternionToMatrix3x3(const double quat[4], double A[3][3]);
237 // Convert a 3x3 matrix into a quaternion. This will provide the
238 // best possible answer even if the matrix is not a pure rotation matrix.
239 // The method used is that of B.K.P. Horn.
240 static void Matrix3x3ToQuaternion(const float A[3][3], float quat[4]);
241 static void Matrix3x3ToQuaternion(const double A[3][3], double quat[4]);
244 // Orthogonalize a 3x3 matrix and put the result in B. If matrix A
245 // has a negative determinant, then B will be a rotation plus a flip
246 // i.e. it will have a determinant of -1.
247 static void Orthogonalize3x3(const float A[3][3], float B[3][3]);
248 static void Orthogonalize3x3(const double A[3][3], double B[3][3]);
251 // Diagonalize a symmetric 3x3 matrix and return the eigenvalues in
252 // w and the eigenvectors in the columns of V. The matrix V will
253 // have a positive determinant, and the three eigenvectors will be
254 // aligned as closely as possible with the x, y, and z axes.
255 static void Diagonalize3x3(const float A[3][3], float w[3], float V[3][3]);
256 static void Diagonalize3x3(const double A[3][3],double w[3],double V[3][3]);
259 // Perform singular value decomposition on a 3x3 matrix. This is not
260 // done using a conventional SVD algorithm, instead it is done using
261 // Orthogonalize3x3 and Diagonalize3x3. Both output matrices U and VT
262 // will have positive determinants, and the w values will be arranged
263 // such that the three rows of VT are aligned as closely as possible
264 // with the x, y, and z axes respectively. If the determinant of A is
265 // negative, then the three w values will be negative.
266 static void SingularValueDecomposition3x3(const float A[3][3],
267 float U[3][3], float w[3],
269 static void SingularValueDecomposition3x3(const double A[3][3],
270 double U[3][3], double w[3],
274 // Solve linear equations Ax = b using Crout's method. Input is square
275 // matrix A and load vector x. Solution x is written over load vector. The
276 // dimension of the matrix is specified in size. If error is found, method
278 static int SolveLinearSystem(double **A, double *x, int size);
281 // Invert input square matrix A into matrix AI.
282 // Note that A is modified during
283 // the inversion. The size variable is the dimension of the matrix. Returns 0
284 // if inverse not computed.
285 static int InvertMatrix(double **A, double **AI, int size);
288 // Thread safe version of InvertMatrix method.
289 // Working memory arrays tmp1SIze and tmp2Size
290 // of length size must be passed in.
291 static int InvertMatrix(double **A, double **AI, int size,
292 int *tmp1Size, double *tmp2Size);
295 // Factor linear equations Ax = b using LU decomposition A = LU where L is
296 // lower triangular matrix and U is upper triangular matrix. Input is
297 // square matrix A, integer array of pivot indices index[0->n-1], and size
298 // of square matrix n. Output factorization LU is in matrix A. If error is
299 // found, method returns 0.
300 static int LUFactorLinearSystem(double **A, int *index, int size);
303 // Thread safe version of LUFactorLinearSystem method.
304 // Working memory array tmpSize of length size
305 // must be passed in.
306 static int LUFactorLinearSystem(double **A, int *index, int size,
310 // Solve linear equations Ax = b using LU decomposition A = LU where L is
311 // lower triangular matrix and U is upper triangular matrix. Input is
312 // factored matrix A=LU, integer array of pivot indices index[0->n-1],
313 // load vector x[0->n-1], and size of square matrix n. Note that A=LU and
314 // index[] are generated from method LUFactorLinearSystem). Also, solution
315 // vector is written directly over input load vector.
316 static void LUSolveLinearSystem(double **A, int *index,
317 double *x, int size);
320 // Estimate the condition number of a LU factored matrix. Used to judge the
321 // accuracy of the solution. The matrix A must have been previously factored
322 // using the method LUFactorLinearSystem. The condition number is the ratio
323 // of the infinity matrix norm (i.e., maximum value of matrix component)
324 // divided by the minimum diagonal value. (This works for triangular matrices
325 // only: see Conte and de Boor, Elementary Numerical Analysis.)
326 static double EstimateMatrixCondition(double **A, int size);
329 // Initialize seed value. NOTE: Random() has the bad property that
330 // the first random number returned after RandomSeed() is called
331 // is proportional to the seed value! To help solve this, call
332 // RandomSeed() a few times inside seed. This doesn't ruin the
333 // repeatability of Random().
334 static void RandomSeed(long s);
337 // Generate random numbers between 0.0 and 1.0.
338 // This is used to provide portability across different systems.
339 static float Random();
342 // Generate random number between (min,max).
343 static float Random(float min, float max);
346 // Jacobi iteration for the solution of eigenvectors/eigenvalues of a 3x3
347 // real symmetric matrix. Square 3x3 matrix a; output eigenvalues in w;
348 // and output eigenvectors in v. Resulting eigenvalues/vectors are sorted
349 // in decreasing order; eigenvectors are normalized.
350 static int Jacobi(float **a, float *w, float **v);
351 static int Jacobi(double **a, double *w, double **v);
354 // JacobiN iteration for the solution of eigenvectors/eigenvalues of a nxn
355 // real symmetric matrix. Square nxn matrix a; size of matrix in n; output
356 // eigenvalues in w; and output eigenvectors in v. Resulting
357 // eigenvalues/vectors are sorted in decreasing order; eigenvectors are
358 // normalized. w and v need to be allocated previously
359 static int JacobiN(float **a, int n, float *w, float **v);
360 static int JacobiN(double **a, int n, double *w, double **v);
363 // Solves a cubic equation c0*t^3 + c1*t^2 + c2*t + c3 = 0 when c0, c1, c2,
364 // and c3 are REAL. Solution is motivated by Numerical Recipes In C 2nd
365 // Ed. Return array contains number of (real) roots (counting multiple
366 // roots as one) followed by roots themselves. The value in roots[4] is a
367 // integer giving further information about the roots (see return codes for
368 // int SolveCubic()).
369 static double* SolveCubic(double c0, double c1, double c2, double c3);
372 // Solves a quadratic equation c1*t^2 + c2*t + c3 = 0 when c1, c2, and c3
373 // are REAL. Solution is motivated by Numerical Recipes In C 2nd Ed.
374 // Return array contains number of (real) roots (counting multiple roots as
375 // one) followed by roots themselves. Note that roots[3] contains a return
376 // code further describing solution - see documentation for SolveCubic()
377 // for meaning of return codes.
378 static double* SolveQuadratic(double c0, double c1, double c2);
381 // Solves a linear equation c2*t + c3 = 0 when c2 and c3 are REAL.
382 // Solution is motivated by Numerical Recipes In C 2nd Ed.
383 // Return array contains number of roots followed by roots themselves.
384 static double* SolveLinear(double c0, double c1);
387 // Solves a cubic equation when c0, c1, c2, And c3 Are REAL. Solution
388 // is motivated by Numerical Recipes In C 2nd Ed. Roots and number of
389 // real roots are stored in user provided variables r1, r2, r3, and
390 // num_roots. Note that the function can return the following integer
391 // values describing the roots: (0)-no solution; (-1)-infinite number
392 // of solutions; (1)-one distinct real root of multiplicity 3 (stored
393 // in r1); (2)-two distinct real roots, one of multiplicity 2 (stored
394 // in r1 & r2); (3)-three distinct real roots; (-2)-quadratic equation
395 // with complex conjugate solution (real part of root returned in r1,
396 // imaginary in r2); (-3)-one real root and a complex conjugate pair
397 // (real root in r1 and real part of pair in r2 and imaginary in r3).
398 static int SolveCubic(double c0, double c1, double c2, double c3,
399 double *r1, double *r2, double *r3, int *num_roots);
402 // Solves A Quadratic Equation c1*t^2 + c2*t + c3 = 0 when
403 // c1, c2, and c3 are REAL.
404 // Solution is motivated by Numerical Recipes In C 2nd Ed.
405 // Roots and number of roots are stored in user provided variables
407 static int SolveQuadratic(double c0, double c1, double c2,
408 double *r1, double *r2, int *num_roots);
411 // Solves a linear equation c2*t + c3 = 0 when c2 and c3 are REAL.
412 // Solution is motivated by Numerical Recipes In C 2nd Ed.
413 // Root and number of (real) roots are stored in user provided variables
415 static int SolveLinear(double c0, double c1, double *r1, int *num_roots);
419 // Solves for the least squares best fit matrix for the equation X'M' = Y'.
420 // Uses pseudoinverse to get the ordinary least squares.
421 // The inputs and output are transposed matrices.
422 // Dimensions: X' is numberOfSamples by xOrder,
423 // Y' is numberOfSamples by yOrder,
424 // M' dimension is xOrder by yOrder.
425 // M' should be pre-allocated. All matrices are row major. The resultant
426 // matrix M' should be pre-multiplied to X' to get Y', or transposed and
427 // then post multiplied to X to get Y
428 static int SolveLeastSquares(int numberOfSamples, double **xt, int xOrder,
429 double **yt, int yOrder, double **mt);
432 // Convert color in RGB format (Red, Green, Blue) to HSV format
433 // (Hue, Saturation, Value). The input color is not modified.
434 static void RGBToHSV(float rgb[3], float hsv[3])
436 RGBToHSV(rgb[0], rgb[1], rgb[2], hsv, hsv+1, hsv+2);
438 static void RGBToHSV(float r, float g, float b, float *h, float *s, float *v);
441 // Convert color in HSV format (Hue, Saturation, Value) to RGB
442 // format (Red, Green, Blue). The input color is not modified.
443 static void HSVToRGB(float hsv[3], float rgb[3])
445 HSVToRGB(hsv[0], hsv[1], hsv[2], rgb, rgb+1, rgb+2);
447 static void HSVToRGB(float h, float s, float v, float *r, float *g, float *b);
455 vtkMath(const vtkMath&); // Not implemented.
456 void operator=(const vtkMath&); // Not implemented.
459 inline float vtkMath::Normalize(float x[3])
462 if ( (den = vtkMath::Norm(x)) != 0.0 )
464 for (int i=0; i < 3; i++)
471 inline double vtkMath::Normalize(double x[3])
474 if ( (den = vtkMath::Norm(x)) != 0.0 )
476 for (int i=0; i < 3; i++)
484 inline float vtkMath::Normalize2D(float x[3])
487 if ( (den = vtkMath::Norm2D(x)) != 0.0 )
489 for (int i=0; i < 2; i++)
497 inline double vtkMath::Normalize2D(double x[3])
500 if ( (den = vtkMath::Norm2D(x)) != 0.0 )
502 for (int i=0; i < 2; i++)
510 inline float vtkMath::Determinant3x3(const float c1[3],
514 return c1[0]*c2[1]*c3[2] + c2[0]*c3[1]*c1[2] + c3[0]*c1[1]*c2[2] -
515 c1[0]*c3[1]*c2[2] - c2[0]*c1[1]*c3[2] - c3[0]*c2[1]*c1[2];
518 inline double vtkMath::Determinant3x3(double a1, double a2, double a3,
519 double b1, double b2, double b3,
520 double c1, double c2, double c3)
522 return ( a1 * vtkMath::Determinant2x2( b2, b3, c2, c3 )
523 - b1 * vtkMath::Determinant2x2( a2, a3, c2, c3 )
524 + c1 * vtkMath::Determinant2x2( a2, a3, b2, b3 ) );
527 inline float vtkMath::Distance2BetweenPoints(const float x[3],
530 return ((x[0]-y[0])*(x[0]-y[0]) + (x[1]-y[1])*(x[1]-y[1]) +
531 (x[2]-y[2])*(x[2]-y[2]));
533 inline double vtkMath::Distance2BetweenPoints(const double x[3],
536 return ((x[0]-y[0])*(x[0]-y[0]) + (x[1]-y[1])*(x[1]-y[1]) +
537 (x[2]-y[2])*(x[2]-y[2]));
540 inline float vtkMath::Random(float min, float max)
542 return (min + vtkMath::Random()*(max-min));
545 // Cross product of two 3-vectors. Result vector in z[3].
546 inline void vtkMath::Cross(const float x[3], const float y[3], float z[3])
548 float Zx = x[1]*y[2] - x[2]*y[1];
549 float Zy = x[2]*y[0] - x[0]*y[2];
550 float Zz = x[0]*y[1] - x[1]*y[0];
551 z[0] = Zx; z[1] = Zy; z[2] = Zz;
554 // Cross product of two 3-vectors. Result vector in z[3].
555 inline void vtkMath::Cross(const double x[3], const double y[3], double z[3])
557 double Zx = x[1]*y[2] - x[2]*y[1];
558 double Zy = x[2]*y[0] - x[0]*y[2];
559 double Zz = x[0]*y[1] - x[1]*y[0];
560 z[0] = Zx; z[1] = Zy; z[2] = Zz;
564 //----------------------------------------------------------------------------
566 inline double vtkDeterminant3x3(T A[3][3])
568 return A[0][0]*A[1][1]*A[2][2] + A[1][0]*A[2][1]*A[0][2] +
569 A[2][0]*A[0][1]*A[1][2] - A[0][0]*A[2][1]*A[1][2] -
570 A[1][0]*A[0][1]*A[2][2] - A[2][0]*A[1][1]*A[0][2];
574 inline double vtkMath::Determinant3x3(float A[3][3])
576 return vtkDeterminant3x3(A);
579 inline double vtkMath::Determinant3x3(double A[3][3])
581 return vtkDeterminant3x3(A);