Context Navigation

← Previous Revision
Latest Revision
Next Revision →
Blame
Revision Log

MatrixOps.h @ 373

Last change on this file since 373 was 4, checked in by Torben Dannhauer, 15 years ago

File size: 27.6 KB

Line
1	/************************************************************** ggt-head beg
2	*
3	* GGT: Generic Graphics Toolkit
4	*
5	* Original Authors:
6	* Allen Bierbaum
7	*
8	* -----------------------------------------------------------------
9	* File: MatrixOps.h,v
10	* Date modified: 2005/05/16 14:19:44
11	* Version: 1.39
12	* -----------------------------------------------------------------
13	*
14	*********************************************************** ggt-head end */
15	/*************************************************************** ggt-cpr beg
16	*
17	* GGT: The Generic Graphics Toolkit
18	* Copyright (C) 2001,2002 Allen Bierbaum
19	*
20	* This library is free software; you can redistribute it and/or
21	* modify it under the terms of the GNU Lesser General Public
22	* License as published by the Free Software Foundation; either
23	* version 2.1 of the License, or (at your option) any later version.
24	*
25	* This library is distributed in the hope that it will be useful,
26	* but WITHOUT ANY WARRANTY; without even the implied warranty of
27	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
28	* Lesser General Public License for more details.
29	*
30	* You should have received a copy of the GNU Lesser General Public
31	* License along with this library; if not, write to the Free Software
32	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
33	*
34	************************************************************ ggt-cpr end */
35	#ifndef _GMTL_MATRIXOPS_H_
36	#define _GMTL_MATRIXOPS_H_
37
38	#include <iostream> // for std::cerr
39	#include <algorithm> // needed for std::swap
40	#include <gmtl/Matrix.h>
41	#include <gmtl/Math.h>
42	#include <gmtl/Vec.h>
43	#include <gmtl/VecOps.h>
44	#include <gmtl/Util/Assert.h>
45
46	namespace gmtl
47	{
48	/** @ingroup Ops
49	* @name Matrix Operations
50	* @{
51	*/
52
53	/** Make identity matrix out the matrix.
54	* @post Every element is 0 except the matrix's diagonal, whose elements are 1.
55	*/
56	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
57	inline Matrix<DATA_TYPE, ROWS, COLS>& identity( Matrix<DATA_TYPE, ROWS, COLS>& result )
58	{
59	if(result.mState != Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY) // if not already ident
60	{
61	// TODO: mp
62	for (unsigned int r = 0; r < ROWS; ++r)
63	for (unsigned int c = 0; c < COLS; ++c)
64	result( r, c ) = (DATA_TYPE)0.0;
65
66	// TODO: mp
67	for (unsigned int x = 0; x < Math::Min( COLS, ROWS ); ++x)
68	result( x, x ) = (DATA_TYPE)1.0;
69
70	result.mState = Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY;
71	// result.mState = Matrix<DATA_TYPE, ROWS, COLS>::FULL;
72	}
73
74	return result;
75	}
76
77
78	/** zero out the matrix.
79	* @post every element is 0.
80	*/
81	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
82	inline Matrix<DATA_TYPE, ROWS, COLS>& zero( Matrix<DATA_TYPE, ROWS, COLS>& result )
83	{
84	if (result.mState == Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY)
85	{
86	for (unsigned int x = 0; x < Math::Min( ROWS, COLS ); ++x)
87	{
88	result( x, x ) = (DATA_TYPE)0;
89	}
90	}
91	else
92	{
93	for (unsigned int x = 0; x < ROWS*COLS; ++x)
94	{
95	result.mData[x] = (DATA_TYPE)0;
96	}
97	}
98	result.mState = Matrix<DATA_TYPE, ROWS, COLS>::ORTHOGONAL;
99	return result;
100	}
101
102
103	/** matrix multiply.
104	* @PRE: With regard to size (ROWS/COLS): if lhs is m x p, and rhs is p x n, then result is m x n (mult func undefined otherwise)
105	* @POST: returns a m x n sized matrix
106	* @post: result = lhs * rhs (where rhs is applied first)
107	*/
108	template <typename DATA_TYPE, unsigned ROWS, unsigned INTERNAL, unsigned COLS>
109	inline Matrix<DATA_TYPE, ROWS, COLS>& mult( Matrix<DATA_TYPE, ROWS, COLS>& result,
110	const Matrix<DATA_TYPE, ROWS, INTERNAL>& lhs,
111	const Matrix<DATA_TYPE, INTERNAL, COLS>& rhs )
112	{
113	Matrix<DATA_TYPE, ROWS, COLS> ret_mat; // prevent aliasing
114	zero( ret_mat );
115
116	// p. 150 Numerical Analysis (second ed.)
117	// if A is m x p, and B is p x n, then AB is m x n
118	// (AB)ij = [k = 1 to p] (a)ik (b)kj (where: 1 <= i <= m, 1 <= j <= n)
119	for (unsigned int i = 0; i < ROWS; ++i) // 1 <= i <= m
120	for (unsigned int j = 0; j < COLS; ++j) // 1 <= j <= n
121	for (unsigned int k = 0; k < INTERNAL; ++k) // [k = 1 to p]
122	ret_mat( i, j ) += lhs( i, k ) * rhs( k, j );
123
124	// track state
125	ret_mat.mState = combineMatrixStates( lhs.mState, rhs.mState );
126	return result = ret_mat;
127	}
128
129	/** matrix * matrix.
130	* @PRE: With regard to size (ROWS/COLS): if lhs is m x p, and rhs is p x n, then result is m x n (mult func undefined otherwise)
131	* @POST: returns a m x n sized matrix == lhs * rhs (where rhs is applied first)
132	* returns a temporary, is slower.
133	*/
134	template <typename DATA_TYPE, unsigned ROWS, unsigned INTERNAL, unsigned COLS>
135	inline Matrix<DATA_TYPE, ROWS, COLS> operator*( const Matrix<DATA_TYPE, ROWS, INTERNAL>& lhs,
136	const Matrix<DATA_TYPE, INTERNAL, COLS>& rhs )
137	{
138	Matrix<DATA_TYPE, ROWS, COLS> temporary;
139	return mult( temporary, lhs, rhs );
140	}
141
142	/** matrix subtraction (algebraic operation for matrix).
143	* @PRE: if lhs is m x n, and rhs is m x n, then result is m x n (mult func undefined otherwise)
144	* @POST: returns a m x n matrix
145	* @TODO: <B>enforce the sizes with templates...</b>
146	*/
147	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
148	inline Matrix<DATA_TYPE, ROWS, COLS>& sub( Matrix<DATA_TYPE, ROWS, COLS>& result,
149	const Matrix<DATA_TYPE, ROWS, COLS>& lhs,
150	const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
151	{
152	// p. 150 Numerical Analysis (second ed.)
153	// if A is m x n, and B is m x n, then AB is m x n
154	// (A - B)ij = (a)ij - (b)ij (where: 1 <= i <= m, 1 <= j <= n)
155	for (unsigned int i = 0; i < ROWS; ++i) // 1 <= i <= m
156	for (unsigned int j = 0; j < COLS; ++j) // 1 <= j <= n
157	result( i, j ) = lhs( i, j ) - rhs( i, j );
158
159	// track state
160	result.mState = combineMatrixStates( lhs.mState, rhs.mState );
161	return result;
162	}
163
164	/** matrix addition (algebraic operation for matrix).
165	* @PRE: if lhs is m x n, and rhs is m x n, then result is m x n (mult func undefined otherwise)
166	* @POST: returns a m x n matrix
167	* TODO: <B>enforce the sizes with templates...</b>
168	*/
169	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
170	inline Matrix<DATA_TYPE, ROWS, COLS>& add( Matrix<DATA_TYPE, ROWS, COLS>& result,
171	const Matrix<DATA_TYPE, ROWS, COLS>& lhs,
172	const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
173	{
174	// p. 150 Numerical Analysis (second ed.)
175	// if A is m x n, and B is m x n, then AB is m x n
176	// (A - B)ij = (a)ij + (b)ij (where: 1 <= i <= m, 1 <= j <= n)
177	for (unsigned int i = 0; i < ROWS; ++i) // 1 <= i <= m
178	for (unsigned int j = 0; j < COLS; ++j) // 1 <= j <= n
179	result( i, j ) = lhs( i, j ) + rhs( i, j );
180
181	// track state
182	result.mState = combineMatrixStates( lhs.mState, rhs.mState );
183	return result;
184	}
185
186	/** matrix postmultiply.
187	* @PRE: args must both be n x n (this function is undefined otherwise)
188	* @POST: result' = result * operand
189	*/
190	template <typename DATA_TYPE, unsigned SIZE>
191	inline Matrix<DATA_TYPE, SIZE, SIZE>& postMult( Matrix<DATA_TYPE, SIZE, SIZE>& result,
192	const Matrix<DATA_TYPE, SIZE, SIZE>& operand )
193	{
194	return mult( result, result, operand );
195	}
196
197	/** matrix preMultiply.
198	* @PRE: args must both be n x n (this function is undefined otherwise)
199	* @POST: result' = operand * result
200	*/
201	template <typename DATA_TYPE, unsigned SIZE>
202	inline Matrix<DATA_TYPE, SIZE, SIZE>& preMult( Matrix<DATA_TYPE, SIZE, SIZE>& result,
203	const Matrix<DATA_TYPE, SIZE, SIZE>& operand )
204	{
205	return mult( result, operand, result );
206	}
207
208	/** matrix postmult (operator*=).
209	* does a postmult on the matrix.
210	* @PRE: args must both be n x n sized (this function is undefined otherwise)
211	* @POST: result' = result * operand (where operand is applied first)
212	*/
213	template <typename DATA_TYPE, unsigned SIZE>
214	inline Matrix<DATA_TYPE, SIZE, SIZE>& operator*=( Matrix<DATA_TYPE, SIZE, SIZE>& result,
215	const Matrix<DATA_TYPE, SIZE, SIZE>& operand )
216	{
217	return postMult( result, operand );
218	}
219
220	/** matrix scalar mult.
221	* mult each elt in a matrix by a scalar value.
222	* @POST: result = mat * scalar
223	*/
224	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
225	inline Matrix<DATA_TYPE, ROWS, COLS>& mult( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, ROWS, COLS>& mat, const DATA_TYPE& scalar )
226	{
227	for (unsigned i = 0; i < ROWS * COLS; ++i)
228	result.mData[i] = mat.mData[i] * scalar;
229	result.mState = mat.mState;
230	return result;
231	}
232
233	/** matrix scalar mult.
234	* mult each elt in a matrix by a scalar value.
235	* @POST: result *= scalar
236	*/
237	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
238	inline Matrix<DATA_TYPE, ROWS, COLS>& mult( Matrix<DATA_TYPE, ROWS, COLS>& result, DATA_TYPE scalar )
239	{
240	for (unsigned i = 0; i < ROWS * COLS; ++i)
241	result.mData[i] *= scalar;
242	return result;
243	}
244
245	/** matrix scalar mult (operator*=).
246	* multiply matrix elements by a scalar
247	* @POST: result *= scalar
248	*/
249	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
250	inline Matrix<DATA_TYPE, ROWS, COLS>& operator*=( Matrix<DATA_TYPE, ROWS, COLS>& result, const DATA_TYPE& scalar )
251	{
252	return mult( result, scalar );
253	}
254
255	/** matrix transpose in place.
256	* @PRE: needs to be an N x N matrix
257	* @POST: flip along diagonal
258	*/
259	template <typename DATA_TYPE, unsigned SIZE>
260	Matrix<DATA_TYPE, SIZE, SIZE>& transpose( Matrix<DATA_TYPE, SIZE, SIZE>& result )
261	{
262	// p. 27 game programming gems #1
263	for (unsigned c = 0; c < SIZE; ++c)
264	for (unsigned r = c + 1; r < SIZE; ++r)
265	std::swap( result( r, c ), result( c, r ) );
266
267	return result;
268	}
269
270	/** matrix transpose from one type to another (i.e. 3x4 to 4x3)
271	* @PRE: source needs to be an M x N matrix, while dest needs to be N x M
272	* @POST: flip along diagonal
273	*/
274	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
275	Matrix<DATA_TYPE, ROWS, COLS>& transpose( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, COLS, ROWS>& source )
276	{
277	// in case result is == source... :(
278	Matrix<DATA_TYPE, COLS, ROWS> temp = source;
279
280	// p. 149 Numerical Analysis (second ed.)
281	for (unsigned i = 0; i < ROWS; ++i)
282	{
283	for (unsigned j = 0; j < COLS; ++j)
284	{
285	result( i, j ) = temp( j, i );
286	}
287	}
288	result.mState = temp.mState;
289	return result;
290	}
291
292	/** translational matrix inversion.
293	* Matrix inversion that acts on a translational matrix (matrix with only translation)
294	* Check for error with Matrix::isError().
295	* @pre: 4x3, 4x4 matrices only
296	* @post: result' = inv( result )
297	* @post: If inversion failed, then error bit is set within the Matrix.
298	*/
299	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
300	inline Matrix<DATA_TYPE, ROWS, COLS>& invertTrans( Matrix<DATA_TYPE, ROWS, COLS>& result,
301	const Matrix<DATA_TYPE, ROWS, COLS>& src )
302	{
303	gmtlASSERT( ROWS == COLS \|\| COLS == ROWS+1 && "invertTrans supports NxN or Nx(N-1) matrices only" );
304
305	if (&result != &src)
306	result = src; // could optimise this a little more (skip the trans copy), favor simplicity for now...
307	for (unsigned x = 0; x < (ROWS-1+(COLS-ROWS)); ++x)
308	{
309	result[x][3] = -result[x][3];
310	}
311	return result;
312	}
313
314	/** orthogonal matrix inversion.
315	* Matrix inversion that acts on a affine matrix (matrix with only trans, rot, uniform scale)
316	* Check for error with Matrix::isError().
317	* @pre: any size matrix
318	* @post: result' = inv( result )
319	* @post: If inversion failed, then error bit is set within the Matrix.
320	*/
321	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
322	inline Matrix<DATA_TYPE, ROWS, COLS>& invertOrthogonal( Matrix<DATA_TYPE, ROWS, COLS>& result,
323	const Matrix<DATA_TYPE, ROWS, COLS>& src )
324	{
325	// in case result is == source... :(
326	Matrix<DATA_TYPE, ROWS, COLS> temp = src;
327
328	// if 3x4, 2x3, etc... can't transpose the last column
329	const unsigned int size = Math::Min( ROWS, COLS );
330
331	// p. 149 Numerical Analysis (second ed.)
332	for (unsigned i = 0; i < size; ++i)
333	{
334	for (unsigned j = 0; j < size; ++j)
335	{
336	result( i, j ) = temp( j, i );
337	}
338	}
339	result.mState = temp.mState;
340	return result;
341	}
342
343	/** affine matrix inversion.
344	* Matrix inversion that acts on a 4x3 affine matrix (matrix with only trans, rot, uniform scale)
345	* Check for error with Matrix::isError().
346	* @pre: 3x3, 4x3, 4x4 matrices only
347	* @POST: result' = inv( result )
348	* @POST: If inversion failed, then error bit is set within the Matrix.
349	*/
350	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
351	inline Matrix<DATA_TYPE, ROWS, COLS>& invertAffine( Matrix<DATA_TYPE, ROWS, COLS>& result,
352	const Matrix<DATA_TYPE, ROWS, COLS>& source )
353	{
354	static const float eps = 0.00000001f;
355
356	// in case &result is == &source... :(
357	Matrix<DATA_TYPE, ROWS, COLS> src = source;
358
359	// The rotational part of the matrix is simply the transpose of the
360	// original matrix.
361	for (int x = 0; x < 3; ++x)
362	for (int y = 0; y < 3; ++y)
363	{
364	result[x][y] = src[y][x];
365	}
366
367	// do non-uniform scale inversion
368	if (src.mState & Matrix<DATA_TYPE, ROWS, COLS>::NON_UNISCALE)
369	{
370	DATA_TYPE l0 = gmtl::lengthSquared( gmtl::Vec<DATA_TYPE, 3>( result[0][0], result[0][1], result[0][2] ) );
371	DATA_TYPE l1 = gmtl::lengthSquared( gmtl::Vec<DATA_TYPE, 3>( result[1][0], result[1][1], result[1][2] ) );
372	DATA_TYPE l2 = gmtl::lengthSquared( gmtl::Vec<DATA_TYPE, 3>( result[2][0], result[2][1], result[2][2] ) );
373	if (gmtl::Math::abs( l0 ) > eps) l0 = 1.0f / l0;
374	if (gmtl::Math::abs( l1 ) > eps) l1 = 1.0f / l1;
375	if (gmtl::Math::abs( l2 ) > eps) l2 = 1.0f / l2;
376	// apply the inverse scale to the 3x3
377	// for each axis: normalize it (1/length), and then mult by inverse scale (1/length)
378	result[0][0] *= l0;
379	result[0][1] *= l0;
380	result[0][2] *= l0;
381	result[1][0] *= l1;
382	result[1][1] *= l1;
383	result[1][2] *= l1;
384	result[2][0] *= l2;
385	result[2][1] *= l2;
386	result[2][2] *= l2;
387	}
388
389	// handle matrices with translation
390	if (COLS == 4)
391	{
392	// The right column vector of the matrix should always be [ 0 0 0 s ]
393	// this represents some shear values
394	result[3][0] = result[3][1] = result[3][2] = 0;
395
396	// The translation components of the original matrix.
397	const DATA_TYPE& tx = src[0][3];
398	const DATA_TYPE& ty = src[1][3];
399	const DATA_TYPE& tz = src[2][3];
400
401
402	// Rresult = -(Tm * Rm) to get the translation part of the inverse
403	if (ROWS == 4)
404	{
405	// invert scale.
406	const DATA_TYPE tw = (gmtl::Math::abs( src[3][3] ) > eps) ? 1.0f / src[3][3] : 0.0f;
407
408	// handle uniform scale in Nx4 matrices
409	result[0][3] = -( result[0][0] * tx + result[0][1] * ty + result[0][2] * tz ) * tw;
410	result[1][3] = -( result[1][0] * tx + result[1][1] * ty + result[1][2] * tz ) * tw;
411	result[2][3] = -( result[2][0] * tx + result[2][1] * ty + result[2][2] * tz ) * tw;
412	result[3][3] = tw;
413	}
414	else if (ROWS == 3)
415	{
416	result[0][3] = -( result[0][0] * tx + result[0][1] * ty + result[0][2] * tz );
417	result[1][3] = -( result[1][0] * tx + result[1][1] * ty + result[1][2] * tz );
418	result[2][3] = -( result[2][0] * tx + result[2][1] * ty + result[2][2] * tz );
419	}
420	}
421
422
423
424	result.mState = src.mState;
425
426	return result;
427	}
428
429	/** Full matrix inversion using Gauss-Jordan elimination.
430	* Check for error with Matrix::isError().
431	* @POST: result' = inv( result )
432	* @POST: If inversion failed, then error bit is set within the Matrix.
433	*/
434	template <typename DATA_TYPE, unsigned SIZE>
435	inline Matrix<DATA_TYPE, SIZE, SIZE>& invertFull_GJ( Matrix<DATA_TYPE, SIZE, SIZE>& result, const Matrix<DATA_TYPE, SIZE, SIZE>& src )
436	{
437	//gmtlASSERT( ROWS == COLS && "invertFull only works with nxn matrices" );
438
439	const DATA_TYPE pivot_eps(1e-20); // Epsilon for the pivot value test (delta with test against zero)
440
441	// Computer inverse of matrix using a Gaussian-Jordan elimination.
442	// Uses max pivot at each point
443	// See: "Essential Mathmatics for Games" for description
444
445	// Do this invert in place
446	result = src;
447	unsigned swapped[SIZE]; // Track swaps. swapped[3] = 2 means that row 3 was swapped with row 2
448
449	unsigned pivot;
450
451	// --- Gaussian elimination step --- //
452	// For each column and row
453	for(pivot=0; pivot<SIZE;++pivot)
454	{
455	unsigned pivot_row(pivot);
456	DATA_TYPE pivot_value(gmtl::Math::abs(result(pivot_row, pivot))); // Initialize to beginning of current row
457
458	// find pivot row - (max pivot element)
459	for(unsigned pr=pivot+1;pr<SIZE;++pr)
460	{
461	const DATA_TYPE cur_val(gmtl::Math::abs(result(pr,pivot))); // get value at current point
462	if (cur_val > pivot_value)
463	{
464	pivot_row = pr;
465	pivot_value = cur_val;
466	}
467	}
468
469	if(gmtl::Math::isEqual(DATA_TYPE(0),pivot_value,pivot_eps))
470	{
471	std::cerr << "* pivot = " << pivot_value << " in mat_inv. *\n";
472	result.setError();
473	return result;
474	}
475
476	// Check for swap of pivot rows
477	swapped[pivot] = pivot_row;
478	if(pivot_row != pivot)
479	{
480	for(unsigned c=0;c<SIZE;++c)
481	{ std::swap(result(pivot,c), result(pivot_row,c)); }
482	}
483	// ASSERT: row to use in now in "row" (check that row starts with max pivot value found)
484	gmtlASSERT(gmtl::Math::isEqual(pivot_value,gmtl::Math::abs(result(pivot,pivot)),DATA_TYPE(0.00001)));
485
486	// Compute pivot factor
487	const DATA_TYPE mult_factor(1.0f/pivot_value);
488
489	// Set pivot row values
490	for(unsigned c=0;c<SIZE;++c)
491	{ result(pivot,c) *= mult_factor; }
492	result(pivot,pivot) = mult_factor; // Copy the 1/pivot since we are inverting in place
493
494	// Clear pivot column in other rows (since we are in place)
495	// - Subtract current row times result(r,col) so that column element becomes 0
496	for(unsigned row=0;row<SIZE;++row)
497	{
498	if(row==pivot) // Don't subtract from our row
499	{ continue; }
500
501	const DATA_TYPE sub_mult_factor(result(row,pivot));
502
503	// Clear the pivot column's element (for invers in place)
504	// ends up being set to -sub_mult_factor*pivotInverse
505	result(row,pivot) = 0;
506
507	// subtract the pivot row from this row
508	for(unsigned col=0;col<SIZE;++col)
509	{ result(row,col) -= (sub_mult_factor*result(pivot,col)); }
510	}
511	} // end: gaussian substitution
512
513
514	// Now undo the swaps in column direction in reverse order
515	unsigned p(SIZE);
516	do
517	{
518	--p;
519	gmtlASSERT(p<SIZE);
520
521	// If row was swapped
522	if(swapped[p] != p)
523	{
524	// Swap the column with same index
525	for(unsigned r=0; r<SIZE; ++r)
526	{ std::swap(result(r, p), result(r, swapped[p])); }
527	}
528	}
529	while(p>0);
530
531	return result;
532	}
533
534
535	/** full matrix inversion.
536	* Check for error with Matrix::isError().
537	* @POST: result' = inv( result )
538	* @POST: If inversion failed, then error bit is set within the Matrix.
539	*/
540	template <typename DATA_TYPE, unsigned SIZE>
541	inline Matrix<DATA_TYPE, SIZE, SIZE>& invertFull_orig( Matrix<DATA_TYPE, SIZE, SIZE>& result, const Matrix<DATA_TYPE, SIZE, SIZE>& src )
542	{
543	/---------------------------------------------------------------------------
544	\| mat_inv: Compute the inverse of a n x n matrix, using the maximum pivot \|
545	\| strategy. n <= MAX1. \|
546	---------------------------------------------------------------------------
547
548	Parameters:
549	a a n x n square matrix
550	b inverse of input a.
551	n dimenstion of matrix a.
552	*/
553
554	const DATA_TYPE* a = src.getData();
555	DATA_TYPE* b = result.mData;
556
557	int n(SIZE);
558	int i, j, k;
559	int r[SIZE], c[SIZE], row[SIZE], col[SIZE];
560	DATA_TYPE m[SIZE][SIZE*2], pivot, max_m, tmp_m, fac;
561
562	/* Initialization */
563	for ( i = 0; i < n; i ++ )
564	{
565	r[ i] = c[ i] = 0;
566	row[ i] = col[ i] = 0;
567	}
568
569	/* Set working matrix */
570	for ( i = 0; i < n; i++ )
571	{
572	for ( j = 0; j < n; j++ )
573	{
574	m[ i][ j] = a[ i * n + j];
575	m[ i][ j + n] = ( i == j ) ? (DATA_TYPE)1.0 : (DATA_TYPE)0.0 ;
576	}
577	}
578
579	/* Begin of loop */
580	for ( k = 0; k < n; k++ )
581	{
582	/* Choosing the pivot */
583	for ( i = 0, max_m = 0; i < n; i++ )
584	{
585	if ( row[ i] )
586	continue;
587	for ( j = 0; j < n; j++ )
588	{
589	if ( col[ j] )
590	continue;
591	tmp_m = gmtl::Math::abs( m[ i][ j]);
592	if ( tmp_m > max_m)
593	{
594	max_m = tmp_m;
595	r[ k] = i;
596	c[ k] = j;
597	}
598	}
599	}
600	row[ r[k] ] = col[ c[k] ] = 1;
601	pivot = m[ r[ k] ][ c[ k] ];
602
603
604	if ( gmtl::Math::abs( pivot) <= 1e-20)
605	{
606	std::cerr << "* pivot = " << pivot << " in mat_inv. *\n";
607	result.setError();
608	return result;
609	}
610
611	/* Normalization */
612	for ( j = 0; j < 2*n; j++ )
613	{
614	if ( j == c[ k] )
615	m[ r[ k]][ j] = (DATA_TYPE)1.0;
616	else
617	m[ r[ k]][ j] /= pivot;
618	}
619
620	/* Reduction */
621	for ( i = 0; i < n; i++ )
622	{
623	if ( i == r[ k] )
624	continue;
625
626	for ( j=0, fac = m[ i][ c[k]]; j < 2*n; j++ )
627	{
628	if ( j == c[ k] )
629	m[ i][ j] = (DATA_TYPE)0.0;
630	else
631	m[ i][ j] -= fac * m[ r[k]][ j];
632	}
633	}
634	}
635
636	/* Assign inverse to a matrix */
637	for ( i = 0; i < n; i++ )
638	for ( j = 0; j < n; j++ )
639	row[ i] = ( c[ j] == i ) ? r[ j] : row[ i];
640
641	for ( i = 0; i < n; i++ )
642	for ( j = 0; j < n; j++ )
643	b[ i * n + j] = m[ row[ i]][ j + n];
644
645	// It worked
646	result.mState = src.mState;
647	return result;
648	}
649
650
651	/** Invert method.
652	* Calls invertFull_orig to do the work.
653	*/
654	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
655	inline Matrix<DATA_TYPE, ROWS, COLS>& invertFull( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, ROWS, COLS>& src )
656	{
657	return invertFull_orig(result,src);
658	}
659
660	/** smart matrix inversion.
661	* Does matrix inversion by intelligently selecting what type of inversion to use depending
662	* on the types of operations your Matrix has been through.
663	*
664	* 5 types of inversion: FULL, AFFINE, ORTHONORMAL, ORTHOGONAL, IDENTITY.
665	*
666	* Check for error with Matrix::isError().
667	* @POST: result' = inv( result )
668	* @POST: If inversion failed, then error bit is set within the Matrix.
669	*/
670	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
671	inline Matrix<DATA_TYPE, ROWS, COLS>& invert( Matrix<DATA_TYPE, ROWS, COLS>& result, const Matrix<DATA_TYPE, ROWS, COLS>& src )
672	{
673	if (src.mState == Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY )
674	return result = src;
675	else if (src.mState == Matrix<DATA_TYPE, ROWS, COLS>::TRANS)
676	return invertTrans( result, src );
677	else if (src.mState == Matrix<DATA_TYPE, ROWS, COLS>::ORTHOGONAL)
678	return invertOrthogonal( result, src );
679	else if (src.mState == Matrix<DATA_TYPE, ROWS, COLS>::AFFINE \|\|
680	src.mState == (Matrix<DATA_TYPE, ROWS, COLS>::AFFINE \| Matrix<DATA_TYPE, ROWS, COLS>::NON_UNISCALE))
681	return invertAffine( result, src );
682	else
683	return invertFull_orig( result, src );
684	}
685
686	/** smart matrix inversion (in place)
687	* Does matrix inversion by intelligently selecting what type of inversion to use depending
688	* on the types of operations your Matrix has been through.
689	*
690	* 5 types of inversion: FULL, AFFINE, ORTHONORMAL, ORTHOGONAL, IDENTITY.
691	*
692	* Check for error with Matrix::isError().
693	* @POST: result' = inv( result )
694	* @POST: If inversion failed, then error bit is set within the Matrix.
695	*/
696	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
697	inline Matrix<DATA_TYPE, ROWS, COLS>& invert( Matrix<DATA_TYPE, ROWS, COLS>& result )
698	{
699	return invert( result, result );
700	}
701
702	/** @} */
703
704	/** @ingroup Compare
705	* @name Matrix Comparitors
706	* @{
707	*/
708
709	/** Tests 2 matrices for equality
710	* @param lhs The first matrix
711	* @param rhs The second matrix
712	* @pre Both matrices must be of the same size.
713	* @return true if the matrices have the same element values; false otherwise
714	*/
715	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
716	inline bool operator==( const Matrix<DATA_TYPE, ROWS, COLS>& lhs, const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
717	{
718	for (unsigned int i = 0; i < ROWS*COLS; ++i)
719	{
720	if (lhs.mData[i] != rhs.mData[i])
721	{
722	return false;
723	}
724	}
725
726	return true;
727
728	/* Would like this
729	return( lhs[0] == rhs[0] &&
730	lhs[1] == rhs[1] &&
731	lhs[2] == rhs[2] );
732	*/
733	}
734
735	/** Tests 2 matrices for inequality
736	* @param lhs The first matrix
737	* @param rhs The second matrix
738	* @pre Both matrices must be of the same size.
739	* @return false if the matrices differ on any element value; true otherwise
740	*/
741	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
742	inline bool operator!=( const Matrix<DATA_TYPE, ROWS, COLS>& lhs, const Matrix<DATA_TYPE, ROWS, COLS>& rhs )
743	{
744	return bool( !(lhs == rhs) );
745	}
746
747	/** Tests 2 matrices for equality within a tolerance
748	* @param lhs The first matrix
749	* @param rhs The second matrix
750	* @param eps The tolerance value
751	* @pre Both matrices must be of the same size.
752	* @return true if the matrices' elements are within the tolerance value of each other; false otherwise
753	*/
754	template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
755	inline bool isEqual( const Matrix<DATA_TYPE, ROWS, COLS>& lhs, const Matrix<DATA_TYPE, ROWS, COLS>& rhs, const DATA_TYPE eps = 0 )
756	{
757	gmtlASSERT( eps >= (DATA_TYPE)0 );
758
759	for (unsigned int i = 0; i < ROWS*COLS; ++i)
760	{
761	if (!Math::isEqual( lhs.mData[i], rhs.mData[i], eps ))
762	return false;
763	}
764	return true;
765	}
766	/** @} */
767
768	} // end of namespace gmtl
769
770	#endif

Note: See TracBrowser for help on using the repository browser.

Context Navigation

source: projectionDesigner/trunk/projdesigner/include/gmtl/MatrixOps.h @ 373

Download in other formats: