1 /*
2 * This program is free software; you can redistribute it and/or
3 * modify it under the terms of the GNU General Public License
4 * as published by the Free Software Foundation; either version 2
5 * of the License, or (at your option) any later version.
6 *
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
11 *
12 * You should have received a copy of the GNU General Public License
13 * along with this program; if not, write to the Free Software Foundation,
14 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15 *
16 * The Original Code is Copyright (C) 2008 Blender Foundation.
17 * All rights reserved.
18 */
19
20 #include "BLI_utildefines.h"
21
22 #include "BCMath.h"
23 #include "BlenderContext.h"
24
rotate_to(Matrix & mat_to)25 void BCQuat::rotate_to(Matrix &mat_to)
26 {
27 Quat qd;
28 Matrix matd;
29 Matrix mati;
30 Matrix mat_from;
31
32 quat_to_mat4(mat_from, q);
33
34 /* Calculate the difference matrix matd between mat_from and mat_to */
35 invert_m4_m4(mati, mat_from);
36 mul_m4_m4m4(matd, mati, mat_to);
37
38 mat4_to_quat(qd, matd);
39
40 mul_qt_qtqt(q, qd, q); /* rotate to the final rotation to mat_to */
41 }
42
BCMatrix(const BCMatrix & mat)43 BCMatrix::BCMatrix(const BCMatrix &mat)
44 {
45 set_transform(mat.matrix);
46 }
47
BCMatrix(Matrix & mat)48 BCMatrix::BCMatrix(Matrix &mat)
49 {
50 set_transform(mat);
51 }
52
BCMatrix(Object * ob)53 BCMatrix::BCMatrix(Object *ob)
54 {
55 set_transform(ob);
56 }
57
BCMatrix()58 BCMatrix::BCMatrix()
59 {
60 unit();
61 }
62
BCMatrix(BC_global_forward_axis global_forward_axis,BC_global_up_axis global_up_axis)63 BCMatrix::BCMatrix(BC_global_forward_axis global_forward_axis, BC_global_up_axis global_up_axis)
64 {
65 float mrot[3][3];
66 float mat[4][4];
67 mat3_from_axis_conversion(
68 BC_DEFAULT_FORWARD, BC_DEFAULT_UP, global_forward_axis, global_up_axis, mrot);
69
70 transpose_m3(
71 mrot); /* TODO: Verify that mat3_from_axis_conversion() returns a transposed matrix */
72 copy_m4_m3(mat, mrot);
73 set_transform(mat);
74 }
75
add_transform(const Matrix & mat,bool inverted)76 void BCMatrix::add_transform(const Matrix &mat, bool inverted)
77 {
78 add_transform(this->matrix, mat, this->matrix, inverted);
79 }
80
add_transform(const BCMatrix & mat,bool inverted)81 void BCMatrix::add_transform(const BCMatrix &mat, bool inverted)
82 {
83 add_transform(this->matrix, mat.matrix, this->matrix, inverted);
84 }
85
apply_transform(const BCMatrix & mat,bool inverted)86 void BCMatrix::apply_transform(const BCMatrix &mat, bool inverted)
87 {
88 apply_transform(this->matrix, mat.matrix, this->matrix, inverted);
89 }
90
add_transform(Matrix & to,const Matrix & transform,const Matrix & from,bool inverted)91 void BCMatrix::add_transform(Matrix &to,
92 const Matrix &transform,
93 const Matrix &from,
94 bool inverted)
95 {
96 if (inverted) {
97 Matrix globinv;
98 invert_m4_m4(globinv, transform);
99 add_transform(to, globinv, from, /*inverted=*/false);
100 }
101 else {
102 mul_m4_m4m4(to, transform, from);
103 }
104 }
105
apply_transform(Matrix & to,const Matrix & transform,const Matrix & from,bool inverse)106 void BCMatrix::apply_transform(Matrix &to,
107 const Matrix &transform,
108 const Matrix &from,
109 bool inverse)
110 {
111 Matrix globinv;
112 invert_m4_m4(globinv, transform);
113 if (inverse) {
114 add_transform(to, globinv, from, /*inverted=*/false);
115 }
116 else {
117 mul_m4_m4m4(to, transform, from);
118 mul_m4_m4m4(to, to, globinv);
119 }
120 }
121
add_inverted_transform(Matrix & to,const Matrix & transform,const Matrix & from)122 void BCMatrix::add_inverted_transform(Matrix &to, const Matrix &transform, const Matrix &from)
123 {
124 Matrix workmat;
125 invert_m4_m4(workmat, transform);
126 mul_m4_m4m4(to, workmat, from);
127 }
128
set_transform(Object * ob)129 void BCMatrix::set_transform(Object *ob)
130 {
131 Matrix lmat;
132
133 BKE_object_matrix_local_get(ob, lmat);
134 copy_m4_m4(matrix, lmat);
135
136 mat4_decompose(this->loc, this->q, this->size, lmat);
137 quat_to_compatible_eul(this->rot, ob->rot, this->q);
138 }
139
set_transform(Matrix & mat)140 void BCMatrix::set_transform(Matrix &mat)
141 {
142 copy_m4_m4(matrix, mat);
143 mat4_decompose(this->loc, this->q, this->size, mat);
144 quat_to_eul(this->rot, this->q);
145 }
146
copy(Matrix & r,Matrix & a)147 void BCMatrix::copy(Matrix &r, Matrix &a)
148 {
149 /* destination comes first: */
150 memcpy(r, a, sizeof(Matrix));
151 }
152
transpose(Matrix & mat)153 void BCMatrix::transpose(Matrix &mat)
154 {
155 transpose_m4(mat);
156 }
157
sanitize(Matrix & mat,int precision)158 void BCMatrix::sanitize(Matrix &mat, int precision)
159 {
160 for (int i = 0; i < 4; i++) {
161 for (int j = 0; j < 4; j++) {
162 double val = (double)mat[i][j];
163 val = double_round(val, precision);
164 mat[i][j] = (float)val;
165 }
166 }
167 }
168
sanitize(DMatrix & mat,int precision)169 void BCMatrix::sanitize(DMatrix &mat, int precision)
170 {
171 for (int i = 0; i < 4; i++) {
172 for (int j = 0; j < 4; j++) {
173 mat[i][j] = double_round(mat[i][j], precision);
174 }
175 }
176 }
177
unit()178 void BCMatrix::unit()
179 {
180 unit_m4(this->matrix);
181 mat4_decompose(this->loc, this->q, this->size, this->matrix);
182 quat_to_eul(this->rot, this->q);
183 }
184
185 /* We need double here because the OpenCollada API needs it.
186 * precision = -1 indicates to not limit the precision. */
get_matrix(DMatrix & mat,const bool transposed,const int precision) const187 void BCMatrix::get_matrix(DMatrix &mat, const bool transposed, const int precision) const
188 {
189 for (int i = 0; i < 4; i++) {
190 for (int j = 0; j < 4; j++) {
191 float val = (transposed) ? matrix[j][i] : matrix[i][j];
192 if (precision >= 0) {
193 val = floor((val * pow(10, precision) + 0.5)) / pow(10, precision);
194 }
195 mat[i][j] = val;
196 }
197 }
198 }
199
get_matrix(Matrix & mat,const bool transposed,const int precision,const bool inverted) const200 void BCMatrix::get_matrix(Matrix &mat,
201 const bool transposed,
202 const int precision,
203 const bool inverted) const
204 {
205 for (int i = 0; i < 4; i++) {
206 for (int j = 0; j < 4; j++) {
207 float val = (transposed) ? matrix[j][i] : matrix[i][j];
208 if (precision >= 0) {
209 val = floor((val * pow(10, precision) + 0.5)) / pow(10, precision);
210 }
211 mat[i][j] = val;
212 }
213 }
214
215 if (inverted) {
216 invert_m4(mat);
217 }
218 }
219
in_range(const BCMatrix & other,float distance) const220 bool BCMatrix::in_range(const BCMatrix &other, float distance) const
221 {
222 for (int i = 0; i < 4; i++) {
223 for (int j = 0; j < 4; j++) {
224 if (fabs(other.matrix[i][j] - matrix[i][j]) > distance) {
225 return false;
226 }
227 }
228 }
229 return true;
230 }
231
232 float (&BCMatrix::location() const)[3]
__anon0d3cc2c60102null233 {
234 return loc;
235 }
236
237 float (&BCMatrix::rotation() const)[3]
__anon0d3cc2c60202null238 {
239 return rot;
240 }
241
242 float (&BCMatrix::scale() const)[3]
__anon0d3cc2c60302null243 {
244 return size;
245 }
246
247 float (&BCMatrix::quat() const)[4]
__anon0d3cc2c60402null248 {
249 return q;
250 }
251