/lib/ode/ode_source/OPCODE/Ice/IceMatrix3x3.h

http://narutortsproject.googlecode.com/ · C++ Header · 496 lines · 333 code · 67 blank · 96 comment · 14 complexity · d02cae97add75928ae7740b575351fcc MD5 · raw file 

    

  1. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    2. 

  2. /**

    3. 

  3.  *	Contains code for 3x3 matrices.

    4. 

  4.  *	\file		IceMatrix3x3.h

    5. 

  5.  *	\author		Pierre Terdiman

    6. 

  6.  *	\date		April, 4, 2000

    7. 

  7.  */

    8. 

  8. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    9. 

  9. 

    10. 

  10. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    11. 

  11. // Include Guard

    12. 

  12. #ifndef __ICEMATRIX3X3_H__

    13. 

  13. #define __ICEMATRIX3X3_H__

    14. 

  14. 

    15. 

  15. 	// Forward declarations

    16. 

  16. 	class Quat;

    17. 

  17. 

    18. 

  18. 	#define	MATRIX3X3_EPSILON		(1.0e-7f)

    19. 

  19. 

    20. 

  20. 	class ICEMATHS_API Matrix3x3

    21. 

  21. 	{

    22. 

  22. 		public:

    23. 

  23. 		//! Empty constructor

    24. 

  24. 		inline_					Matrix3x3()									{}

    25. 

  25. 		//! Constructor from 9 values

    26. 

  26. 		inline_					Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)

    27. 

  27. 								{

    28. 

  28. 									m[0][0] = m00;	m[0][1] = m01;	m[0][2] = m02;

    29. 

  29. 									m[1][0] = m10;	m[1][1] = m11;	m[1][2] = m12;

    30. 

  30. 									m[2][0] = m20;	m[2][1] = m21;	m[2][2] = m22;

    31. 

  31. 								}

    32. 

  32. 		//! Copy constructor

    33. 

  33. 		inline_					Matrix3x3(const Matrix3x3& mat)				{ CopyMemory(m, &mat.m, 9*sizeof(float));	}

    34. 

  34. 		//! Destructor

    35. 

  35. 		inline_					~Matrix3x3()								{}

    36. 

  36. 

    37. 

  37. 		//! Assign values

    38. 

  38. 		inline_	void			Set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)

    39. 

  39. 								{

    40. 

  40. 									m[0][0] = m00;	m[0][1] = m01;	m[0][2] = m02;

    41. 

  41. 									m[1][0] = m10;	m[1][1] = m11;	m[1][2] = m12;

    42. 

  42. 									m[2][0] = m20;	m[2][1] = m21;	m[2][2] = m22;

    43. 

  43. 								}

    44. 

  44. 

    45. 

  45. 		//! Sets the scale from a Point. The point is put on the diagonal.

    46. 

  46. 		inline_	void			SetScale(const Point& p)					{ m[0][0] = p.x;	m[1][1] = p.y;	m[2][2] = p.z;	}

    47. 

  47. 

    48. 

  48. 		//! Sets the scale from floats. Values are put on the diagonal.

    49. 

  49. 		inline_	void			SetScale(float sx, float sy, float sz)		{ m[0][0] = sx;		m[1][1] = sy;	m[2][2] = sz;	}

    50. 

  50. 

    51. 

  51. 		//! Scales from a Point. Each row is multiplied by a component.

    52. 

  52. 		inline_	void			Scale(const Point& p)

    53. 

  53. 								{

    54. 

  54. 									m[0][0] *= p.x;	m[0][1] *= p.x;	m[0][2] *= p.x;

    55. 

  55. 									m[1][0] *= p.y;	m[1][1] *= p.y;	m[1][2] *= p.y;

    56. 

  56. 									m[2][0] *= p.z;	m[2][1] *= p.z;	m[2][2] *= p.z;

    57. 

  57. 								}

    58. 

  58. 

    59. 

  59. 		//! Scales from floats. Each row is multiplied by a value.

    60. 

  60. 		inline_	void			Scale(float sx, float sy, float sz)

    61. 

  61. 								{

    62. 

  62. 									m[0][0] *= sx;	m[0][1] *= sx;	m[0][2] *= sx;

    63. 

  63. 									m[1][0] *= sy;	m[1][1] *= sy;	m[1][2] *= sy;

    64. 

  64. 									m[2][0] *= sz;	m[2][1] *= sz;	m[2][2] *= sz;

    65. 

  65. 								}

    66. 

  66. 

    67. 

  67. 		//! Copy from a Matrix3x3

    68. 

  68. 		inline_	void			Copy(const Matrix3x3& source)				{ CopyMemory(m, source.m, 9*sizeof(float));			}

    69. 

  69. 

    70. 

  70. 		// Row-column access

    71. 

  71. 		//! Returns a row.

    72. 

  72. 		inline_	void			GetRow(const udword r, Point& p)	const	{ p.x = m[r][0];	p.y = m[r][1];	p.z = m[r][2];	}

    73. 

  73. 		//! Returns a row.

    74. 

  74. 		inline_	const Point&	GetRow(const udword r)				const	{ return *(const Point*)&m[r][0];	}

    75. 

  75. 		//! Returns a row.

    76. 

  76. 		inline_	Point&			GetRow(const udword r)						{ return *(Point*)&m[r][0];			}

    77. 

  77. 		//! Sets a row.

    78. 

  78. 		inline_	void			SetRow(const udword r, const Point& p)		{ m[r][0] = p.x;	m[r][1] = p.y;	m[r][2] = p.z;	}

    79. 

  79. 		//! Returns a column.

    80. 

  80. 		inline_	void			GetCol(const udword c, Point& p)	const	{ p.x = m[0][c];	p.y = m[1][c];	p.z = m[2][c];	}

    81. 

  81. 		//! Sets a column.

    82. 

  82. 		inline_	void			SetCol(const udword c, const Point& p)		{ m[0][c] = p.x;	m[1][c] = p.y;	m[2][c] = p.z;	}

    83. 

  83. 

    84. 

  84. 		//! Computes the trace. The trace is the sum of the 3 diagonal components.

    85. 

  85. 		inline_	float			Trace()					const				{ return m[0][0] + m[1][1] + m[2][2];				}

    86. 

  86. 		//! Clears the matrix.

    87. 

  87. 		inline_	void			Zero()										{ ZeroMemory(&m, sizeof(m));						}

    88. 

  88. 		//! Sets the identity matrix.

    89. 

  89. 		inline_	void			Identity()									{ Zero(); m[0][0] = m[1][1] = m[2][2] = 1.0f; 		}

    90. 

  90. 		//! Checks for identity

    91. 

  91. 		inline_	bool			IsIdentity()			const

    92. 

  92. 								{

    93. 

  93. 									if(IR(m[0][0])!=IEEE_1_0)	return false;

    94. 

  94. 									if(IR(m[0][1])!=0)			return false;

    95. 

  95. 									if(IR(m[0][2])!=0)			return false;

    96. 

  96. 

    97. 

  97. 									if(IR(m[1][0])!=0)			return false;

    98. 

  98. 									if(IR(m[1][1])!=IEEE_1_0)	return false;

    99. 

  99. 									if(IR(m[1][2])!=0)			return false;

    100. 

  100. 

    101. 

  101. 									if(IR(m[2][0])!=0)			return false;

    102. 

  102. 									if(IR(m[2][1])!=0)			return false;

    103. 

  103. 									if(IR(m[2][2])!=IEEE_1_0)	return false;

    104. 

  104. 

    105. 

  105. 									return true;

    106. 

  106. 								}

    107. 

  107. 

    108. 

  108. 		//! Checks matrix validity

    109. 

  109. 		inline_	BOOL			IsValid()				const

    110. 

  110. 								{

    111. 

  111. 									for(udword j=0;j<3;j++)

    112. 

  112. 									{

    113. 

  113. 										for(udword i=0;i<3;i++)

    114. 

  114. 										{

    115. 

  115. 											if(!IsValidFloat(m[j][i]))	return FALSE;

    116. 

  116. 										}

    117. 

  117. 									}

    118. 

  118. 									return TRUE;

    119. 

  119. 								}

    120. 

  120. 

    121. 

  121. 		//! Makes a skew-symmetric matrix (a.k.a. Star(*) Matrix)

    122. 

  122. 		//!	[  0.0  -a.z   a.y ]

    123. 

  123. 		//!	[  a.z   0.0  -a.x ]

    124. 

  124. 		//!	[ -a.y   a.x   0.0 ]

    125. 

  125. 		//! This is also called a "cross matrix" since for any vectors A and B,

    126. 

  126. 		//! A^B = Skew(A) * B = - B * Skew(A);

    127. 

  127. 		inline_	void			SkewSymmetric(const Point& a)

    128. 

  128. 								{

    129. 

  129. 									m[0][0] = 0.0f;

    130. 

  130. 									m[0][1] = -a.z;

    131. 

  131. 									m[0][2] = a.y;

    132. 

  132. 

    133. 

  133. 									m[1][0] = a.z;

    134. 

  134. 									m[1][1] = 0.0f;

    135. 

  135. 									m[1][2] = -a.x;

    136. 

  136. 

    137. 

  137. 									m[2][0] = -a.y;

    138. 

  138. 									m[2][1] = a.x;

    139. 

  139. 									m[2][2] = 0.0f;

    140. 

  140. 								}

    141. 

  141. 

    142. 

  142. 		//! Negates the matrix

    143. 

  143. 		inline_	void			Neg()

    144. 

  144. 								{

    145. 

  145. 									m[0][0] = -m[0][0];	m[0][1] = -m[0][1];	m[0][2] = -m[0][2];

    146. 

  146. 									m[1][0] = -m[1][0];	m[1][1] = -m[1][1];	m[1][2] = -m[1][2];

    147. 

  147. 									m[2][0] = -m[2][0];	m[2][1] = -m[2][1];	m[2][2] = -m[2][2];

    148. 

  148. 								}

    149. 

  149. 

    150. 

  150. 		//! Neg from another matrix

    151. 

  151. 		inline_	void			Neg(const Matrix3x3& mat)

    152. 

  152. 								{

    153. 

  153. 									m[0][0] = -mat.m[0][0];	m[0][1] = -mat.m[0][1];	m[0][2] = -mat.m[0][2];

    154. 

  154. 									m[1][0] = -mat.m[1][0];	m[1][1] = -mat.m[1][1];	m[1][2] = -mat.m[1][2];

    155. 

  155. 									m[2][0] = -mat.m[2][0];	m[2][1] = -mat.m[2][1];	m[2][2] = -mat.m[2][2];

    156. 

  156. 								}

    157. 

  157. 

    158. 

  158. 		//! Add another matrix

    159. 

  159. 		inline_	void			Add(const Matrix3x3& mat)

    160. 

  160. 								{

    161. 

  161. 									m[0][0] += mat.m[0][0];	m[0][1] += mat.m[0][1];	m[0][2] += mat.m[0][2];

    162. 

  162. 									m[1][0] += mat.m[1][0];	m[1][1] += mat.m[1][1];	m[1][2] += mat.m[1][2];

    163. 

  163. 									m[2][0] += mat.m[2][0];	m[2][1] += mat.m[2][1];	m[2][2] += mat.m[2][2];

    164. 

  164. 								}

    165. 

  165. 

    166. 

  166. 		//! Sub another matrix

    167. 

  167. 		inline_	void			Sub(const Matrix3x3& mat)

    168. 

  168. 								{

    169. 

  169. 									m[0][0] -= mat.m[0][0];	m[0][1]	-= mat.m[0][1];	m[0][2] -= mat.m[0][2];

    170. 

  170. 									m[1][0] -= mat.m[1][0];	m[1][1] -= mat.m[1][1];	m[1][2] -= mat.m[1][2];

    171. 

  171. 									m[2][0] -= mat.m[2][0];	m[2][1] -= mat.m[2][1];	m[2][2] -= mat.m[2][2];

    172. 

  172. 								}

    173. 

  173. 		//! Mac

    174. 

  174. 		inline_	void			Mac(const Matrix3x3& a, const Matrix3x3& b, float s)

    175. 

  175. 								{

    176. 

  176. 									m[0][0] = a.m[0][0] + b.m[0][0] * s;

    177. 

  177. 									m[0][1] = a.m[0][1] + b.m[0][1] * s;

    178. 

  178. 									m[0][2] = a.m[0][2] + b.m[0][2] * s;

    179. 

  179. 

    180. 

  180. 									m[1][0] = a.m[1][0] + b.m[1][0] * s;

    181. 

  181. 									m[1][1] = a.m[1][1] + b.m[1][1] * s;

    182. 

  182. 									m[1][2] = a.m[1][2] + b.m[1][2] * s;

    183. 

  183. 

    184. 

  184. 									m[2][0] = a.m[2][0] + b.m[2][0] * s;

    185. 

  185. 									m[2][1] = a.m[2][1] + b.m[2][1] * s;

    186. 

  186. 									m[2][2] = a.m[2][2] + b.m[2][2] * s;

    187. 

  187. 								}

    188. 

  188. 		//! Mac

    189. 

  189. 		inline_	void			Mac(const Matrix3x3& a, float s)

    190. 

  190. 								{

    191. 

  191. 									m[0][0] += a.m[0][0] * s;	m[0][1] += a.m[0][1] * s;	m[0][2] += a.m[0][2] * s;

    192. 

  192. 									m[1][0] += a.m[1][0] * s;	m[1][1] += a.m[1][1] * s;	m[1][2] += a.m[1][2] * s;

    193. 

  193. 									m[2][0] += a.m[2][0] * s;	m[2][1] += a.m[2][1] * s;	m[2][2] += a.m[2][2] * s;

    194. 

  194. 								}

    195. 

  195. 

    196. 

  196. 		//! this = A * s

    197. 

  197. 		inline_	void			Mult(const Matrix3x3& a, float s)

    198. 

  198. 								{

    199. 

  199. 									m[0][0] = a.m[0][0] * s;	m[0][1] = a.m[0][1] * s;	m[0][2] = a.m[0][2] * s;

    200. 

  200. 									m[1][0] = a.m[1][0] * s;	m[1][1] = a.m[1][1] * s;	m[1][2] = a.m[1][2] * s;

    201. 

  201. 									m[2][0] = a.m[2][0] * s;	m[2][1] = a.m[2][1] * s;	m[2][2] = a.m[2][2] * s;

    202. 

  202. 								}

    203. 

  203. 

    204. 

  204. 		inline_	void			Add(const Matrix3x3& a, const Matrix3x3& b)

    205. 

  205. 								{

    206. 

  206. 									m[0][0] = a.m[0][0] + b.m[0][0];	m[0][1] = a.m[0][1] + b.m[0][1];	m[0][2] = a.m[0][2] + b.m[0][2];

    207. 

  207. 									m[1][0] = a.m[1][0] + b.m[1][0];	m[1][1] = a.m[1][1] + b.m[1][1];	m[1][2] = a.m[1][2] + b.m[1][2];

    208. 

  208. 									m[2][0] = a.m[2][0] + b.m[2][0];	m[2][1] = a.m[2][1] + b.m[2][1];	m[2][2] = a.m[2][2] + b.m[2][2];

    209. 

  209. 								}

    210. 

  210. 

    211. 

  211. 		inline_	void			Sub(const Matrix3x3& a, const Matrix3x3& b)

    212. 

  212. 								{

    213. 

  213. 									m[0][0] = a.m[0][0] - b.m[0][0];	m[0][1] = a.m[0][1] - b.m[0][1];	m[0][2] = a.m[0][2] - b.m[0][2];

    214. 

  214. 									m[1][0] = a.m[1][0] - b.m[1][0];	m[1][1] = a.m[1][1] - b.m[1][1];	m[1][2] = a.m[1][2] - b.m[1][2];

    215. 

  215. 									m[2][0] = a.m[2][0] - b.m[2][0];	m[2][1] = a.m[2][1] - b.m[2][1];	m[2][2] = a.m[2][2] - b.m[2][2];

    216. 

  216. 								}

    217. 

  217. 

    218. 

  218. 		//! this = a * b

    219. 

  219. 		inline_	void			Mult(const Matrix3x3& a, const Matrix3x3& b)

    220. 

  220. 								{

    221. 

  221. 									m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0];

    222. 

  222. 									m[0][1] = a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1];

    223. 

  223. 									m[0][2] = a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2];

    224. 

  224. 									m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0];

    225. 

  225. 									m[1][1] = a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1];

    226. 

  226. 									m[1][2] = a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2];

    227. 

  227. 									m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0];

    228. 

  228. 									m[2][1] = a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1];

    229. 

  229. 									m[2][2] = a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2];

    230. 

  230. 								}

    231. 

  231. 

    232. 

  232. 		//! this = transpose(a) * b

    233. 

  233. 		inline_	void			MultAtB(const Matrix3x3& a, const Matrix3x3& b)

    234. 

  234. 								{

    235. 

  235. 									m[0][0] = a.m[0][0] * b.m[0][0] + a.m[1][0] * b.m[1][0] + a.m[2][0] * b.m[2][0];

    236. 

  236. 									m[0][1] = a.m[0][0] * b.m[0][1] + a.m[1][0] * b.m[1][1] + a.m[2][0] * b.m[2][1];

    237. 

  237. 									m[0][2] = a.m[0][0] * b.m[0][2] + a.m[1][0] * b.m[1][2] + a.m[2][0] * b.m[2][2];

    238. 

  238. 									m[1][0] = a.m[0][1] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[2][1] * b.m[2][0];

    239. 

  239. 									m[1][1] = a.m[0][1] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[2][1] * b.m[2][1];

    240. 

  240. 									m[1][2] = a.m[0][1] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[2][1] * b.m[2][2];

    241. 

  241. 									m[2][0] = a.m[0][2] * b.m[0][0] + a.m[1][2] * b.m[1][0] + a.m[2][2] * b.m[2][0];

    242. 

  242. 									m[2][1] = a.m[0][2] * b.m[0][1] + a.m[1][2] * b.m[1][1] + a.m[2][2] * b.m[2][1];

    243. 

  243. 									m[2][2] = a.m[0][2] * b.m[0][2] + a.m[1][2] * b.m[1][2] + a.m[2][2] * b.m[2][2];

    244. 

  244. 								}

    245. 

  245. 

    246. 

  246. 		//! this = a * transpose(b)

    247. 

  247. 		inline_	void			MultABt(const Matrix3x3& a, const Matrix3x3& b)

    248. 

  248. 								{

    249. 

  249. 									m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[0][1] + a.m[0][2] * b.m[0][2];

    250. 

  250. 									m[0][1] = a.m[0][0] * b.m[1][0] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[1][2];

    251. 

  251. 									m[0][2] = a.m[0][0] * b.m[2][0] + a.m[0][1] * b.m[2][1] + a.m[0][2] * b.m[2][2];

    252. 

  252. 									m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[0][1] + a.m[1][2] * b.m[0][2];

    253. 

  253. 									m[1][1] = a.m[1][0] * b.m[1][0] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[1][2];

    254. 

  254. 									m[1][2] = a.m[1][0] * b.m[2][0] + a.m[1][1] * b.m[2][1] + a.m[1][2] * b.m[2][2];

    255. 

  255. 									m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[0][1] + a.m[2][2] * b.m[0][2];

    256. 

  256. 									m[2][1] = a.m[2][0] * b.m[1][0] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[1][2];

    257. 

  257. 									m[2][2] = a.m[2][0] * b.m[2][0] + a.m[2][1] * b.m[2][1] + a.m[2][2] * b.m[2][2];

    258. 

  258. 								}

    259. 

  259. 

    260. 

  260. 		//! Makes a rotation matrix mapping vector "from" to vector "to".

    261. 

  261. 				Matrix3x3&		FromTo(const Point& from, const Point& to);

    262. 

  262. 

    263. 

  263. 		//! Set a rotation matrix around the X axis.

    264. 

  264. 		//!		 1		0		0

    265. 

  265. 		//!	RX = 0		cx		sx

    266. 

  266. 		//!		 0		-sx		cx

    267. 

  267. 				void			RotX(float angle);

    268. 

  268. 		//! Set a rotation matrix around the Y axis.

    269. 

  269. 		//!		 cy		0		-sy

    270. 

  270. 		//!	RY = 0		1		0

    271. 

  271. 		//!		 sy		0		cy

    272. 

  272. 				void			RotY(float angle);

    273. 

  273. 		//! Set a rotation matrix around the Z axis.

    274. 

  274. 		//!		 cz		sz		0

    275. 

  275. 		//!	RZ = -sz	cz		0

    276. 

  276. 		//!		 0		0		1

    277. 

  277. 				void			RotZ(float angle);

    278. 

  278. 		//!			cy		sx.sy		-sy.cx

    279. 

  279. 		//!	RY.RX	0		cx			sx

    280. 

  280. 		//!			sy		-sx.cy		cx.cy

    281. 

  281. 				void			RotYX(float y, float x);

    282. 

  282. 

    283. 

  283. 		//! Make a rotation matrix about an arbitrary axis

    284. 

  284. 				Matrix3x3&		Rot(float angle, const Point& axis);

    285. 

  285. 

    286. 

  286. 		//! Transpose the matrix.

    287. 

  287. 				void			Transpose()

    288. 

  288. 								{

    289. 

  289. 									IR(m[1][0]) ^= IR(m[0][1]);	IR(m[0][1]) ^= IR(m[1][0]);	IR(m[1][0]) ^= IR(m[0][1]);

    290. 

  290. 									IR(m[2][0]) ^= IR(m[0][2]);	IR(m[0][2]) ^= IR(m[2][0]);	IR(m[2][0]) ^= IR(m[0][2]);

    291. 

  291. 									IR(m[2][1]) ^= IR(m[1][2]);	IR(m[1][2]) ^= IR(m[2][1]);	IR(m[2][1]) ^= IR(m[1][2]);

    292. 

  292. 								}

    293. 

  293. 

    294. 

  294. 		//! this = Transpose(a)

    295. 

  295. 				void			Transpose(const Matrix3x3& a)

    296. 

  296. 								{

    297. 

  297. 									m[0][0] = a.m[0][0];	m[0][1] = a.m[1][0];	m[0][2] = a.m[2][0];

    298. 

  298. 									m[1][0] = a.m[0][1];	m[1][1] = a.m[1][1];	m[1][2] = a.m[2][1];

    299. 

  299. 									m[2][0] = a.m[0][2];	m[2][1] = a.m[1][2];	m[2][2] = a.m[2][2];

    300. 

  300. 								}

    301. 

  301. 

    302. 

  302. 		//! Compute the determinant of the matrix. We use the rule of Sarrus.

    303. 

  303. 				float			Determinant()					const

    304. 

  304. 								{

    305. 

  305. 									return (m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1])

    306. 

  306. 										-  (m[2][0]*m[1][1]*m[0][2] + m[2][1]*m[1][2]*m[0][0] + m[2][2]*m[1][0]*m[0][1]);

    307. 

  307. 								}

    308. 

  308. /*

    309. 

  309. 		//! Compute a cofactor. Used for matrix inversion.

    310. 

  310. 				float			CoFactor(ubyte row, ubyte column)	const

    311. 

  311. 				{

    312. 

  312. 					static const sdword gIndex[3+2] = { 0, 1, 2, 0, 1 };

    313. 

  313. 					return	(m[gIndex[row+1]][gIndex[column+1]]*m[gIndex[row+2]][gIndex[column+2]] - m[gIndex[row+2]][gIndex[column+1]]*m[gIndex[row+1]][gIndex[column+2]]);

    314. 

  314. 				}

    315. 

  315. */

    316. 

  316. 		//! Invert the matrix. Determinant must be different from zero, else matrix can't be inverted.

    317. 

  317. 				Matrix3x3&		Invert()

    318. 

  318. 								{

    319. 

  319. 									float Det = Determinant();	// Must be !=0

    320. 

  320. 									float OneOverDet = 1.0f / Det;

    321. 

  321. 

    322. 

  322. 									Matrix3x3 Temp;

    323. 

  323. 									Temp.m[0][0] = +(m[1][1] * m[2][2] - m[2][1] * m[1][2]) * OneOverDet;

    324. 

  324. 									Temp.m[1][0] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]) * OneOverDet;

    325. 

  325. 									Temp.m[2][0] = +(m[1][0] * m[2][1] - m[2][0] * m[1][1]) * OneOverDet;

    326. 

  326. 									Temp.m[0][1] = -(m[0][1] * m[2][2] - m[2][1] * m[0][2]) * OneOverDet;

    327. 

  327. 									Temp.m[1][1] = +(m[0][0] * m[2][2] - m[2][0] * m[0][2]) * OneOverDet;

    328. 

  328. 									Temp.m[2][1] = -(m[0][0] * m[2][1] - m[2][0] * m[0][1]) * OneOverDet;

    329. 

  329. 									Temp.m[0][2] = +(m[0][1] * m[1][2] - m[1][1] * m[0][2]) * OneOverDet;

    330. 

  330. 									Temp.m[1][2] = -(m[0][0] * m[1][2] - m[1][0] * m[0][2]) * OneOverDet;

    331. 

  331. 									Temp.m[2][2] = +(m[0][0] * m[1][1] - m[1][0] * m[0][1]) * OneOverDet;

    332. 

  332. 

    333. 

  333. 									*this = Temp;

    334. 

  334. 

    335. 

  335. 									return	*this;

    336. 

  336. 								}

    337. 

  337. 

    338. 

  338. 				Matrix3x3&		Normalize();

    339. 

  339. 

    340. 

  340. 		//! this = exp(a)

    341. 

  341. 				Matrix3x3&		Exp(const Matrix3x3& a);

    342. 

  342. 

    343. 

  343. void FromQuat(const Quat &q);

    344. 

  344. void FromQuatL2(const Quat &q, float l2);

    345. 

  345. 

    346. 

  346. 		// Arithmetic operators

    347. 

  347. 		//! Operator for Matrix3x3 Plus = Matrix3x3 + Matrix3x3;

    348. 

  348. 		inline_	Matrix3x3		operator+(const Matrix3x3& mat)	const

    349. 

  349. 								{

    350. 

  350. 									return Matrix3x3(

    351. 

  351. 									m[0][0] + mat.m[0][0],	m[0][1] + mat.m[0][1],	m[0][2] + mat.m[0][2],

    352. 

  352. 									m[1][0] + mat.m[1][0],	m[1][1] + mat.m[1][1],	m[1][2] + mat.m[1][2],

    353. 

  353. 									m[2][0] + mat.m[2][0],	m[2][1] + mat.m[2][1],	m[2][2] + mat.m[2][2]);

    354. 

  354. 								}

    355. 

  355. 

    356. 

  356. 		//! Operator for Matrix3x3 Minus = Matrix3x3 - Matrix3x3;

    357. 

  357. 		inline_	Matrix3x3		operator-(const Matrix3x3& mat)	const

    358. 

  358. 								{

    359. 

  359. 									return Matrix3x3(

    360. 

  360. 									m[0][0] - mat.m[0][0],	m[0][1] - mat.m[0][1],	m[0][2] - mat.m[0][2],

    361. 

  361. 									m[1][0] - mat.m[1][0],	m[1][1] - mat.m[1][1],	m[1][2] - mat.m[1][2],

    362. 

  362. 									m[2][0] - mat.m[2][0],	m[2][1] - mat.m[2][1],	m[2][2] - mat.m[2][2]);

    363. 

  363. 								}

    364. 

  364. 

    365. 

  365. 		//! Operator for Matrix3x3 Mul = Matrix3x3 * Matrix3x3;

    366. 

  366. 		inline_	Matrix3x3		operator*(const Matrix3x3& mat)	const

    367. 

  367. 								{

    368. 

  368. 									return Matrix3x3(

    369. 

  369. 									m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0],

    370. 

  370. 									m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1],

    371. 

  371. 									m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2],

    372. 

  372. 

    373. 

  373. 									m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0],

    374. 

  374. 									m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1],

    375. 

  375. 									m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2],

    376. 

  376. 

    377. 

  377. 									m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0],

    378. 

  378. 									m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1],

    379. 

  379. 									m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2]);

    380. 

  380. 								}

    381. 

  381. 

    382. 

  382. 		//! Operator for Point Mul = Matrix3x3 * Point;

    383. 

  383. 		inline_	Point			operator*(const Point& v)		const		{ return Point(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v); }

    384. 

  384. 

    385. 

  385. 		//! Operator for Matrix3x3 Mul = Matrix3x3 * float;

    386. 

  386. 		inline_	Matrix3x3		operator*(float s)				const

    387. 

  387. 								{

    388. 

  388. 									return Matrix3x3(

    389. 

  389. 									m[0][0]*s,	m[0][1]*s,	m[0][2]*s,

    390. 

  390. 									m[1][0]*s,	m[1][1]*s,	m[1][2]*s,

    391. 

  391. 									m[2][0]*s,	m[2][1]*s,	m[2][2]*s);

    392. 

  392. 								}

    393. 

  393. 

    394. 

  394. 		//! Operator for Matrix3x3 Mul = float * Matrix3x3;

    395. 

  395. 		inline_	friend Matrix3x3 operator*(float s, const Matrix3x3& mat)

    396. 

  396. 								{

    397. 

  397. 									return Matrix3x3(

    398. 

  398. 									s*mat.m[0][0],	s*mat.m[0][1],	s*mat.m[0][2],

    399. 

  399. 									s*mat.m[1][0],	s*mat.m[1][1],	s*mat.m[1][2],

    400. 

  400. 									s*mat.m[2][0],	s*mat.m[2][1],	s*mat.m[2][2]);

    401. 

  401. 								}

    402. 

  402. 

    403. 

  403. 		//! Operator for Matrix3x3 Div = Matrix3x3 / float;

    404. 

  404. 		inline_	Matrix3x3		operator/(float s)				const

    405. 

  405. 								{

    406. 

  406. 									if (s)	s = 1.0f / s;

    407. 

  407. 									return Matrix3x3(

    408. 

  408. 									m[0][0]*s,	m[0][1]*s,	m[0][2]*s,

    409. 

  409. 									m[1][0]*s,	m[1][1]*s,	m[1][2]*s,

    410. 

  410. 									m[2][0]*s,	m[2][1]*s,	m[2][2]*s);

    411. 

  411. 								}

    412. 

  412. 

    413. 

  413. 		//! Operator for Matrix3x3 Div = float / Matrix3x3;

    414. 

  414. 		inline_	friend Matrix3x3 operator/(float s, const Matrix3x3& mat)

    415. 

  415. 								{

    416. 

  416. 									return Matrix3x3(

    417. 

  417. 									s/mat.m[0][0],	s/mat.m[0][1],	s/mat.m[0][2],

    418. 

  418. 									s/mat.m[1][0],	s/mat.m[1][1],	s/mat.m[1][2],

    419. 

  419. 									s/mat.m[2][0],	s/mat.m[2][1],	s/mat.m[2][2]);

    420. 

  420. 								}

    421. 

  421. 

    422. 

  422. 		//! Operator for Matrix3x3 += Matrix3x3

    423. 

  423. 		inline_	Matrix3x3&		operator+=(const Matrix3x3& mat)

    424. 

  424. 								{

    425. 

  425. 									m[0][0] += mat.m[0][0];		m[0][1] += mat.m[0][1];		m[0][2] += mat.m[0][2];

    426. 

  426. 									m[1][0] += mat.m[1][0];		m[1][1] += mat.m[1][1];		m[1][2] += mat.m[1][2];

    427. 

  427. 									m[2][0] += mat.m[2][0];		m[2][1] += mat.m[2][1];		m[2][2] += mat.m[2][2];

    428. 

  428. 									return	*this;

    429. 

  429. 								}

    430. 

  430. 

    431. 

  431. 		//! Operator for Matrix3x3 -= Matrix3x3

    432. 

  432. 		inline_	Matrix3x3&		operator-=(const Matrix3x3& mat)

    433. 

  433. 								{

    434. 

  434. 									m[0][0] -= mat.m[0][0];		m[0][1] -= mat.m[0][1];		m[0][2] -= mat.m[0][2];

    435. 

  435. 									m[1][0] -= mat.m[1][0];		m[1][1] -= mat.m[1][1];		m[1][2] -= mat.m[1][2];

    436. 

  436. 									m[2][0] -= mat.m[2][0];		m[2][1] -= mat.m[2][1];		m[2][2] -= mat.m[2][2];

    437. 

  437. 									return	*this;

    438. 

  438. 								}

    439. 

  439. 

    440. 

  440. 		//! Operator for Matrix3x3 *= Matrix3x3

    441. 

  441. 		inline_	Matrix3x3&		operator*=(const Matrix3x3& mat)

    442. 

  442. 								{

    443. 

  443. 									Point TempRow;

    444. 

  444. 

    445. 

  445. 									GetRow(0, TempRow);

    446. 

  446. 									m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];

    447. 

  447. 									m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];

    448. 

  448. 									m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];

    449. 

  449. 

    450. 

  450. 									GetRow(1, TempRow);

    451. 

  451. 									m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];

    452. 

  452. 									m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];

    453. 

  453. 									m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];

    454. 

  454. 

    455. 

  455. 									GetRow(2, TempRow);

    456. 

  456. 									m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];

    457. 

  457. 									m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];

    458. 

  458. 									m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];

    459. 

  459. 									return	*this;

    460. 

  460. 								}

    461. 

  461. 

    462. 

  462. 		//! Operator for Matrix3x3 *= float

    463. 

  463. 		inline_	Matrix3x3&		operator*=(float s)

    464. 

  464. 								{

    465. 

  465. 									m[0][0] *= s;	m[0][1] *= s;	m[0][2] *= s;

    466. 

  466. 									m[1][0] *= s;	m[1][1] *= s;	m[1][2] *= s;

    467. 

  467. 									m[2][0] *= s;	m[2][1] *= s;	m[2][2] *= s;

    468. 

  468. 									return	*this;

    469. 

  469. 								}

    470. 

  470. 

    471. 

  471. 		//! Operator for Matrix3x3 /= float

    472. 

  472. 		inline_	Matrix3x3&		operator/=(float s)

    473. 

  473. 								{

    474. 

  474. 									if (s)	s = 1.0f / s;

    475. 

  475. 									m[0][0] *= s;	m[0][1] *= s;	m[0][2] *= s;

    476. 

  476. 									m[1][0] *= s;	m[1][1] *= s;	m[1][2] *= s;

    477. 

  477. 									m[2][0] *= s;	m[2][1] *= s;	m[2][2] *= s;

    478. 

  478. 									return	*this;

    479. 

  479. 								}

    480. 

  480. 

    481. 

  481. 		// Cast operators

    482. 

  482. 		//! Cast a Matrix3x3 to a Matrix4x4.

    483. 

  483. 								operator Matrix4x4()	const;

    484. 

  484. 		//! Cast a Matrix3x3 to a Quat.

    485. 

  485. 								operator Quat()			const;

    486. 

  486. 

    487. 

  487. 		inline_	const Point&	operator[](int row)		const	{ return *(const Point*)&m[row][0];	}

    488. 

  488. 		inline_	Point&			operator[](int row)				{ return *(Point*)&m[row][0];		}

    489. 

  489. 

    490. 

  490. 		public:

    491. 

  491. 

    492. 

  492. 				float			m[3][3];

    493. 

  493. 	};

    494. 

  494. 

    495. 

  495. #endif // __ICEMATRIX3X3_H__
    496.