1 |
4 |
specular |
//=======================================================================
|
2 |
|
|
// Project Monophony
|
3 |
|
|
// Wire-Frame 3D Graphics Accelerator IP Core
|
4 |
|
|
//
|
5 |
|
|
// File:
|
6 |
|
|
// mp_matrix4.c
|
7 |
|
|
//
|
8 |
|
|
// Abstract:
|
9 |
|
|
// matrix4 class implementation
|
10 |
|
|
//
|
11 |
|
|
// Author:
|
12 |
|
|
// Kenji Ishimaru (info.wf3d@gmail.com)
|
13 |
|
|
//
|
14 |
|
|
//======================================================================
|
15 |
|
|
//
|
16 |
|
|
// Copyright (c) 2015, Kenji Ishimaru
|
17 |
|
|
// All rights reserved.
|
18 |
|
|
//
|
19 |
|
|
// Redistribution and use in source and binary forms, with or without
|
20 |
|
|
// modification, are permitted provided that the following conditions are met:
|
21 |
|
|
//
|
22 |
|
|
// -Redistributions of source code must retain the above copyright notice,
|
23 |
|
|
// this list of conditions and the following disclaimer.
|
24 |
|
|
// -Redistributions in binary form must reproduce the above copyright notice,
|
25 |
|
|
// this list of conditions and the following disclaimer in the documentation
|
26 |
|
|
// and/or other materials provided with the distribution.
|
27 |
|
|
//
|
28 |
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
29 |
|
|
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
30 |
|
|
// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
31 |
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
32 |
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
33 |
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
34 |
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
35 |
|
|
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
36 |
|
|
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
37 |
|
|
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
38 |
|
|
// EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
39 |
|
|
//
|
40 |
|
|
// Revision History
|
41 |
|
|
|
42 |
2 |
specular |
#include <math.h>
|
43 |
|
|
#include "mp_matrix4.h"
|
44 |
|
|
#include "mp_lib.h"
|
45 |
|
|
|
46 |
|
|
extern t_fui u_fui;
|
47 |
|
|
|
48 |
|
|
static int TR[16] = { 0,4,8,12,1,5,9,13,2,6,10,14,3,7,11,15 };
|
49 |
|
|
static int AA[16][16] = {
|
50 |
|
|
/* aa00 */ {5,6,7,9,10,11,13,14,15},
|
51 |
|
|
/* aa10 */ {1,2,3,9,10,11,13,14,15},
|
52 |
|
|
/* aa20 */ {1,2,3,5,6,7,13,14,15},
|
53 |
|
|
/* aa30 */ {1,2,3,5,6,7,9,10,11},
|
54 |
|
|
/* aa01 */ {4,6,7,8,10,11,12,14,15},
|
55 |
|
|
/* aa11 */ {0,2,3,8,10,11,12,14,15},
|
56 |
|
|
/* aa21 */ {0,2,3,4,6,7,12,14,15},
|
57 |
|
|
/* aa31 */ {0,2,3,4,6,7,8,10,11},
|
58 |
|
|
/* aa02 */ {4,5,7,8,9,11,12,13,15},
|
59 |
|
|
/* aa12 */ {0,1,3,8,9,11,12,13,15},
|
60 |
|
|
/* aa22 */ {0,1,3,4,5,7,12,13,15},
|
61 |
|
|
/* aa32 */ {0,1,3,4,5,7,8,9,11},
|
62 |
|
|
/* aa03 */ {4,5,6,8,9,10,12,13,14},
|
63 |
|
|
/* aa13 */ {0,1,2,8,9,10,12,13,14},
|
64 |
|
|
/* aa23 */ {0,1,2,4,5,6,12,13,14},
|
65 |
|
|
/* aa33 */ {0,1,2,4,5,6,8,9,10}};
|
66 |
|
|
|
67 |
|
|
void mp_matrix4_init(mp_matrix4 *dst) {
|
68 |
|
|
int i;
|
69 |
|
|
for (i=0;i<16;i++) dst->a[i] = 0.0;
|
70 |
|
|
dst->a[0] = dst->a[5] = dst->a[10] = dst->a[15] = 1.0;
|
71 |
|
|
}
|
72 |
|
|
|
73 |
|
|
void mp_matrix4_set0(mp_matrix4 *dst, float b[]) {
|
74 |
|
|
int i;
|
75 |
|
|
for (i=0;i<16;i++) dst->a[i] = b[i];
|
76 |
|
|
}
|
77 |
|
|
|
78 |
|
|
void mp_matrix4_set0_t(mp_matrix4 *dst, float b[]) {
|
79 |
|
|
int i;
|
80 |
|
|
for (i=0;i<16;i++) dst->a[TR[i]] = b[i];
|
81 |
|
|
}
|
82 |
|
|
|
83 |
|
|
void mp_matrix4_set1(mp_matrix4 *dst, unsigned int b[]) {
|
84 |
|
|
int i;
|
85 |
|
|
for (i = 0; i <16; i++) {
|
86 |
|
|
u_fui.ui= b[i];
|
87 |
|
|
dst->a[i] = u_fui.f;
|
88 |
|
|
}
|
89 |
|
|
}
|
90 |
|
|
|
91 |
|
|
void mp_matrix4_set2(mp_matrix4 *dst, mp_matrix4 *t) {
|
92 |
|
|
int i;
|
93 |
|
|
for (i=0;i<16;i++) dst->a[i] = t->a[i];
|
94 |
|
|
}
|
95 |
|
|
|
96 |
|
|
void mp_matrix4_identity(mp_matrix4 *dst) {
|
97 |
|
|
mp_matrix4_init(dst);
|
98 |
|
|
}
|
99 |
|
|
|
100 |
|
|
void mp_matrix4_assign(mp_matrix4 *dst, mp_matrix4 *v) {
|
101 |
|
|
int i;
|
102 |
|
|
for (i=0 ; i < 16 ; i++ ) dst->a[i] = v->a[i];
|
103 |
|
|
}
|
104 |
|
|
|
105 |
|
|
void mp_matrix4_multiply_vector4(mp_vector4 *dst, mp_matrix4 *m, mp_vector4 *v) {
|
106 |
|
|
mp_vector4 t;
|
107 |
|
|
mp_vector4_set2(&t, v);
|
108 |
|
|
dst->x = m->a[0]*t.x + m->a[1]*t.y + m->a[2]*t.z + m->a[3]*t.w;
|
109 |
|
|
dst->y = m->a[4]*t.x + m->a[5]*t.y + m->a[6]*t.z + m->a[7]*t.w;
|
110 |
|
|
dst->z = m->a[8]*t.x + m->a[9]*t.y + m->a[10]*t.z + m->a[11]*t.w;
|
111 |
|
|
dst->w = m->a[12]*t.x + m->a[13]*t.y + m->a[14]*t.z + m->a[15]*t.w;
|
112 |
|
|
}
|
113 |
|
|
|
114 |
|
|
void mp_matrix4_multiply_f(mp_matrix4 *m, float *v) {
|
115 |
|
|
float r[4];
|
116 |
|
|
r[0] = m->a[0]*v[0] + m->a[1]*v[1] + m->a[2]*v[2] + m->a[3]*v[3];
|
117 |
|
|
r[1] = m->a[4]*v[0] + m->a[5]*v[1] + m->a[6]*v[2] + m->a[7]*v[3];
|
118 |
|
|
r[2] = m->a[8]*v[0] + m->a[9]*v[1] + m->a[10]*v[2] + m->a[11]*v[3];
|
119 |
|
|
r[3] = m->a[12]*v[0] + m->a[13]*v[1] + m->a[14]*v[2] + m->a[15]*v[3];
|
120 |
|
|
v[0] = r[0];
|
121 |
|
|
v[1] = r[1];
|
122 |
|
|
v[2] = r[2];
|
123 |
|
|
v[3] = r[3];
|
124 |
|
|
}
|
125 |
|
|
|
126 |
|
|
void mp_matrix4_multiply_f2(mp_matrix4 *m, float *dst, float *src) {
|
127 |
|
|
// w is always 1.0
|
128 |
|
|
dst[0] = m->a[0]*src[0] + m->a[1]*src[1] + m->a[2]*src[2] + m->a[3];
|
129 |
|
|
dst[1] = m->a[4]*src[0] + m->a[5]*src[1] + m->a[6]*src[2] + m->a[7];
|
130 |
|
|
dst[2] = m->a[8]*src[0] + m->a[9]*src[1] + m->a[10]*src[2] + m->a[11];
|
131 |
|
|
dst[3] = m->a[12]*src[0] + m->a[13]*src[1] + m->a[14]*src[2] + m->a[15];
|
132 |
|
|
}
|
133 |
|
|
|
134 |
|
|
void mp_matrix4_multiply_matrix4(mp_matrix4 *dst, mp_matrix4 *mm, mp_matrix4 *b) {
|
135 |
|
|
int i,j,k,m,n,p,r,s;
|
136 |
|
|
mp_matrix4 c;
|
137 |
|
|
for ( i = r = 0 ; i < 4 ; i ++, r += 4 ) {
|
138 |
|
|
for ( j = 0 ; j < 4 ; j ++ ) {
|
139 |
|
|
m = r + j;
|
140 |
|
|
c.a[m] = 0.0;
|
141 |
|
|
for ( k = s = 0 ; k < 4 ; k++, s += 4 ){
|
142 |
|
|
n = r + k;
|
143 |
|
|
p = s + j;
|
144 |
|
|
c.a[m] += b->a[n] * mm->a[p];
|
145 |
|
|
}
|
146 |
|
|
}
|
147 |
|
|
}
|
148 |
|
|
for ( k = 0 ; k < 16 ; k++ ) dst->a[k] = c.a[k];
|
149 |
|
|
}
|
150 |
|
|
|
151 |
|
|
void mp_matrix4_multiply_matrix4i(mp_matrix4 *dst, mp_matrix4 *mm, mp_matrix4 *b) {
|
152 |
|
|
int i,j,k,m,n,p,r,s;
|
153 |
|
|
mp_matrix4 c;
|
154 |
|
|
for ( i = r = 0 ; i < 4 ; i ++, r += 4 ) {
|
155 |
|
|
for ( j = 0 ; j < 4 ; j ++ ) {
|
156 |
|
|
m = r + j;
|
157 |
|
|
c.a[m] = 0.0;
|
158 |
|
|
for ( k = s = 0 ; k < 4 ; k++, s += 4 ){
|
159 |
|
|
n = r + k;
|
160 |
|
|
p = s + j;
|
161 |
|
|
c.a[m] += mm->a[n] * b->a[p];
|
162 |
|
|
}
|
163 |
|
|
}
|
164 |
|
|
}
|
165 |
|
|
for ( k = 0 ; k < 16 ; k++ ) dst->a[k] = c.a[k];
|
166 |
|
|
}
|
167 |
|
|
|
168 |
|
|
void mp_matrix4_transpose(mp_matrix4 *dst,mp_matrix4 *src) {
|
169 |
|
|
int i;
|
170 |
|
|
for (i = 0 ; i < 16 ; i++ ){
|
171 |
|
|
dst->a[i] = src->a[TR[i]];
|
172 |
|
|
}
|
173 |
|
|
}
|
174 |
|
|
|
175 |
|
|
void mp_matrix4_inverse(mp_matrix4 *dst,mp_matrix4 *src) {
|
176 |
|
|
float determinant4, determinant3;
|
177 |
|
|
float v[9];
|
178 |
|
|
|
179 |
|
|
int m,n,sign;
|
180 |
|
|
int i,j;
|
181 |
|
|
mp_matrix3 w;
|
182 |
|
|
for (i=0 ; i < 16 ; i++ ){
|
183 |
|
|
m = (i+1)/4 + 1; n = (i+1)%4;
|
184 |
|
|
if (n==0) { n = 4; m--; }
|
185 |
|
|
if ((m+n)%2 == 1) sign = -1; else sign = 1;
|
186 |
|
|
for (j=0 ; j < 9 ; j++ ) v[j] = src->a[AA[i][j]];
|
187 |
|
|
mp_matrix3_set0(&w, v);
|
188 |
|
|
determinant3 = mp_matrix3_determinant0(&w);
|
189 |
|
|
dst->a[i] = sign * determinant3;
|
190 |
|
|
}
|
191 |
|
|
determinant4 =
|
192 |
|
|
src->a[0]*dst->a[0]
|
193 |
|
|
+ src->a[4]*dst->a[1]
|
194 |
|
|
+ src->a[8]*dst->a[2]
|
195 |
|
|
+ src->a[12]*dst->a[3];
|
196 |
|
|
|
197 |
|
|
determinant4 = 1.0F / determinant4;
|
198 |
|
|
for (i=0 ; i < 16 ; i++ ) dst->a[i] *= determinant4;
|
199 |
|
|
}
|
200 |
|
|
|
201 |
|
|
void mp_matrix4_rotate(mp_matrix4 *dst, mp_vector3 *v, float theta) {
|
202 |
|
|
mp_matrix3 m3a;
|
203 |
|
|
mp_matrix3_init(&m3a);
|
204 |
|
|
mp_matrix3_rotate0(&m3a, v, theta);
|
205 |
|
|
mp_matrix4_init(dst);
|
206 |
|
|
dst->a[0] = m3a.a[0];
|
207 |
|
|
dst->a[1] = m3a.a[1];
|
208 |
|
|
dst->a[2] = m3a.a[2];
|
209 |
|
|
dst->a[4] = m3a.a[3];
|
210 |
|
|
dst->a[5] = m3a.a[4];
|
211 |
|
|
dst->a[6] = m3a.a[5];
|
212 |
|
|
dst->a[8] = m3a.a[6];
|
213 |
|
|
dst->a[9] = m3a.a[7];
|
214 |
|
|
dst->a[10] = m3a.a[8];
|
215 |
|
|
}
|
216 |
|
|
|
217 |
|
|
void mp_matrix4_get_elements(mp_matrix4 *dst, unsigned int *b) {
|
218 |
|
|
int i;
|
219 |
|
|
for (i = 0; i <16; i++) {
|
220 |
|
|
u_fui.f= dst->a[i];
|
221 |
|
|
b[i] = u_fui.ui;
|
222 |
|
|
}
|
223 |
|
|
}
|
224 |
|
|
|