1 | /*
|
---|
2 | * ----------------------------------------------------------------------
|
---|
3 | * Mat4x4.cpp: Mat4x4 class
|
---|
4 | *
|
---|
5 | * ======================================================================
|
---|
6 | * AUTHOR: Wei Qiao <qiaow@purdue.edu>
|
---|
7 | * Purdue Rendering and Perceptualization Lab (PURPL)
|
---|
8 | *
|
---|
9 | * Copyright (c) 2004-2006 Purdue Research Foundation
|
---|
10 | *
|
---|
11 | * See the file "license.terms" for information on usage and
|
---|
12 | * redistribution of this file, and for a DISCLAIMER OF ALL WARRANTIES.
|
---|
13 | * ======================================================================
|
---|
14 | */
|
---|
15 | #include "Mat4x4.h"
|
---|
16 | #include <math.h>
|
---|
17 | #include <stdio.h>
|
---|
18 | #include <assert.h>
|
---|
19 |
|
---|
20 | Mat4x4::Mat4x4(){}
|
---|
21 |
|
---|
22 | Mat4x4::Mat4x4(float *vals){
|
---|
23 | int i;
|
---|
24 | for (i=0;i<16;i++)
|
---|
25 | m[i] = vals[i];
|
---|
26 | }
|
---|
27 |
|
---|
28 | Mat4x4::~Mat4x4(){}
|
---|
29 |
|
---|
30 | void Mat4x4::print(){
|
---|
31 | fprintf(stderr, "\n%f %f %f %f\n", m[0], m[1], m[2], m[3]);
|
---|
32 | fprintf(stderr, "%f %f %f %f\n", m[4], m[5], m[6], m[7]);
|
---|
33 | fprintf(stderr, "%f %f %f %f\n", m[8], m[9], m[10], m[11]);
|
---|
34 | fprintf(stderr, "%f %f %f %f\n\n", m[12], m[13], m[14], m[15]);
|
---|
35 | }
|
---|
36 |
|
---|
37 | Mat4x4 Mat4x4::inverse(){
|
---|
38 | Mat4x4 p;
|
---|
39 |
|
---|
40 | float m00 = m[0], m01 = m[4], m02 = m[8], m03 = m[12];
|
---|
41 | float m10 = m[1], m11 = m[5], m12 = m[9], m13 = m[13];
|
---|
42 | float m20 = m[2], m21 = m[6], m22 = m[10], m23 = m[14];
|
---|
43 | float m30 = m[3], m31 = m[7], m32 = m[11], m33 = m[15];
|
---|
44 |
|
---|
45 | float det0, inv0;
|
---|
46 |
|
---|
47 | #define det33(a1,a2,a3,b1,b2,b3,c1,c2,c3) \
|
---|
48 | (a1*(b2*c3-b3*c2) + b1*(c2*a3-a2*c3) + c1*(a2*b3-a3*b2))
|
---|
49 |
|
---|
50 | float cof00 = det33(m11, m12, m13, m21, m22, m23, m31, m32, m33);
|
---|
51 | float cof01 = -det33(m12, m13, m10, m22, m23, m20, m32, m33, m30);
|
---|
52 | float cof02 = det33(m13, m10, m11, m23, m20, m21, m33, m30, m31);
|
---|
53 | float cof03 = -det33(m10, m11, m12, m20, m21, m22, m30, m31, m32);
|
---|
54 |
|
---|
55 | float cof10 = -det33(m21, m22, m23, m31, m32, m33, m01, m02, m03);
|
---|
56 | float cof11 = det33(m22, m23, m20, m32, m33, m30, m02, m03, m00);
|
---|
57 | float cof12 = -det33(m23, m20, m21, m33, m30, m31, m03, m00, m01);
|
---|
58 | float cof13 = det33(m20, m21, m22, m30, m31, m32, m00, m01, m02);
|
---|
59 |
|
---|
60 | float cof20 = det33(m31, m32, m33, m01, m02, m03, m11, m12, m13);
|
---|
61 | float cof21 = -det33(m32, m33, m30, m02, m03, m00, m12, m13, m10);
|
---|
62 | float cof22 = det33(m33, m30, m31, m03, m00, m01, m13, m10, m11);
|
---|
63 | float cof23 = -det33(m30, m31, m32, m00, m01, m02, m10, m11, m12);
|
---|
64 |
|
---|
65 | float cof30 = -det33(m01, m02, m03, m11, m12, m13, m21, m22, m23);
|
---|
66 | float cof31 = det33(m02, m03, m00, m12, m13, m10, m22, m23, m20);
|
---|
67 | float cof32 = -det33(m03, m00, m01, m13, m10, m11, m23, m20, m21);
|
---|
68 | float cof33 = det33(m00, m01, m02, m10, m11, m12, m20, m21, m22);
|
---|
69 |
|
---|
70 | #undef det33
|
---|
71 |
|
---|
72 | #if 0
|
---|
73 | fprintf(stderr, "Invert:\n");
|
---|
74 | fprintf(stderr, " %12.9f %12.9f %12.9f %12.9f\n", m00, m01, m02, m03);
|
---|
75 | fprintf(stderr, " %12.9f %12.9f %12.9f %12.9f\n", m10, m11, m12, m13);
|
---|
76 | fprintf(stderr, " %12.9f %12.9f %12.9f %12.9f\n", m20, m21, m22, m23);
|
---|
77 | fprintf(stderr, " %12.9f %12.9f %12.9f %12.9f\n", m30, m31, m32, m33);
|
---|
78 | #endif
|
---|
79 |
|
---|
80 | #if 0
|
---|
81 | det0 = m00 * cof00 + m01 * cof01 + m02 * cof02 + m03 * cof03;
|
---|
82 | #else
|
---|
83 | det0 = m30 * cof30 + m31 * cof31 + m32 * cof32 + m33 * cof33;
|
---|
84 | #endif
|
---|
85 | inv0 = (det0 == 0.0f) ? 0.0f : 1.0f / det0;
|
---|
86 |
|
---|
87 | p.m[0] = cof00 * inv0;
|
---|
88 | p.m[1] = cof01 * inv0;
|
---|
89 | p.m[2] = cof02 * inv0;
|
---|
90 | p.m[3] = cof03 * inv0;
|
---|
91 |
|
---|
92 | p.m[4] = cof10 * inv0;
|
---|
93 | p.m[5] = cof11 * inv0;
|
---|
94 | p.m[6] = cof12 * inv0;
|
---|
95 | p.m[7] = cof13 * inv0;
|
---|
96 |
|
---|
97 | p.m[8] = cof20 * inv0;
|
---|
98 | p.m[9] = cof21 * inv0;
|
---|
99 | p.m[10] = cof22 * inv0;
|
---|
100 | p.m[11] = cof23 * inv0;
|
---|
101 |
|
---|
102 | p.m[12] = cof30 * inv0;
|
---|
103 | p.m[13] = cof31 * inv0;
|
---|
104 | p.m[14] = cof32 * inv0;
|
---|
105 | p.m[15]= cof33 * inv0;
|
---|
106 |
|
---|
107 | return p;
|
---|
108 | /*
|
---|
109 | Mat4x4 ret;
|
---|
110 | int i, j;
|
---|
111 | double dst[4][4];
|
---|
112 | double tmp[12];
|
---|
113 |
|
---|
114 | // calculate pairs for first 8 mements (cofactors)
|
---|
115 | tmp[0] = m[10] * m[15];
|
---|
116 | tmp[1] = m[11] * m[14];
|
---|
117 | tmp[2] = m[9] * m[15];
|
---|
118 | tmp[3] = m[11] * m[13];
|
---|
119 | tmp[4] = m[9] * m[14];
|
---|
120 | tmp[5] = m[10] * m[13];
|
---|
121 | tmp[6] = m[8] * m[15];
|
---|
122 | tmp[7] = m[11] * m[12];
|
---|
123 | tmp[8] = m[8] * m[14];
|
---|
124 | tmp[9] = m[10] * m[12];
|
---|
125 | tmp[10] = m[8] * m[13];
|
---|
126 | tmp[11] = m[9] * m[12];
|
---|
127 |
|
---|
128 | // calculate first 8 mements (cofactors)
|
---|
129 | dst[0][0] = tmp[0]*m[5] + tmp[3]*m[6] + tmp[4]*m[7];
|
---|
130 | dst[0][0] -= tmp[1]*m[5] + tmp[2]*m[6] + tmp[5]*m[7];
|
---|
131 | dst[0][1] = tmp[1]*m[4] + tmp[6]*m[6] + tmp[9]*m[7];
|
---|
132 | dst[0][1]-= tmp[0]*m[4] + tmp[7]*m[6] + tmp[8]*m[7];
|
---|
133 | dst[0][2] = tmp[2]*m[4] + tmp[7]*m[5] + tmp[10]*m[7];
|
---|
134 | dst[0][2] -= tmp[3]*m[4] + tmp[6]*m[5] + tmp[11]*m[7];
|
---|
135 | dst[0][3] = tmp[5]*m[4] + tmp[8]*m[5] + tmp[11]*m[6];
|
---|
136 | dst[0][3] -= tmp[4]*m[4] + tmp[9]*m[5] + tmp[10]*m[6];
|
---|
137 | dst[1][0] = tmp[1]*m[1] + tmp[2]*m[2] + tmp[5]*m[3];
|
---|
138 | dst[1][0] -= tmp[0]*m[1] + tmp[3]*m[2] + tmp[4]*m[3];
|
---|
139 | dst[1][1] = tmp[0]*m[0] + tmp[7]*m[2] + tmp[8]*m[3];
|
---|
140 | dst[1][1] -= tmp[1]*m[0] + tmp[6]*m[2] + tmp[9]*m[3];
|
---|
141 | dst[1][2] = tmp[3]*m[0] + tmp[6]*m[1] + tmp[11]*m[3];
|
---|
142 | dst[1][2] -= tmp[2]*m[0] + tmp[7]*m[1] + tmp[10]*m[3];
|
---|
143 | dst[1][3] = tmp[4]*m[0] + tmp[9]*m[1] + tmp[10]*m[2];
|
---|
144 | dst[1][3] -= tmp[5]*m[0] + tmp[8]*m[1] + tmp[11]*m[2];
|
---|
145 |
|
---|
146 | // calculate pairs for second 8 mements (cofactors)
|
---|
147 | tmp[0] = m[2]*m[7];
|
---|
148 | tmp[1] = m[3]*m[6];
|
---|
149 | tmp[2] = m[1]*m[7];
|
---|
150 | tmp[3] = m[3]*m[5];
|
---|
151 | tmp[4] = m[1]*m[6];
|
---|
152 | tmp[5] = m[2]*m[5];
|
---|
153 | tmp[6] = m[0]*m[7];
|
---|
154 | tmp[7] = m[3]*m[4];
|
---|
155 | tmp[8] = m[0]*m[6];
|
---|
156 | tmp[9] = m[2]*m[4];
|
---|
157 | tmp[10] = m[0]*m[5];
|
---|
158 | tmp[11] = m[1]*m[4];
|
---|
159 |
|
---|
160 | // calculate second 8 mements (cofactors)
|
---|
161 | dst[2][0] = tmp[0]*m[13] + tmp[3]*m[14] + tmp[4]*m[15];
|
---|
162 | dst[2][0] -= tmp[1]*m[13] + tmp[2]*m[14] + tmp[5]*m[15];
|
---|
163 | dst[2][1] = tmp[1]*m[12] + tmp[6]*m[14] + tmp[9]*m[15];
|
---|
164 | dst[2][1] -= tmp[0]*m[12] + tmp[7]*m[14] + tmp[8]*m[15];
|
---|
165 | dst[2][2] = tmp[2]*m[12] + tmp[7]*m[13] + tmp[10]*m[15];
|
---|
166 | dst[2][2]-= tmp[3]*m[12] + tmp[6]*m[13] + tmp[11]*m[15];
|
---|
167 | dst[2][3] = tmp[5]*m[12] + tmp[8]*m[13] + tmp[11]*m[14];
|
---|
168 | dst[2][3]-= tmp[4]*m[12] + tmp[9]*m[13] + tmp[10]*m[14];
|
---|
169 | dst[3][0] = tmp[2]*m[10] + tmp[5]*m[11] + tmp[1]*m[9];
|
---|
170 | dst[3][0]-= tmp[4]*m[11] + tmp[0]*m[9] + tmp[3]*m[10];
|
---|
171 | dst[3][1] = tmp[8]*m[11] + tmp[0]*m[8] + tmp[7]*m[10];
|
---|
172 | dst[3][1]-= tmp[6]*m[10] + tmp[9]*m[11] + tmp[1]*m[8];
|
---|
173 | dst[3][2] = tmp[6]*m[9] + tmp[11]*m[11] + tmp[3]*m[8];
|
---|
174 | dst[3][2]-= tmp[10]*m[11] + tmp[2]*m[8] + tmp[7]*m[9];
|
---|
175 | dst[3][3] = tmp[10]*m[10] + tmp[4]*m[8] + tmp[9]*m[9];
|
---|
176 | dst[3][3]-= tmp[8]*m[9] + tmp[11]*m[10] + tmp[5]*m[8];
|
---|
177 |
|
---|
178 | // calculate determinant
|
---|
179 | double determinant=m[0]*dst[0][0]+m[1]*dst[0][1]+m[2]*dst[0][2]+m[3]*dst[0][3];
|
---|
180 |
|
---|
181 | assert(fabs(determinant) != 0.0);
|
---|
182 |
|
---|
183 | // calculate matrix inverse
|
---|
184 | determinant = 1/determinant;
|
---|
185 | for(i = 0; i < 4; i++){
|
---|
186 | for (j = 0; j < 4; j++){
|
---|
187 | dst[i][j] *= determinant;
|
---|
188 | ret.m[i*4+j] = dst[i][j];
|
---|
189 | }
|
---|
190 | }
|
---|
191 |
|
---|
192 | return ret;
|
---|
193 | */
|
---|
194 | }
|
---|
195 |
|
---|
196 | Vector4 Mat4x4::multiply_row_vector(Vector4 v){
|
---|
197 | Vector4 ret;
|
---|
198 | ret.x = m[0]*v.x + m[1]*v.y + m[2]*v.z + m[3]*v.w;
|
---|
199 | ret.y = m[4]*v.x + m[5]*v.y + m[6]*v.z + m[7]*v.w;
|
---|
200 | ret.z = m[8]*v.x + m[9]*v.y + m[10]*v.z + m[11]*v.w;
|
---|
201 | ret.w = m[12]*v.x + m[13]*v.y + m[14]*v.z + m[15]*v.w;
|
---|
202 | return ret;
|
---|
203 | }
|
---|
204 |
|
---|
205 | Vector4 Mat4x4::transform(Vector4 vector){
|
---|
206 | Vector4 ret;
|
---|
207 | ret.x = vector.x*m[0]+vector.y*m[4]+vector.z*m[8]+vector.w*m[12];
|
---|
208 | ret.y = vector.x*m[1]+vector.y*m[5]+vector.z*m[9]+vector.w*m[13];
|
---|
209 | ret.z = vector.x*m[2]+vector.y*m[6]+vector.z*m[10]+vector.w*m[14];
|
---|
210 | ret.w = vector.x*m[3]+vector.y*m[7]+vector.z*m[11]+vector.w*m[15];
|
---|
211 |
|
---|
212 | return ret;
|
---|
213 | }
|
---|
214 |
|
---|
215 | Mat4x4 Mat4x4::operator *(Mat4x4 mat){
|
---|
216 | Mat4x4 ret;
|
---|
217 |
|
---|
218 | ret.m[0] = m[0]*mat.m[0]+m[1]*mat.m[4]+m[2]*mat.m[8]+m[3]*mat.m[12];
|
---|
219 | ret.m[1] = m[0]*mat.m[1]+m[1]*mat.m[5]+m[2]*mat.m[9]+m[3]*mat.m[13];
|
---|
220 | ret.m[2] = m[0]*mat.m[2]+m[1]*mat.m[6]+m[2]*mat.m[10]+m[3]*mat.m[14];
|
---|
221 | ret.m[3] = m[0]*mat.m[3]+m[1]*mat.m[7]+m[2]*mat.m[11]+m[3]*mat.m[15];
|
---|
222 |
|
---|
223 | ret.m[4] = m[4]*mat.m[0]+m[5]*mat.m[4]+m[6]*mat.m[8]+m[7]*mat.m[12];
|
---|
224 | ret.m[5] = m[4]*mat.m[1]+m[5]*mat.m[5]+m[6]*mat.m[9]+m[7]*mat.m[13];
|
---|
225 | ret.m[6] = m[4]*mat.m[2]+m[5]*mat.m[6]+m[6]*mat.m[10]+m[7]*mat.m[14];
|
---|
226 | ret.m[7] = m[4]*mat.m[3]+m[5]*mat.m[7]+m[6]*mat.m[11]+m[7]*mat.m[15];
|
---|
227 |
|
---|
228 | ret.m[8] = m[8]*mat.m[0]+m[9]*mat.m[4]+m[10]*mat.m[8]+m[11]*mat.m[12];
|
---|
229 | ret.m[9] = m[8]*mat.m[1]+m[9]*mat.m[5]+m[10]*mat.m[9]+m[11]*mat.m[13];
|
---|
230 | ret.m[10] = m[8]*mat.m[2]+m[9]*mat.m[6]+m[10]*mat.m[10]+m[11]*mat.m[14];
|
---|
231 | ret.m[11] = m[8]*mat.m[3]+m[9]*mat.m[7]+m[10]*mat.m[11]+m[11]*mat.m[15];
|
---|
232 |
|
---|
233 | ret.m[12] = m[12]*mat.m[0]+m[13]*mat.m[4]+m[14]*mat.m[8]+m[15]*mat.m[12];
|
---|
234 | ret.m[13] = m[12]*mat.m[1]+m[13]*mat.m[5]+m[14]*mat.m[9]+m[15]*mat.m[13];
|
---|
235 | ret.m[14] = m[12]*mat.m[2]+m[13]*mat.m[6]+m[14]*mat.m[10]+m[15]*mat.m[14];
|
---|
236 | ret.m[15] = m[12]*mat.m[3]+m[13]*mat.m[7]+m[14]*mat.m[11]+m[15]*mat.m[15];
|
---|
237 |
|
---|
238 | return ret;
|
---|
239 | }
|
---|
240 |
|
---|
241 | Mat4x4 Mat4x4::transpose(){
|
---|
242 | Mat4x4 ret;
|
---|
243 | for(int i=0; i<4; i++){
|
---|
244 | for(int j=0; j<4; j++){
|
---|
245 | ret.m[j+4*i] = this->m[i+4*j];
|
---|
246 | }
|
---|
247 | }
|
---|
248 | return ret;
|
---|
249 | }
|
---|
250 |
|
---|
251 |
|
---|