1 |
2 |
specular |
//=======================================================================
|
2 |
|
|
// Project Monophony
|
3 |
|
|
// Wire-Frame 3D Graphics Accelerator IP Core
|
4 |
|
|
//
|
5 |
|
|
// File:
|
6 |
|
|
// mp_matrix3.c
|
7 |
|
|
//
|
8 |
|
|
// Abstract:
|
9 |
|
|
// matrix3 class implementation
|
10 |
|
|
//
|
11 |
|
|
// Author:
|
12 |
4 |
specular |
// Kenji Ishimaru (info.wf3d@gmail.com)
|
13 |
2 |
specular |
//
|
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 |
|
|
#include <math.h>
|
43 |
|
|
#include "mp_matrix3.h"
|
44 |
|
|
|
45 |
|
|
void mp_matrix3_init(mp_matrix3 *dst) {
|
46 |
|
|
int i;
|
47 |
|
|
for (i=0;i<9;i++) dst->a[i] = 0.0;
|
48 |
|
|
dst->a[0] = dst->a[4] = dst->a[8] = 1.0;
|
49 |
|
|
}
|
50 |
|
|
|
51 |
|
|
void mp_matrix3_set0(mp_matrix3 *dst, float b[]) {
|
52 |
|
|
int i;
|
53 |
|
|
for (i=0;i<9;i++) dst->a[i] = b[i];
|
54 |
|
|
}
|
55 |
|
|
|
56 |
|
|
void mp_matrix3_set1(mp_matrix3 *dst,
|
57 |
|
|
float a0, float a1, float a2,
|
58 |
|
|
float a3, float a4, float a5,
|
59 |
|
|
float a6, float a7, float a8 ) {
|
60 |
|
|
dst->a[0] = a0; dst->a[1] = a1; dst->a[2] = a2;
|
61 |
|
|
dst->a[3] = a3; dst->a[4] = a4; dst->a[5] = a5;
|
62 |
|
|
dst->a[6] = a6; dst->a[7] = a7; dst->a[8] = a8;
|
63 |
|
|
|
64 |
|
|
}
|
65 |
|
|
|
66 |
|
|
void mp_matrix3_set2(mp_matrix3 *dst, mp_matrix3 *t) {
|
67 |
|
|
int i;
|
68 |
|
|
for (i=0;i<9;i++) dst->a[i] = t->a[i];
|
69 |
|
|
}
|
70 |
|
|
|
71 |
|
|
float mp_matrix3_determinant0(mp_matrix3 *dst) {
|
72 |
|
|
float t =
|
73 |
|
|
dst->a[0]*dst->a[4]*dst->a[8] +
|
74 |
|
|
dst->a[1]*dst->a[5]*dst->a[6] +
|
75 |
|
|
dst->a[2]*dst->a[7]*dst->a[3] -
|
76 |
|
|
dst->a[6]*dst->a[4]*dst->a[2] -
|
77 |
|
|
dst->a[5]*dst->a[7]*dst->a[0] -
|
78 |
|
|
dst->a[8]*dst->a[3]*dst->a[1];
|
79 |
|
|
return t;
|
80 |
|
|
}
|
81 |
|
|
|
82 |
|
|
float mp_matrix3_determinant1(
|
83 |
|
|
float a0, float a1, float a2,
|
84 |
|
|
float a3, float a4, float a5,
|
85 |
|
|
float a6, float a7, float a8) {
|
86 |
|
|
float t =
|
87 |
|
|
a0*a4*a8 +
|
88 |
|
|
a1*a5*a6 +
|
89 |
|
|
a2*a7*a3 -
|
90 |
|
|
a6*a4*a2 -
|
91 |
|
|
a5*a7*a0 -
|
92 |
|
|
a8*a3*a1;
|
93 |
|
|
return t;
|
94 |
|
|
}
|
95 |
|
|
|
96 |
|
|
void mp_matrix3_multiply_vector30(mp_vector3 *dst, mp_matrix3 *m, mp_vector3 *v) {
|
97 |
|
|
dst->x = m->a[0]*v->x + m->a[1]*v->y + m->a[2]*v->z;
|
98 |
|
|
dst->y = m->a[3]*v->x + m->a[4]*v->y + m->a[5]*v->z;
|
99 |
|
|
dst->z = m->a[6]*v->x + m->a[7]*v->y + m->a[8]*v->z;
|
100 |
|
|
}
|
101 |
|
|
|
102 |
|
|
void mp_matrix3_multiply(mp_matrix3 *dst, mp_matrix3 *b) {
|
103 |
|
|
mp_matrix3 c;
|
104 |
|
|
int i,j,k,p,q,r,s;
|
105 |
|
|
for ( k=0 , p=0, q=0 ; k < 3 ; k++, q += 3 ){
|
106 |
|
|
for ( j=0 ; j < 3 ; j++ ){
|
107 |
|
|
r = q;
|
108 |
|
|
s = j;
|
109 |
|
|
c.a[p] = 0;
|
110 |
|
|
for ( i=0 ; i < 3 ; i++, r++, s += 3 ){
|
111 |
|
|
c.a[p] += b->a[r] * dst->a[s];
|
112 |
|
|
}
|
113 |
|
|
p++;
|
114 |
|
|
}
|
115 |
|
|
}
|
116 |
|
|
for (k = 0; k < 9 ; k++) dst->a[k] = c.a[k];
|
117 |
|
|
}
|
118 |
|
|
|
119 |
|
|
void mp_matrix3_inverse(mp_matrix3 *dst) {
|
120 |
|
|
float t = mp_matrix3_determinant0(dst);
|
121 |
|
|
t = 1.0F/t;
|
122 |
|
|
float b[9];
|
123 |
|
|
b[0] = (dst->a[4]*dst->a[8]-dst->a[5]*dst->a[7])*t;
|
124 |
|
|
b[1] = -(dst->a[1]*dst->a[8]-dst->a[7]*dst->a[2])*t;
|
125 |
|
|
b[2] = (dst->a[1]*dst->a[5]-dst->a[2]*dst->a[4])*t;
|
126 |
|
|
b[3] = -(dst->a[3]*dst->a[8]-dst->a[5]*dst->a[6])*t;
|
127 |
|
|
b[4] = (dst->a[0]*dst->a[8]-dst->a[2]*dst->a[6])*t;
|
128 |
|
|
b[5] = -(dst->a[0]*dst->a[5]-dst->a[2]*dst->a[3])*t;
|
129 |
|
|
b[6] = (dst->a[3]*dst->a[7]-dst->a[4]*dst->a[6])*t;
|
130 |
|
|
b[7] = -(dst->a[0]*dst->a[7]-dst->a[1]*dst->a[6])*t;
|
131 |
|
|
b[8] = (dst->a[0]*dst->a[4]-dst->a[1]*dst->a[3])*t;
|
132 |
|
|
|
133 |
|
|
mp_matrix3_set0(dst,b);
|
134 |
|
|
}
|
135 |
|
|
|
136 |
|
|
void mp_matrix3_rotate0(mp_matrix3 *dst, mp_vector3 *v, float theta) {
|
137 |
|
|
mp_vector3 w;
|
138 |
|
|
mp_vector3_assign0(&w,v);
|
139 |
|
|
mp_vector3_normalize(&w);
|
140 |
|
|
|
141 |
|
|
float ss = sin(theta);
|
142 |
|
|
float cc = cos(theta);
|
143 |
|
|
float s = sin(theta);
|
144 |
|
|
float c = cos(theta);
|
145 |
|
|
float t = 1-c;
|
146 |
|
|
|
147 |
|
|
dst->a[0] = t*w.x*w.x + c;
|
148 |
|
|
dst->a[1] = t*w.x*w.y - w.z*s;
|
149 |
|
|
dst->a[2] = t*w.z*w.x + w.y*s;
|
150 |
|
|
dst->a[3] = t*w.x*w.y + w.z*s;
|
151 |
|
|
dst->a[4] = t*w.y*w.y + c;
|
152 |
|
|
dst->a[5] = t*w.y*w.z - w.x*s;
|
153 |
|
|
dst->a[6] = t*w.z*w.x - w.y*s;
|
154 |
|
|
dst->a[7] = t*w.y*w.z + w.x*s;
|
155 |
|
|
dst->a[8] = t*w.z*w.z + c;
|
156 |
|
|
|
157 |
|
|
}
|
158 |
|
|
|
159 |
|
|
void mp_matrix3_rotate1(mp_matrix3 *dst, float dx, float dy,
|
160 |
|
|
float dz, float theta) {
|
161 |
|
|
mp_vector3 v;
|
162 |
|
|
mp_vector3_set0(&v, dx,dy,dz);
|
163 |
|
|
mp_matrix3_rotate0(dst, &v,theta);
|
164 |
|
|
}
|
165 |
|
|
|
166 |
|
|
void mp_matrix3_rotate2(mp_matrix3 *dst, mp_vector4 *v) {
|
167 |
|
|
mp_matrix3_rotate1(dst, v->x,v->y,v->z,v->w);
|
168 |
|
|
}
|
169 |
|
|
|