/lib/ode/ode_source/OPCODE/Ice/IceMatrix4x4.cpp

http://narutortsproject.googlecode.com/ · C++ · 135 lines · 63 code · 16 blank · 56 comment · 1 complexity · 90595b10e8077df3cacb235d3f829d39 MD5 · raw file 

    

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

    2. 

  2. /**

    3. 

  3.  *	Contains code for 4x4 matrices.

    4. 

  4.  *	\file		IceMatrix4x4.cpp

    5. 

  5.  *	\author		Pierre Terdiman

    6. 

  6.  *	\date		April, 4, 2000

    7. 

  7.  */

    8. 

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

    9. 

  9. 

    10. 

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

    11. 

  11. /**

    12. 

  12.  *	4x4 matrix.

    13. 

  13.  *	DirectX-compliant, ie row-column order, ie m[Row][Col].

    14. 

  14.  *	Same as:

    15. 

  15.  *	m11  m12  m13  m14	first row.

    16. 

  16.  *	m21  m22  m23  m24	second row.

    17. 

  17.  *	m31  m32  m33  m34	third row.

    18. 

  18.  *	m41  m42  m43  m44	fourth row.

    19. 

  19.  *	Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1).

    20. 

  20.  *	Stored in memory as m11 m12 m13 m14 m21...

    21. 

  21.  *

    22. 

  22.  *	Multiplication rules:

    23. 

  23.  *

    24. 

  24.  *	[x'y'z'1] = [xyz1][M]

    25. 

  25.  *

    26. 

  26.  *	x' = x*m11 + y*m21 + z*m31 + m41

    27. 

  27.  *	y' = x*m12 + y*m22 + z*m32 + m42

    28. 

  28.  *	z' = x*m13 + y*m23 + z*m33 + m43

    29. 

  29.  *	1' =     0 +     0 +     0 + m44

    30. 

  30.  *

    31. 

  31.  *	\class		Matrix4x4

    32. 

  32.  *	\author		Pierre Terdiman

    33. 

  33.  *	\version	1.0

    34. 

  34.  */

    35. 

  35. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    36. 

  36. 

    37. 

  37. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    38. 

  38. // Precompiled Header

    39. 

  39. #include "Stdafx.h"

    40. 

  40. 

    41. 

  41. using namespace IceMaths;

    42. 

  42. 

    43. 

  43. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    44. 

  44. /**

    45. 

  45.  *	Inverts a PR matrix. (which only contains a rotation and a translation)

    46. 

  46.  *	This is faster and less subject to FPU errors than the generic inversion code.

    47. 

  47.  *

    48. 

  48.  *	\relates	Matrix4x4

    49. 

  49.  *	\fn			InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)

    50. 

  50.  *	\param		dest	[out] destination matrix

    51. 

  51.  *	\param		src		[in] source matrix

    52. 

  52.  */

    53. 

  53. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    54. 

  54. ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)

    55. 

  55. {

    56. 

  56. 	dest.m[0][0] = src.m[0][0];

    57. 

  57. 	dest.m[1][0] = src.m[0][1];

    58. 

  58. 	dest.m[2][0] = src.m[0][2];

    59. 

  59. 	dest.m[3][0] = -(src.m[3][0]*src.m[0][0] + src.m[3][1]*src.m[0][1] + src.m[3][2]*src.m[0][2]);

    60. 

  60. 

    61. 

  61. 	dest.m[0][1] = src.m[1][0];

    62. 

  62. 	dest.m[1][1] = src.m[1][1];

    63. 

  63. 	dest.m[2][1] = src.m[1][2];

    64. 

  64. 	dest.m[3][1] = -(src.m[3][0]*src.m[1][0] + src.m[3][1]*src.m[1][1] + src.m[3][2]*src.m[1][2]);

    65. 

  65. 

    66. 

  66. 	dest.m[0][2] = src.m[2][0];

    67. 

  67. 	dest.m[1][2] = src.m[2][1];

    68. 

  68. 	dest.m[2][2] = src.m[2][2];

    69. 

  69. 	dest.m[3][2] = -(src.m[3][0]*src.m[2][0] + src.m[3][1]*src.m[2][1] + src.m[3][2]*src.m[2][2]);

    70. 

  70. 

    71. 

  71. 	dest.m[0][3] = 0.0f;

    72. 

  72. 	dest.m[1][3] = 0.0f;

    73. 

  73. 	dest.m[2][3] = 0.0f;

    74. 

  74. 	dest.m[3][3] = 1.0f;

    75. 

  75. }

    76. 

  76. 

    77. 

  77. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    78. 

  78. // Compute the cofactor of the Matrix at a specified location

    79. 

  79. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    80. 

  80. float Matrix4x4::CoFactor(udword row, udword col) const

    81. 

  81. {

    82. 

  82. 	return	 (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] +

    83. 

  83. 				m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] +

    84. 

  84. 				m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3])

    85. 

  85. 			-  (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] +

    86. 

  86. 				m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] +

    87. 

  87. 				m[(row+3)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+1)&3][(col+2)&3])) * ((row + col) & 1 ? -1.0f : +1.0f);

    88. 

  88. }

    89. 

  89. 

    90. 

  90. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    91. 

  91. // Compute the determinant of the Matrix

    92. 

  92. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    93. 

  93. float Matrix4x4::Determinant() const

    94. 

  94. {

    95. 

  95. 	return	m[0][0] * CoFactor(0, 0) +

    96. 

  96. 			m[0][1] * CoFactor(0, 1) +

    97. 

  97. 			m[0][2] * CoFactor(0, 2) +

    98. 

  98. 			m[0][3] * CoFactor(0, 3);

    99. 

  99. }

    100. 

  100. 

    101. 

  101. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    102. 

  102. // Compute the inverse of the matrix

    103. 

  103. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    104. 

  104. Matrix4x4& Matrix4x4::Invert()

    105. 

  105. {

    106. 

  106. 	float Det = Determinant();

    107. 

  107. 	Matrix4x4 Temp;

    108. 

  108. 

    109. 

  109. 	if(fabsf(Det) < MATRIX4X4_EPSILON)

    110. 

  110. 		return	*this;		// The matrix is not invertible! Singular case!

    111. 

  111. 

    112. 

  112. 	float IDet = 1.0f / Det;

    113. 

  113. 

    114. 

  114. 	Temp.m[0][0] = CoFactor(0,0) * IDet;

    115. 

  115. 	Temp.m[1][0] = CoFactor(0,1) * IDet;

    116. 

  116. 	Temp.m[2][0] = CoFactor(0,2) * IDet;

    117. 

  117. 	Temp.m[3][0] = CoFactor(0,3) * IDet;

    118. 

  118. 	Temp.m[0][1] = CoFactor(1,0) * IDet;

    119. 

  119. 	Temp.m[1][1] = CoFactor(1,1) * IDet;

    120. 

  120. 	Temp.m[2][1] = CoFactor(1,2) * IDet;

    121. 

  121. 	Temp.m[3][1] = CoFactor(1,3) * IDet;

    122. 

  122. 	Temp.m[0][2] = CoFactor(2,0) * IDet;

    123. 

  123. 	Temp.m[1][2] = CoFactor(2,1) * IDet;

    124. 

  124. 	Temp.m[2][2] = CoFactor(2,2) * IDet;

    125. 

  125. 	Temp.m[3][2] = CoFactor(2,3) * IDet;

    126. 

  126. 	Temp.m[0][3] = CoFactor(3,0) * IDet;

    127. 

  127. 	Temp.m[1][3] = CoFactor(3,1) * IDet;

    128. 

  128. 	Temp.m[2][3] = CoFactor(3,2) * IDet;

    129. 

  129. 	Temp.m[3][3] = CoFactor(3,3) * IDet;

    130. 

  130. 

    131. 

  131. 	*this = Temp;

    132. 

  132. 

    133. 

  133. 	return	*this;

    134. 

  134. }
    135.