Branch data Line data Source code
1 : : #include "std.h"
2 : : #include "Mat4.h"
3 : :
4 [ + - ][ + - ]: 468 : Mat4 Mat4::identity(Vec4(1,0,0,0),Vec4(0,1,0,0),Vec4(0,0,1,0),Vec4(0,0,0,1));
[ + - ][ + - ]
5 [ + - ][ + - ]: 468 : Mat4 Mat4::zero(Vec4(0,0,0,0),Vec4(0,0,0,0),Vec4(0,0,0,0),Vec4(0,0,0,0));
[ + - ][ + - ]
6 [ + - ][ + - ]: 468 : Mat4 Mat4::unit(Vec4(1,1,1,1),Vec4(1,1,1,1),Vec4(1,1,1,1),Vec4(1,1,1,1));
[ + - ][ + - ]
7 : :
8 : 0 : Mat4 Mat4::trans(double x, double y, double z)
9 : : {
10 : : return Mat4(Vec4(1,0,0,x),
11 : : Vec4(0,1,0,y),
12 : : Vec4(0,0,1,z),
13 [ # # ][ # # ]: 0 : Vec4(0,0,0,1));
[ # # ][ # # ]
14 : : }
15 : :
16 : 0 : Mat4 Mat4::scale(double x, double y, double z)
17 : : {
18 : : return Mat4(Vec4(x,0,0,0),
19 : : Vec4(0,y,0,0),
20 : : Vec4(0,0,z,0),
21 [ # # ][ # # ]: 0 : Vec4(0,0,0,1));
[ # # ][ # # ]
22 : : }
23 : :
24 : 0 : Mat4 Mat4::xrot(double a)
25 : : {
26 : 0 : double c = cos(a);
27 : 0 : double s = sin(a);
28 : :
29 : : return Mat4(Vec4(1, 0, 0, 0),
30 : : Vec4(0, c,-s, 0),
31 : : Vec4(0, s, c, 0),
32 [ # # ][ # # ]: 0 : Vec4(0, 0, 0, 1));
[ # # ][ # # ]
33 : : }
34 : :
35 : 0 : Mat4 Mat4::yrot(double a)
36 : : {
37 : 0 : double c = cos(a);
38 : 0 : double s = sin(a);
39 : :
40 : : return Mat4(Vec4(c, 0, s, 0),
41 : : Vec4(0, 1, 0, 0),
42 : : Vec4(-s,0, c, 0),
43 [ # # ][ # # ]: 0 : Vec4(0, 0, 0, 1));
[ # # ][ # # ]
44 : : }
45 : :
46 : 0 : Mat4 Mat4::zrot(double a)
47 : : {
48 : 0 : double c = cos(a);
49 : 0 : double s = sin(a);
50 : :
51 : : return Mat4(Vec4(c,-s, 0, 0),
52 : : Vec4(s, c, 0, 0),
53 : : Vec4(0, 0, 1, 0),
54 [ # # ][ # # ]: 0 : Vec4(0, 0, 0, 1));
[ # # ][ # # ]
55 : : }
56 : :
57 : 0 : Mat4 Mat4::operator*(const Mat4& m) const
58 : : {
59 : 0 : Mat4 A;
60 : : int i,j;
61 : :
62 [ # # ]: 0 : for(i=0;i<4;i++)
63 [ # # ]: 0 : for(j=0;j<4;j++)
64 [ # # ]: 0 : A(i,j) = row[i]*m.col(j);
65 : :
66 : 0 : return A;
67 : : }
68 : :
69 : 0 : double Mat4::det() const
70 : : {
71 [ # # ]: 0 : return row[0] * cross(row[1], row[2], row[3]);
72 : : }
73 : :
74 : 0 : Mat4 Mat4::transpose() const
75 : : {
76 [ # # ][ # # ]: 0 : return Mat4(col(0), col(1), col(2), col(3));
[ # # ][ # # ]
77 : : }
78 : :
79 : 0 : Mat4 Mat4::adjoint() const
80 : : {
81 : 0 : Mat4 A;
82 : :
83 [ # # ]: 0 : A.row[0] = cross( row[1], row[2], row[3]);
84 [ # # ][ # # ]: 0 : A.row[1] = cross(-row[0], row[2], row[3]);
85 [ # # ]: 0 : A.row[2] = cross( row[0], row[1], row[3]);
86 [ # # ][ # # ]: 0 : A.row[3] = cross(-row[0], row[1], row[2]);
87 : :
88 : 0 : return A;
89 : : }
90 : :
91 : 0 : double Mat4::cramerInverse(Mat4& inv) const
92 : : {
93 [ # # ]: 0 : Mat4 A = adjoint();
94 [ # # ]: 0 : double d = A.row[0] * row[0];
95 : :
96 [ # # ]: 0 : if( d==0.0 )
97 : 0 : return 0.0;
98 : :
99 [ # # ][ # # ]: 0 : inv = A.transpose() / d;
[ # # ]
100 : 0 : return d;
101 : : }
102 : :
103 : :
104 : :
105 : : // Matrix inversion code for 4x4 matrices.
106 : : // Originally ripped off and degeneralized from Paul's matrix library
107 : : // for the view synthesis (Chen) software.
108 : : //
109 : : // Returns determinant of a, and b=a inverse.
110 : : // If matrix is singular, returns 0 and leaves trash in b.
111 : : //
112 : : // Uses Gaussian elimination with partial pivoting.
113 : :
114 : : #define SWAP(a, b, t) {t = a; a = b; b = t;}
115 : 363696 : double Mat4::inverse(Mat4& B) const
116 : : {
117 [ + - ]: 363696 : Mat4 A(*this);
118 : :
119 : : int i, j, k;
120 : : double max, t, det, pivot;
121 : :
122 : : /*---------- forward elimination ----------*/
123 : :
124 [ + + ]: 1818480 : for (i=0; i<4; i++) /* put identity matrix in B */
125 [ + + ]: 7273920 : for (j=0; j<4; j++)
126 [ + - ][ + + ]: 5819136 : B(i, j) = (double)(i==j);
127 : :
128 : 363696 : det = 1.0;
129 [ + + ]: 1818480 : for (i=0; i<4; i++) { /* eliminate in column i, below diag */
130 : 1454784 : max = -1.;
131 [ + + ]: 5091744 : for (k=i; k<4; k++) /* find pivot for column i */
132 [ + - ][ + + ]: 3636960 : if (fabs(A(k, i)) > max) {
133 [ + - ]: 2141184 : max = fabs(A(k, i));
134 : 2141184 : j = k;
135 : : }
136 [ - + ]: 1454784 : if (max<=0.) return 0.; /* if no nonzero pivot, PUNT */
137 [ + + ]: 1454784 : if (j!=i) { /* swap rows i and j */
138 [ + + ]: 2569752 : for (k=i; k<4; k++)
139 [ + - ][ + - ]: 2011968 : SWAP(A(i, k), A(j, k), t);
[ + - ][ + - ]
140 [ + + ]: 2788920 : for (k=0; k<4; k++)
141 [ + - ][ + - ]: 2231136 : SWAP(B(i, k), B(j, k), t);
[ + - ][ + - ]
142 : 557784 : det = -det;
143 : : }
144 [ + - ]: 1454784 : pivot = A(i, i);
145 : 1454784 : det *= pivot;
146 [ + + ]: 3636960 : for (k=i+1; k<4; k++) /* only do elems to right of pivot */
147 [ + - ]: 2182176 : A(i, k) /= pivot;
148 [ + + ]: 7273920 : for (k=0; k<4; k++)
149 [ + - ]: 5819136 : B(i, k) /= pivot;
150 : : /* we know that A(i, i) will be set to 1, so don't bother to do it */
151 : :
152 [ + + ]: 3636960 : for (j=i+1; j<4; j++) { /* eliminate in rows below i */
153 [ + - ]: 2182176 : t = A(j, i); /* we're gonna zero this guy */
154 [ + + ]: 7273920 : for (k=i+1; k<4; k++) /* subtract scaled row i from row j */
155 [ + - ][ + - ]: 5091744 : A(j, k) -= A(i, k)*t; /* (ignore k<=i, we know they're 0) */
156 [ + + ]: 10910880 : for (k=0; k<4; k++)
157 [ + - ][ + - ]: 8728704 : B(j, k) -= B(i, k)*t;
158 : : }
159 : : }
160 : :
161 : : /*---------- backward elimination ----------*/
162 : :
163 [ + + ]: 1454784 : for (i=4-1; i>0; i--) { /* eliminate in column i, above diag */
164 [ + + ]: 3273264 : for (j=0; j<i; j++) { /* eliminate in rows above i */
165 [ + - ]: 2182176 : t = A(j, i); /* we're gonna zero this guy */
166 [ + + ]: 10910880 : for (k=0; k<4; k++) /* subtract scaled row i from row j */
167 [ + - ][ + - ]: 8728704 : B(j, k) -= B(i, k)*t;
168 : : }
169 : : }
170 : :
171 : 363696 : return det;
172 [ + - ][ + - ]: 1872 : }
|
