00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #ifndef _matrix4x4_impl_h
00013 #define _matrix4x4_impl_h
00014
00015 #include <cmath>
00016 #include "Vector4.h"
00017 #include <algorithm>
00018
00019 template <typename Real> Matrix4x4<Real> Matrix4x4<Real>::identityMatrix = Matrix4x4<Real>();
00020
00021 template <typename Real>
00022 Matrix4x4<Real>::Matrix4x4()
00023 {
00024 m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
00025 m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
00026 m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
00027 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
00028 }
00029
00030
00031 template <typename Real>
00032 Matrix4x4<Real>::Matrix4x4(const Real m[4][4])
00033 {
00034 this->m[0][0] = m[0][0]; this->m[0][1] = m[0][1]; this->m[0][2] = m[0][2]; this->m[0][3] = m[0][3];
00035 this->m[1][0] = m[1][0]; this->m[1][1] = m[1][1]; this->m[1][2] = m[1][2]; this->m[1][3] = m[1][3];
00036 this->m[2][0] = m[2][0]; this->m[2][1] = m[2][1]; this->m[2][2] = m[2][2]; this->m[2][3] = m[2][3];
00037 this->m[3][0] = m[3][0]; this->m[3][1] = m[3][1]; this->m[3][2] = m[3][2]; this->m[3][3] = m[3][3];
00038 }
00039
00040
00041 template <typename Real>
00042 Matrix4x4<Real> Matrix4x4<Real>::identity()
00043 {
00044 return identityMatrix;
00045 }
00046
00047
00048 template <typename Real>
00049 Vector4<Real> Matrix4x4<Real>::operator*(const Vector4<Real>& vec4) const
00050 {
00051 Vector4<Real> res;
00052
00053 res[0] = vec4[0] * m[0][0] + vec4[1] * m[0][1] + vec4[2] * m[0][2] + vec4[3] * m[0][3];
00054 res[1] = vec4[0] * m[1][0] + vec4[1] * m[1][1] + vec4[2] * m[1][2] + vec4[3] * m[1][3];
00055 res[2] = vec4[0] * m[2][0] + vec4[1] * m[2][1] + vec4[2] * m[2][2] + vec4[3] * m[2][3];
00056 res[3] = vec4[0] * m[3][0] + vec4[1] * m[3][1] + vec4[2] * m[3][2] + vec4[3] * m[3][3];
00057
00058 return res;
00059 }
00060
00061
00062 template <typename Real>
00063 Matrix4x4<Real> Matrix4x4<Real>::operator*(const Matrix4x4& m2) const
00064 {
00065 Matrix4x4 res;
00066 unsigned int i, j, k;
00067
00068 for (i=0; i<4; i++)
00069 {
00070 for (j=0; j<4; j++)
00071 {
00072 res.m[i][j] = Real(0.0);
00073 for (k=0; k<4; k++)
00074 {
00075 res.m[i][j] += m[i][k] * m2.m[k][j];
00076 }
00077 }
00078 }
00079
00080 return res;
00081 }
00082
00083
00084
00085 template <typename Real>
00086 Real &Matrix4x4<Real>::operator()(unsigned int i, unsigned int j)
00087 {
00088 return m[i][j];
00089 }
00090
00091
00092 template <typename Real>
00093 Matrix4x4<Real> Matrix4x4<Real>::inverse() const
00094 {
00095 Matrix4x4 a = Matrix4x4(m);
00096 Matrix4x4 b = identity();
00097
00098
00100
00101
00102
00103 int i,icol,irow,j,k,l,ll;
00104 Real big,dum,pivinv,temp;
00105
00106
00107 int n = 4;
00108
00109 int m = 4;
00110
00111 Vector4<int> indxc, indxr, ipiv;
00112 for (j=0;j<n;j++) ipiv[j]=0;
00113 for (i=0;i<n;i++) {
00114 big=0.0;
00115 for (j=0;j<n;j++)
00116 if (ipiv[j] != 1)
00117 for (k=0;k<n;k++) {
00118 if (ipiv[k] == 0) {
00119 if (fabs(a[j][k]) >= big) {
00120 big=fabs(a[j][k]);
00121 irow=j;
00122 icol=k;
00123 }
00124 }
00125 }
00126 ++(ipiv[icol]);
00127 if (irow != icol) {
00128 for (l=0;l<n;l++) std::swap(a[irow][l],a[icol][l]);
00129 for (l=0;l<m;l++) std::swap(b[irow][l],b[icol][l]);
00130 }
00131 indxr[i]=irow;
00132 indxc[i]=icol;
00133
00134 pivinv=1.0/a[icol][icol];
00135 a[icol][icol]=1.0;
00136 for (l=0;l<n;l++) a[icol][l] *= pivinv;
00137 for (l=0;l<m;l++) b[icol][l] *= pivinv;
00138 for (ll=0;ll<n;ll++)
00139 if (ll != icol) {
00140 dum=a[ll][icol];
00141 a[ll][icol]=0.0;
00142 for (l=0;l<n;l++) a[ll][l] -= a[icol][l]*dum;
00143 for (l=0;l<m;l++) b[ll][l] -= b[icol][l]*dum;
00144 }
00145 }
00146 for (l=n-1;l>=0;l--) {
00147 if (indxr[l] != indxc[l])
00148 for (k=0;k<n;k++)
00149 std::swap(a[k][indxr[l]],a[k][indxc[l]]);
00150 }
00152
00153
00154 return a;
00155 }
00156
00157
00158
00159
00160
00161 template <typename Real>
00162 Matrix4x4<Real> Matrix4x4<Real>::scale(Real sx, Real sy, Real sz)
00163 {
00164 Real m[4][4];
00165
00166 m[0][0] = sx; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
00167 m[1][0] = 0; m[1][1] = sy; m[1][2] = 0; m[1][3] = 0;
00168 m[2][0] = 0; m[2][1] = 0; m[2][2] = sz; m[2][3] = 0;
00169 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
00170
00171 return Matrix4x4(m);
00172 }
00173
00174
00175 template <typename Real>
00176 Matrix4x4<Real> Matrix4x4<Real>::scale(Real scale)
00177 {
00178 Real m[4][4];
00179
00180 m[0][0] = scale; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
00181 m[1][0] = 0; m[1][1] = scale; m[1][2] = 0; m[1][3] = 0;
00182 m[2][0] = 0; m[2][1] = 0; m[2][2] = scale; m[2][3] = 0;
00183 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
00184
00185 return Matrix4x4(m);
00186 }
00187
00188 template <typename Real>
00189 Matrix4x4<Real> Matrix4x4<Real>::rotationXYZ(Real rx, Real ry, Real rz)
00190 {
00191 Real m[4][4];
00192
00193 m[0][0] = cos(ry) * cos(rz);
00194 m[0][1] = cos(ry) * sin(rz);
00195 m[0][2] = -sin(ry);
00196 m[0][3] = 0;
00197 m[1][0] = sin(rx) * sin(ry) * cos(rz) - cos(rx) * sin(rz);
00198 m[1][1] = sin(rx) * sin(ry) * sin(rz) + cos(rx) * cos(rz);
00199 m[1][2] = cos(ry) * sin(rx);
00200 m[1][3] = 0;
00201 m[2][0] = cos(rx) * sin(ry) * cos(rz) + sin(rx) * sin(rz);
00202 m[2][1] = cos(rx) * sin(ry) * sin(rz) - sin(rx) * cos(rz);
00203 m[2][2] = cos(ry) * cos(rx);
00204 m[2][3] = 0;
00205 m[3][0] = 0;
00206 m[3][1] = 0;
00207 m[3][2] = 0;
00208 m[3][3] = 1;
00209
00210 return Matrix4x4(m);
00211 }
00212
00213 template <typename Real>
00214 Matrix4x4<Real> Matrix4x4<Real>::translation(Real tx, Real ty, Real tz)
00215 {
00216 Real m[4][4];
00217
00218 m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = tx;
00219 m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = ty;
00220 m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = tz;
00221 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
00222
00223 return Matrix4x4(m);
00224 }
00225
00226 template <typename Real>
00227 void Matrix4x4<Real>::toGLMatrix(float glMatrix[16]) const
00228 {
00229 int glIndex = 0;
00230 for (int i = 0; i < 4; i++){
00231 for (int j = 0; j < 4; j++){
00232 glMatrix[glIndex] = m[j][i];
00233 glIndex++;
00234 }
00235 }
00236 }
00237
00238 #endif