Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/OPCODE/Ice/IceMatrix4x4.cpp @ 216

Last change on this file since 216 was 216, checked in by mathiask, 16 years ago

[Physik] add ode-0.9
File size: 6.1 KB
Line 
1///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2/**
3 *      Contains code for 4x4 matrices.
4 *      \file           IceMatrix4x4.cpp
5 *      \author         Pierre Terdiman
6 *      \date           April, 4, 2000
7 */
8///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
9
10///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
11/**
12 *      4x4 matrix.
13 *      DirectX-compliant, ie row-column order, ie m[Row][Col].
14 *      Same as:
15 *      m11  m12  m13  m14      first row.
16 *      m21  m22  m23  m24      second row.
17 *      m31  m32  m33  m34      third row.
18 *      m41  m42  m43  m44      fourth row.
19 *      Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1).
20 *      Stored in memory as m11 m12 m13 m14 m21...
21 *
22 *      Multiplication rules:
23 *
24 *      [x'y'z'1] = [xyz1][M]
25 *
26 *      x' = x*m11 + y*m21 + z*m31 + m41
27 *      y' = x*m12 + y*m22 + z*m32 + m42
28 *      z' = x*m13 + y*m23 + z*m33 + m43
29 *      1' =     0 +     0 +     0 + m44
30 *
31 *      \class          Matrix4x4
32 *      \author         Pierre Terdiman
33 *      \version        1.0
34 */
35///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
36
37///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
38// Precompiled Header
39#include "Stdafx.h"
40
41using namespace IceMaths;
42
43///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
44/**
45 *      Inverts a PR matrix. (which only contains a rotation and a translation)
46 *      This is faster and less subject to FPU errors than the generic inversion code.
47 *
48 *      \relates        Matrix4x4
49 *      \fn                     InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
50 *      \param          dest    [out] destination matrix
51 *      \param          src             [in] source matrix
52 */
53///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
54ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
55{
56        dest.m[0][0] = src.m[0][0];
57        dest.m[1][0] = src.m[0][1];
58        dest.m[2][0] = src.m[0][2];
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
61        dest.m[0][1] = src.m[1][0];
62        dest.m[1][1] = src.m[1][1];
63        dest.m[2][1] = src.m[1][2];
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
66        dest.m[0][2] = src.m[2][0];
67        dest.m[1][2] = src.m[2][1];
68        dest.m[2][2] = src.m[2][2];
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
71        dest.m[0][3] = 0.0f;
72        dest.m[1][3] = 0.0f;
73        dest.m[2][3] = 0.0f;
74        dest.m[3][3] = 1.0f;
75}
76
77///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78// Compute the cofactor of the Matrix at a specified location
79///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
80float Matrix4x4::CoFactor(udword row, udword col) const
81{
82        return   (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] +
83                                m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] +
84                                m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3])
85                        -  (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] +
86                                m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] +
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}
89
90///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
91// Compute the determinant of the Matrix
92///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
93float Matrix4x4::Determinant() const
94{
95        return  m[0][0] * CoFactor(0, 0) +
96                        m[0][1] * CoFactor(0, 1) +
97                        m[0][2] * CoFactor(0, 2) +
98                        m[0][3] * CoFactor(0, 3);
99}
100
101///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
102// Compute the inverse of the matrix
103///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104Matrix4x4& Matrix4x4::Invert()
105{
106        float Det = Determinant();
107        Matrix4x4 Temp;
108
109        if(fabsf(Det) < MATRIX4X4_EPSILON)
110                return  *this;          // The matrix is not invertible! Singular case!
111
112        float IDet = 1.0f / Det;
113
114        Temp.m[0][0] = CoFactor(0,0) * IDet;
115        Temp.m[1][0] = CoFactor(0,1) * IDet;
116        Temp.m[2][0] = CoFactor(0,2) * IDet;
117        Temp.m[3][0] = CoFactor(0,3) * IDet;
118        Temp.m[0][1] = CoFactor(1,0) * IDet;
119        Temp.m[1][1] = CoFactor(1,1) * IDet;
120        Temp.m[2][1] = CoFactor(1,2) * IDet;
121        Temp.m[3][1] = CoFactor(1,3) * IDet;
122        Temp.m[0][2] = CoFactor(2,0) * IDet;
123        Temp.m[1][2] = CoFactor(2,1) * IDet;
124        Temp.m[2][2] = CoFactor(2,2) * IDet;
125        Temp.m[3][2] = CoFactor(2,3) * IDet;
126        Temp.m[0][3] = CoFactor(3,0) * IDet;
127        Temp.m[1][3] = CoFactor(3,1) * IDet;
128        Temp.m[2][3] = CoFactor(3,2) * IDet;
129        Temp.m[3][3] = CoFactor(3,3) * IDet;
130
131        *this = Temp;
132
133        return  *this;
134}
135
Note: See TracBrowser for help on using the repository browser.