LEFT | RIGHT |
1 /* | 1 /* |
| 2 * $Id$ |
| 3 * |
2 * ***** BEGIN GPL LICENSE BLOCK ***** | 4 * ***** BEGIN GPL LICENSE BLOCK ***** |
3 * | 5 * |
4 * This program is free software; you can redistribute it and/or | 6 * This program is free software; you can redistribute it and/or |
5 * modify it under the terms of the GNU General Public License | 7 * modify it under the terms of the GNU General Public License |
6 * as published by the Free Software Foundation; either version 2 | 8 * as published by the Free Software Foundation; either version 2 |
7 * of the License, or (at your option) any later version. | 9 * of the License, or (at your option) any later version. |
8 * | 10 * |
9 * This program is distributed in the hope that it will be useful, | 11 * This program is distributed in the hope that it will be useful, |
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of | 12 * but WITHOUT ANY WARRANTY; without even the implied warranty of |
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
12 * GNU General Public License for more details. | 14 * GNU General Public License for more details. |
13 * | 15 * |
14 * You should have received a copy of the GNU General Public License | 16 * You should have received a copy of the GNU General Public License |
15 * along with this program; if not, write to the Free Software Foundation, | 17 * along with this program; if not, write to the Free Software Foundation, |
16 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. | 18 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. |
17 * | 19 * |
18 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. | 20 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. |
19 * All rights reserved. | 21 * All rights reserved. |
20 | 22 |
21 * The Original Code is: some of this file. | 23 * The Original Code is: some of this file. |
22 * | 24 * |
23 * ***** END GPL LICENSE BLOCK ***** | 25 * ***** END GPL LICENSE BLOCK ***** |
24 * */ | 26 * */ |
25 | 27 |
26 /** \file blender/blenlib/intern/math_rotation.c | 28 /** \file blender/blenlib/intern/math_rotation.c |
27 * \ingroup bli | 29 * \ingroup bli |
28 */ | 30 */ |
29 | 31 |
30 | 32 |
31 | 33 |
32 #include <assert.h> | 34 #include <assert.h> |
33 #include "BLI_math.h" | 35 #include "BLI_math.h" |
34 | 36 |
35 /******************************** Quaternions ********************************/ | 37 /******************************** Quaternions ********************************/ |
36 | 38 |
37 /* used to test is a quat is not normalized (only used for debug prints) */ | 39 /* used to test is a quat is not normalized */ |
38 #ifdef DEBUG | 40 #define QUAT_EPSILON 0.0001 |
39 # define QUAT_EPSILON 0.0001 | |
40 #endif | |
41 | 41 |
42 /* convenience, avoids setting Y axis everywhere */ | 42 /* convenience, avoids setting Y axis everywhere */ |
43 void unit_axis_angle(float axis[3], float *angle) | 43 void unit_axis_angle(float axis[3], float *angle) |
44 { | 44 { |
45 » axis[0] = 0.0f; | 45 » axis[0]= 0.0f; |
46 » axis[1] = 1.0f; | 46 » axis[1]= 1.0f; |
47 » axis[2] = 0.0f; | 47 » axis[2]= 0.0f; |
48 » *angle = 0.0f; | 48 » *angle= 0.0f; |
49 } | 49 } |
| 50 |
50 | 51 |
51 void unit_qt(float q[4]) | 52 void unit_qt(float q[4]) |
52 { | 53 { |
53 » q[0] = 1.0f; | 54 » q[0]= 1.0f; |
54 » q[1] = q[2] = q[3] = 0.0f; | 55 » q[1]= q[2]= q[3]= 0.0f; |
55 } | 56 } |
56 | 57 |
57 void copy_qt_qt(float q1[4], const float q2[4]) | 58 void copy_qt_qt(float *q1, const float *q2) |
58 { | 59 { |
59 » q1[0] = q2[0]; | 60 » q1[0]= q2[0]; |
60 » q1[1] = q2[1]; | 61 » q1[1]= q2[1]; |
61 » q1[2] = q2[2]; | 62 » q1[2]= q2[2]; |
62 » q1[3] = q2[3]; | 63 » q1[3]= q2[3]; |
63 } | 64 } |
64 | 65 |
65 bool is_zero_qt(const float q[4]) | 66 int is_zero_qt(float *q) |
66 { | 67 { |
67 return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0); | 68 return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0); |
68 } | 69 } |
69 | 70 |
70 void mul_qt_qtqt(float q[4], const float q1[4], const float q2[4]) | 71 void mul_qt_qtqt(float *q, const float *q1, const float *q2) |
| 72 { |
| 73 » float t0,t1,t2; |
| 74 |
| 75 » t0= q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3]; |
| 76 » t1= q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2]; |
| 77 » t2= q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3]; |
| 78 » q[3]= q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1]; |
| 79 » q[0]=t0;· |
| 80 » q[1]=t1;· |
| 81 » q[2]=t2; |
| 82 } |
| 83 |
| 84 /* Assumes a unit quaternion */ |
| 85 void mul_qt_v3(const float *q, float *v) |
71 { | 86 { |
72 float t0, t1, t2; | 87 float t0, t1, t2; |
73 | 88 |
74 » t0 = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; | 89 » t0= -q[1]*v[0]-q[2]*v[1]-q[3]*v[2]; |
75 » t1 = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; | 90 » t1= q[0]*v[0]+q[2]*v[2]-q[3]*v[1]; |
76 » t2 = q1[0] * q2[2] + q1[2] * q2[0] + q1[3] * q2[1] - q1[1] * q2[3]; | 91 » t2= q[0]*v[1]+q[3]*v[0]-q[1]*v[2]; |
77 » q[3] = q1[0] * q2[3] + q1[3] * q2[0] + q1[1] * q2[2] - q1[2] * q2[1]; | 92 » v[2]= q[0]*v[2]+q[1]*v[1]-q[2]*v[0]; |
78 » q[0] = t0; | 93 » v[0]=t1;· |
79 » q[1] = t1; | 94 » v[1]=t2; |
80 » q[2] = t2; | 95 |
81 } | 96 » t1= t0*-q[1]+v[0]*q[0]-v[1]*q[3]+v[2]*q[2]; |
82 | 97 » t2= t0*-q[2]+v[1]*q[0]-v[2]*q[1]+v[0]*q[3]; |
83 /** | 98 » v[2]= t0*-q[3]+v[2]*q[0]-v[0]*q[2]+v[1]*q[1]; |
84 * \note: | 99 » v[0]=t1;· |
85 * Assumes a unit quaternion? | 100 » v[1]=t2; |
86 * | 101 } |
87 * \note: multiplying by 3x3 matrix is ~25% faster. | 102 |
88 * | 103 void conjugate_qt(float *q) |
89 * in fact not, but you may want to use a unit quat, read on... | |
90 * | |
91 * Shortcut for 'q v q*' when \a v is actually a quaternion. | |
92 * This removes the need for converting a vector to a quaternion, | |
93 * calculating q's conjugate and converting back to a vector. | |
94 * It also happens to be faster (17+,24* vs * 24+,32*). | |
95 * If \a q is not a unit quaternion, then \a v will be both rotated by | |
96 * the same amount as if q was a unit quaternion, and scaled by the square of | |
97 * the length of q. | |
98 * | |
99 * For people used to python mathutils, its like: | |
100 * def mul_qt_v3(q, v): (q * Quaternion((0.0, v[0], v[1], v[2])) * q.conjugated(
))[1:] | |
101 */ | |
102 void mul_qt_v3(const float q[4], float v[3]) | |
103 { | |
104 » float t0, t1, t2; | |
105 | |
106 » t0 = -q[1] * v[0] - q[2] * v[1] - q[3] * v[2]; | |
107 » t1 = q[0] * v[0] + q[2] * v[2] - q[3] * v[1]; | |
108 » t2 = q[0] * v[1] + q[3] * v[0] - q[1] * v[2]; | |
109 » v[2] = q[0] * v[2] + q[1] * v[1] - q[2] * v[0]; | |
110 » v[0] = t1; | |
111 » v[1] = t2; | |
112 | |
113 » t1 = t0 * -q[1] + v[0] * q[0] - v[1] * q[3] + v[2] * q[2]; | |
114 » t2 = t0 * -q[2] + v[1] * q[0] - v[2] * q[1] + v[0] * q[3]; | |
115 » v[2] = t0 * -q[3] + v[2] * q[0] - v[0] * q[2] + v[1] * q[1]; | |
116 » v[0] = t1; | |
117 » v[1] = t2; | |
118 } | |
119 | |
120 void conjugate_qt_qt(float q1[4], const float q2[4]) | |
121 { | |
122 » q1[0] = q2[0]; | |
123 » q1[1] = -q2[1]; | |
124 » q1[2] = -q2[2]; | |
125 » q1[3] = -q2[3]; | |
126 } | |
127 | |
128 void conjugate_qt(float q[4]) | |
129 { | 104 { |
130 q[1] = -q[1]; | 105 q[1] = -q[1]; |
131 q[2] = -q[2]; | 106 q[2] = -q[2]; |
132 q[3] = -q[3]; | 107 q[3] = -q[3]; |
133 } | 108 } |
134 | 109 |
135 float dot_qtqt(const float q1[4], const float q2[4]) | 110 float dot_qtqt(const float q1[4], const float q2[4]) |
136 { | 111 { |
137 » return q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3]; | 112 » return q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3]; |
138 } | 113 } |
139 | 114 |
140 void invert_qt(float q[4]) | 115 void invert_qt(float *q) |
141 { | 116 { |
142 float f = dot_qtqt(q, q); | 117 float f = dot_qtqt(q, q); |
143 | 118 |
144 if (f == 0.0f) | 119 if (f == 0.0f) |
145 return; | 120 return; |
146 | 121 |
147 conjugate_qt(q); | 122 conjugate_qt(q); |
148 » mul_qt_fl(q, 1.0f / f); | 123 » mul_qt_fl(q, 1.0f/f); |
149 } | 124 } |
150 | 125 |
151 void invert_qt_qt(float q1[4], const float q2[4]) | 126 void invert_qt_qt(float *q1, const float *q2) |
152 { | 127 { |
153 copy_qt_qt(q1, q2); | 128 copy_qt_qt(q1, q2); |
154 invert_qt(q1); | 129 invert_qt(q1); |
155 } | 130 } |
156 | 131 |
157 /* simple mult */ | 132 /* simple mult */ |
158 void mul_qt_fl(float q[4], const float f) | 133 void mul_qt_fl(float *q, const float f) |
159 { | 134 { |
160 q[0] *= f; | 135 q[0] *= f; |
161 q[1] *= f; | 136 q[1] *= f; |
162 q[2] *= f; | 137 q[2] *= f; |
163 q[3] *= f; | 138 q[3] *= f; |
164 } | 139 } |
165 | 140 |
166 void sub_qt_qtqt(float q[4], const float q1[4], const float q2[4]) | 141 void sub_qt_qtqt(float q[4], const float q1[4], const float q2[4]) |
167 { | 142 { |
168 float nq2[4]; | 143 float nq2[4]; |
169 | 144 |
170 » nq2[0] = -q2[0]; | 145 » nq2[0]= -q2[0]; |
171 » nq2[1] = q2[1]; | 146 » nq2[1]= q2[1]; |
172 » nq2[2] = q2[2]; | 147 » nq2[2]= q2[2]; |
173 » nq2[3] = q2[3]; | 148 » nq2[3]= q2[3]; |
174 | 149 |
175 mul_qt_qtqt(q, q1, nq2); | 150 mul_qt_qtqt(q, q1, nq2); |
176 } | 151 } |
177 | 152 |
178 /* angular mult factor */ | 153 /* angular mult factor */ |
179 void mul_fac_qt_fl(float q[4], const float fac) | 154 void mul_fac_qt_fl(float *q, const float fac) |
180 { | 155 { |
181 » const float angle = fac * saacos(q[0]); /* quat[0] = cos(0.5 * angle), b
ut now the 0.5 and 2.0 rule out */ | 156 » float angle= fac*saacos(q[0]);» /* quat[0]= cos(0.5*angle), but now the
0.5 and 2.0 rule out */ |
182 » const float co = cosf(angle); | 157 » |
183 » const float si = sinf(angle); | 158 » float co= (float)cos(angle); |
184 » q[0] = co; | 159 » float si= (float)sin(angle); |
185 » normalize_v3(q + 1); | 160 » q[0]= co; |
186 » mul_v3_fl(q + 1, si); | 161 » normalize_v3(q+1); |
| 162 » mul_v3_fl(q+1, si); |
187 } | 163 } |
188 | 164 |
189 /* skip error check, currently only needed by mat3_to_quat_is_ok */ | 165 /* skip error check, currently only needed by mat3_to_quat_is_ok */ |
190 static void quat_to_mat3_no_error(float m[3][3], const float q[4]) | 166 static void quat_to_mat3_no_error(float m[][3], const float q[4]) |
191 { | 167 { |
192 » double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc; | 168 » double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc; |
193 | 169 |
194 » q0 = M_SQRT2 * (double)q[0]; | 170 » q0= M_SQRT2 * (double)q[0]; |
195 » q1 = M_SQRT2 * (double)q[1]; | 171 » q1= M_SQRT2 * (double)q[1]; |
196 » q2 = M_SQRT2 * (double)q[2]; | 172 » q2= M_SQRT2 * (double)q[2]; |
197 » q3 = M_SQRT2 * (double)q[3]; | 173 » q3= M_SQRT2 * (double)q[3]; |
198 | 174 |
199 » qda = q0 * q1; | 175 » qda= q0*q1; |
200 » qdb = q0 * q2; | 176 » qdb= q0*q2; |
201 » qdc = q0 * q3; | 177 » qdc= q0*q3; |
202 » qaa = q1 * q1; | 178 » qaa= q1*q1; |
203 » qab = q1 * q2; | 179 » qab= q1*q2; |
204 » qac = q1 * q3; | 180 » qac= q1*q3; |
205 » qbb = q2 * q2; | 181 » qbb= q2*q2; |
206 » qbc = q2 * q3; | 182 » qbc= q2*q3; |
207 » qcc = q3 * q3; | 183 » qcc= q3*q3; |
208 | 184 |
209 » m[0][0] = (float)(1.0 - qbb - qcc); | 185 » m[0][0]= (float)(1.0-qbb-qcc); |
210 » m[0][1] = (float)(qdc + qab); | 186 » m[0][1]= (float)(qdc+qab); |
211 » m[0][2] = (float)(-qdb + qac); | 187 » m[0][2]= (float)(-qdb+qac); |
212 | 188 |
213 » m[1][0] = (float)(-qdc + qab); | 189 » m[1][0]= (float)(-qdc+qab); |
214 » m[1][1] = (float)(1.0 - qaa - qcc); | 190 » m[1][1]= (float)(1.0-qaa-qcc); |
215 » m[1][2] = (float)(qda + qbc); | 191 » m[1][2]= (float)(qda+qbc); |
216 | 192 |
217 » m[2][0] = (float)(qdb + qac); | 193 » m[2][0]= (float)(qdb+qac); |
218 » m[2][1] = (float)(-qda + qbc); | 194 » m[2][1]= (float)(-qda+qbc); |
219 » m[2][2] = (float)(1.0 - qaa - qbb); | 195 » m[2][2]= (float)(1.0-qaa-qbb); |
220 } | 196 } |
221 | 197 |
222 void quat_to_mat3(float m[3][3], const float q[4]) | 198 |
| 199 void quat_to_mat3(float m[][3], const float q[4]) |
223 { | 200 { |
224 #ifdef DEBUG | 201 #ifdef DEBUG |
225 float f; | 202 float f; |
226 » if (!((f = dot_qtqt(q, q)) == 0.0f || (fabsf(f - 1.0f) < (float)QUAT_EPS
ILON))) { | 203 » if(!((f=dot_qtqt(q, q))==0.0f || (fabsf(f-1.0f) < (float)QUAT_EPSILON)))
{ |
227 fprintf(stderr, "Warning! quat_to_mat3() called with non-normali
zed: size %.8f *** report a bug ***\n", f); | 204 fprintf(stderr, "Warning! quat_to_mat3() called with non-normali
zed: size %.8f *** report a bug ***\n", f); |
228 } | 205 } |
229 #endif | 206 #endif |
230 | 207 |
231 quat_to_mat3_no_error(m, q); | 208 quat_to_mat3_no_error(m, q); |
232 } | 209 } |
233 | 210 |
234 void quat_to_mat4(float m[4][4], const float q[4]) | 211 void quat_to_mat4(float m[][4], const float q[4]) |
235 { | 212 { |
236 » double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc; | 213 » double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc; |
237 | 214 |
238 #ifdef DEBUG | 215 #ifdef DEBUG |
239 » if (!((q0 = dot_qtqt(q, q)) == 0.0 || (fabs(q0 - 1.0) < QUAT_EPSILON)))
{ | 216 » if(!((q0=dot_qtqt(q, q))==0.0f || (fabs(q0-1.0) < QUAT_EPSILON))) { |
240 fprintf(stderr, "Warning! quat_to_mat4() called with non-normali
zed: size %.8f *** report a bug ***\n", (float)q0); | 217 fprintf(stderr, "Warning! quat_to_mat4() called with non-normali
zed: size %.8f *** report a bug ***\n", (float)q0); |
241 } | 218 } |
242 #endif | 219 #endif |
243 | 220 |
244 » q0 = M_SQRT2 * (double)q[0]; | 221 » q0= M_SQRT2 * (double)q[0]; |
245 » q1 = M_SQRT2 * (double)q[1]; | 222 » q1= M_SQRT2 * (double)q[1]; |
246 » q2 = M_SQRT2 * (double)q[2]; | 223 » q2= M_SQRT2 * (double)q[2]; |
247 » q3 = M_SQRT2 * (double)q[3]; | 224 » q3= M_SQRT2 * (double)q[3]; |
248 | 225 |
249 » qda = q0 * q1; | 226 » qda= q0*q1; |
250 » qdb = q0 * q2; | 227 » qdb= q0*q2; |
251 » qdc = q0 * q3; | 228 » qdc= q0*q3; |
252 » qaa = q1 * q1; | 229 » qaa= q1*q1; |
253 » qab = q1 * q2; | 230 » qab= q1*q2; |
254 » qac = q1 * q3; | 231 » qac= q1*q3; |
255 » qbb = q2 * q2; | 232 » qbb= q2*q2; |
256 » qbc = q2 * q3; | 233 » qbc= q2*q3; |
257 » qcc = q3 * q3; | 234 » qcc= q3*q3; |
258 | 235 |
259 » m[0][0] = (float)(1.0 - qbb - qcc); | 236 » m[0][0]= (float)(1.0-qbb-qcc); |
260 » m[0][1] = (float)(qdc + qab); | 237 » m[0][1]= (float)(qdc+qab); |
261 » m[0][2] = (float)(-qdb + qac); | 238 » m[0][2]= (float)(-qdb+qac); |
262 » m[0][3] = 0.0f; | 239 » m[0][3]= 0.0f; |
263 | 240 |
264 » m[1][0] = (float)(-qdc + qab); | 241 » m[1][0]= (float)(-qdc+qab); |
265 » m[1][1] = (float)(1.0 - qaa - qcc); | 242 » m[1][1]= (float)(1.0-qaa-qcc); |
266 » m[1][2] = (float)(qda + qbc); | 243 » m[1][2]= (float)(qda+qbc); |
267 » m[1][3] = 0.0f; | 244 » m[1][3]= 0.0f; |
268 | 245 |
269 » m[2][0] = (float)(qdb + qac); | 246 » m[2][0]= (float)(qdb+qac); |
270 » m[2][1] = (float)(-qda + qbc); | 247 » m[2][1]= (float)(-qda+qbc); |
271 » m[2][2] = (float)(1.0 - qaa - qbb); | 248 » m[2][2]= (float)(1.0-qaa-qbb); |
272 » m[2][3] = 0.0f; | 249 » m[2][3]= 0.0f; |
273 | 250 »······· |
274 » m[3][0] = m[3][1] = m[3][2] = 0.0f; | 251 » m[3][0]= m[3][1]= m[3][2]= 0.0f; |
275 » m[3][3] = 1.0f; | 252 » m[3][3]= 1.0f; |
276 } | 253 } |
277 | 254 |
278 void mat3_to_quat(float q[4], float wmat[3][3]) | 255 void mat3_to_quat(float *q, float wmat[][3]) |
279 { | 256 { |
280 double tr, s; | 257 double tr, s; |
281 float mat[3][3]; | 258 float mat[3][3]; |
282 | 259 |
283 /* work on a copy */ | 260 /* work on a copy */ |
284 copy_m3_m3(mat, wmat); | 261 copy_m3_m3(mat, wmat); |
285 » normalize_m3(mat); /* this is needed AND a 'normalize_qt' in the end */ | 262 » normalize_m3(mat);» » » /* this is needed AND a NormalQu
at in the end */ |
286 | 263 »······· |
287 » tr = 0.25 * (double)(1.0f + mat[0][0] + mat[1][1] + mat[2][2]); | 264 » tr= 0.25* (double)(1.0f+mat[0][0]+mat[1][1]+mat[2][2]); |
288 | 265 »······· |
289 » if (tr > (double)1e-4f) { | 266 » if(tr>(double)FLT_EPSILON) { |
290 » » s = sqrt(tr); | 267 » » s= sqrt(tr); |
291 » » q[0] = (float)s; | 268 » » q[0]= (float)s; |
292 » » s = 1.0 / (4.0 * s); | 269 » » s= 1.0/(4.0*s); |
293 » » q[1] = (float)((double)(mat[1][2] - mat[2][1]) * s); | 270 » » q[1]= (float)((mat[1][2]-mat[2][1])*s); |
294 » » q[2] = (float)((double)(mat[2][0] - mat[0][2]) * s); | 271 » » q[2]= (float)((mat[2][0]-mat[0][2])*s); |
295 » » q[3] = (float)((double)(mat[0][1] - mat[1][0]) * s); | 272 » » q[3]= (float)((mat[0][1]-mat[1][0])*s); |
296 } | 273 } |
297 else { | 274 else { |
298 » » if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) { | 275 » » if(mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) { |
299 » » » s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2
]); | 276 » » » s= 2.0*sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]); |
300 » » » q[1] = (float)(0.25 * s); | 277 » » » q[1]= (float)(0.25*s); |
301 | 278 |
302 » » » s = 1.0 / s; | 279 » » » s= 1.0/s; |
303 » » » q[0] = (float)((double)(mat[1][2] - mat[2][1]) * s); | 280 » » » q[0]= (float)((mat[2][1] - mat[1][2])*s); |
304 » » » q[2] = (float)((double)(mat[1][0] + mat[0][1]) * s); | 281 » » » q[2]= (float)((mat[1][0] + mat[0][1])*s); |
305 » » » q[3] = (float)((double)(mat[2][0] + mat[0][2]) * s); | 282 » » » q[3]= (float)((mat[2][0] + mat[0][2])*s); |
306 } | 283 } |
307 » » else if (mat[1][1] > mat[2][2]) { | 284 » » else if(mat[1][1] > mat[2][2]) { |
308 » » » s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2
]); | 285 » » » s= 2.0*sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]); |
309 » » » q[2] = (float)(0.25 * s); | 286 » » » q[2]= (float)(0.25*s); |
310 | 287 |
311 » » » s = 1.0 / s; | 288 » » » s= 1.0/s; |
312 » » » q[0] = (float)((double)(mat[2][0] - mat[0][2]) * s); | 289 » » » q[0]= (float)((mat[2][0] - mat[0][2])*s); |
313 » » » q[1] = (float)((double)(mat[1][0] + mat[0][1]) * s); | 290 » » » q[1]= (float)((mat[1][0] + mat[0][1])*s); |
314 » » » q[3] = (float)((double)(mat[2][1] + mat[1][2]) * s); | 291 » » » q[3]= (float)((mat[2][1] + mat[1][2])*s); |
315 } | 292 } |
316 else { | 293 else { |
317 » » » s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1
]); | 294 » » » s= 2.0*sqrtf(1.0 + mat[2][2] - mat[0][0] - mat[1][1]); |
318 » » » q[3] = (float)(0.25 * s); | 295 » » » q[3]= (float)(0.25*s); |
319 | 296 |
320 » » » s = 1.0 / s; | 297 » » » s= 1.0/s; |
321 » » » q[0] = (float)((double)(mat[0][1] - mat[1][0]) * s); | 298 » » » q[0]= (float)((mat[1][0] - mat[0][1])*s); |
322 » » » q[1] = (float)((double)(mat[2][0] + mat[0][2]) * s); | 299 » » » q[1]= (float)((mat[2][0] + mat[0][2])*s); |
323 » » » q[2] = (float)((double)(mat[2][1] + mat[1][2]) * s); | 300 » » » q[2]= (float)((mat[2][1] + mat[1][2])*s); |
324 } | 301 } |
325 } | 302 } |
326 | 303 |
327 normalize_qt(q); | 304 normalize_qt(q); |
328 } | 305 } |
329 | 306 |
330 void mat4_to_quat(float q[4], float m[4][4]) | 307 void mat4_to_quat(float *q, float m[][4]) |
331 { | 308 { |
332 float mat[3][3]; | 309 float mat[3][3]; |
333 | 310 » |
334 copy_m3_m4(mat, m); | 311 copy_m3_m4(mat, m); |
335 » mat3_to_quat(q, mat); | 312 » mat3_to_quat(q,mat); |
336 } | 313 } |
337 | 314 |
338 void mat3_to_quat_is_ok(float q[4], float wmat[3][3]) | 315 void mat3_to_quat_is_ok(float q[4], float wmat[3][3]) |
339 { | 316 { |
340 float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, no
r[3]; | 317 float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, no
r[3]; |
341 | 318 |
342 /* work on a copy */ | 319 /* work on a copy */ |
343 copy_m3_m3(mat, wmat); | 320 copy_m3_m3(mat, wmat); |
344 normalize_m3(mat); | 321 normalize_m3(mat); |
345 | 322 » |
346 /* rotate z-axis of matrix to z-axis */ | 323 /* rotate z-axis of matrix to z-axis */ |
347 | 324 |
348 » nor[0] = mat[2][1]; /* cross product with (0,0,1) */ | 325 » nor[0] = mat[2][1];» » /* cross product with (0,0,1) */ |
349 » nor[1] = -mat[2][0]; | 326 » nor[1] = -mat[2][0]; |
350 nor[2] = 0.0; | 327 nor[2] = 0.0; |
351 normalize_v3(nor); | 328 normalize_v3(nor); |
352 | 329 » |
353 » co = mat[2][2]; | 330 » co= mat[2][2]; |
354 » angle = 0.5f * saacos(co); | 331 » angle= 0.5f*saacos(co); |
355 | 332 » |
356 » co = cosf(angle); | 333 » co= (float)cos(angle); |
357 » si = sinf(angle); | 334 » si= (float)sin(angle); |
358 » q1[0] = co; | 335 » q1[0]= co; |
359 » q1[1] = -nor[0] * si; /* negative here, but why? */ | 336 » q1[1]= -nor[0]*si;» » /* negative here, but why? */ |
360 » q1[2] = -nor[1] * si; | 337 » q1[2]= -nor[1]*si; |
361 » q1[3] = -nor[2] * si; | 338 » q1[3]= -nor[2]*si; |
362 | 339 |
363 /* rotate back x-axis from mat, using inverse q1 */ | 340 /* rotate back x-axis from mat, using inverse q1 */ |
364 » quat_to_mat3_no_error(matr, q1); | 341 » quat_to_mat3_no_error( matr,q1); |
365 invert_m3_m3(matn, matr); | 342 invert_m3_m3(matn, matr); |
366 mul_m3_v3(matn, mat[0]); | 343 mul_m3_v3(matn, mat[0]); |
367 | 344 » |
368 /* and align x-axes */ | 345 /* and align x-axes */ |
369 » angle = (float)(0.5 * atan2(mat[0][1], mat[0][0])); | 346 » angle= (float)(0.5*atan2(mat[0][1], mat[0][0])); |
370 | 347 » |
371 » co = cosf(angle); | 348 » co= (float)cos(angle); |
372 » si = sinf(angle); | 349 » si= (float)sin(angle); |
373 » q2[0] = co; | 350 » q2[0]= co; |
374 » q2[1] = 0.0f; | 351 » q2[1]= 0.0f; |
375 » q2[2] = 0.0f; | 352 » q2[2]= 0.0f; |
376 » q2[3] = si; | 353 » q2[3]= si; |
377 | 354 » |
378 mul_qt_qtqt(q, q1, q2); | 355 mul_qt_qtqt(q, q1, q2); |
379 } | 356 } |
380 | 357 |
381 float normalize_qt(float q[4]) | 358 |
| 359 float normalize_qt(float *q) |
382 { | 360 { |
383 float len; | 361 float len; |
384 | 362 » |
385 » len = sqrtf(dot_qtqt(q, q)); | 363 » len= (float)sqrt(dot_qtqt(q, q)); |
386 » if (len != 0.0f) { | 364 » if(len!=0.0f) { |
387 » » mul_qt_fl(q, 1.0f / len); | 365 » » mul_qt_fl(q, 1.0f/len); |
388 } | 366 } |
389 else { | 367 else { |
390 » » q[1] = 1.0f; | 368 » » q[1]= 1.0f; |
391 » » q[0] = q[2] = q[3] = 0.0f; | 369 » » q[0]= q[2]= q[3]= 0.0f;»» » |
392 } | 370 } |
393 | 371 |
394 return len; | 372 return len; |
395 } | 373 } |
396 | 374 |
397 float normalize_qt_qt(float r[4], const float q[4]) | 375 float normalize_qt_qt(float r[4], const float q[4]) |
398 { | 376 { |
399 copy_qt_qt(r, q); | 377 copy_qt_qt(r, q); |
400 return normalize_qt(r); | 378 return normalize_qt(r); |
401 } | 379 } |
402 | 380 |
403 /* note: expects vectors to be normalized */ | 381 /* note: expects vectors to be normalized */ |
404 void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2
[3]) | 382 void rotation_between_vecs_to_quat(float *q, const float v1[3], const float v2[3
]) |
405 { | 383 { |
406 float axis[3]; | 384 float axis[3]; |
407 float angle; | 385 float angle; |
408 | 386 » |
409 cross_v3_v3v3(axis, v1, v2); | 387 cross_v3_v3v3(axis, v1, v2); |
410 | 388 » |
411 angle = angle_normalized_v3v3(v1, v2); | 389 angle = angle_normalized_v3v3(v1, v2); |
412 | 390 » |
413 axis_angle_to_quat(q, axis, angle); | 391 axis_angle_to_quat(q, axis, angle); |
414 } | 392 } |
415 | 393 |
416 void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q
2[4]) | 394 void rotation_between_quats_to_quat(float *q, const float q1[4], const float q2[
4]) |
417 { | 395 { |
418 float tquat[4]; | 396 float tquat[4]; |
419 | 397 » double dot = 0.0f; |
420 » conjugate_qt_qt(tquat, q1); | 398 » int x; |
421 | 399 |
422 » mul_qt_fl(tquat, 1.0f / dot_qtqt(tquat, tquat)); | 400 » copy_qt_qt(tquat, q1); |
| 401 » conjugate_qt(tquat); |
| 402 » dot = 1.0f / dot_qtqt(tquat, tquat); |
| 403 |
| 404 » for(x = 0; x < 4; x++) |
| 405 » » tquat[x] *= dot; |
423 | 406 |
424 mul_qt_qtqt(q, tquat, q2); | 407 mul_qt_qtqt(q, tquat, q2); |
425 } | 408 } |
426 | 409 |
| 410 |
427 void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag) | 411 void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag) |
428 { | 412 { |
429 » float nor[3], tvec[3]; | 413 » float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1; |
430 » float angle, si, co, len; | 414 » |
431 | |
432 assert(axis >= 0 && axis <= 5); | 415 assert(axis >= 0 && axis <= 5); |
433 assert(upflag >= 0 && upflag <= 2); | 416 assert(upflag >= 0 && upflag <= 2); |
434 | 417 »······· |
435 » /* first set the quat to unit */ | 418 » /* first rotate to axis */ |
436 » unit_qt(q); | 419 » if(axis>2) {»··· |
437 | 420 » » x2= vec[0] ; y2= vec[1] ; z2= vec[2]; |
438 » len = len_v3(vec); | 421 » » axis-= 3; |
439 | |
440 » if (UNLIKELY(len == 0.0f)) { | |
441 » » return; | |
442 » } | |
443 | |
444 » /* rotate to axis */ | |
445 » if (axis > 2) { | |
446 » » copy_v3_v3(tvec, vec); | |
447 » » axis -= 3; | |
448 } | 422 } |
449 else { | 423 else { |
450 » » negate_v3_v3(tvec, vec); | 424 » » x2= -vec[0] ; y2= -vec[1] ; z2= -vec[2]; |
451 » } | 425 » } |
| 426 »······· |
| 427 » q[0]=1.0;· |
| 428 » q[1]=q[2]=q[3]= 0.0; |
| 429 |
| 430 » len1= (float)sqrt(x2*x2+y2*y2+z2*z2); |
| 431 » if(len1 == 0.0f) return; |
452 | 432 |
453 /* nasty! I need a good routine for this... | 433 /* nasty! I need a good routine for this... |
454 * problem is a rotation of an Y axis to the negative Y-axis for example
. | 434 * problem is a rotation of an Y axis to the negative Y-axis for example
. |
455 */ | 435 */ |
456 | 436 |
457 » if (axis == 0) { /* x-axis */ | 437 » if(axis==0) {» /* x-axis */ |
458 » » nor[0] = 0.0; | 438 » » nor[0]= 0.0; |
459 » » nor[1] = -tvec[2]; | 439 » » nor[1]= -z2; |
460 » » nor[2] = tvec[1]; | 440 » » nor[2]= y2; |
461 | 441 |
462 » » if (fabsf(tvec[1]) + fabsf(tvec[2]) < 0.0001f) | 442 » » if(fabs(y2)+fabs(z2)<0.0001) |
463 » » » nor[1] = 1.0f; | 443 » » » nor[1]= 1.0; |
464 | 444 |
465 » » co = tvec[0]; | 445 » » co= x2; |
466 » } | 446 » } |
467 » else if (axis == 1) { /* y-axis */ | 447 » else if(axis==1) {» /* y-axis */ |
468 » » nor[0] = tvec[2]; | 448 » » nor[0]= z2; |
469 » » nor[1] = 0.0; | 449 » » nor[1]= 0.0; |
470 » » nor[2] = -tvec[0]; | 450 » » nor[2]= -x2; |
471 | 451 » »······· |
472 » » if (fabsf(tvec[0]) + fabsf(tvec[2]) < 0.0001f) | 452 » » if(fabs(x2)+fabs(z2)<0.0001) |
473 » » » nor[2] = 1.0f; | 453 » » » nor[2]= 1.0; |
474 | 454 » »······· |
475 » » co = tvec[1]; | 455 » » co= y2; |
476 » } | 456 » } |
477 » else { /* z-axis */ | 457 » else {» » » /* z-axis */ |
478 » » nor[0] = -tvec[1]; | 458 » » nor[0]= -y2; |
479 » » nor[1] = tvec[0]; | 459 » » nor[1]= x2; |
480 » » nor[2] = 0.0; | 460 » » nor[2]= 0.0; |
481 | 461 |
482 » » if (fabsf(tvec[0]) + fabsf(tvec[1]) < 0.0001f) | 462 » » if(fabs(x2)+fabs(y2)<0.0001) |
483 » » » nor[0] = 1.0f; | 463 » » » nor[0]= 1.0; |
484 | 464 |
485 » » co = tvec[2]; | 465 » » co= z2; |
486 » } | 466 » } |
487 » co /= len; | 467 » co/= len1; |
488 | 468 |
489 normalize_v3(nor); | 469 normalize_v3(nor); |
490 | 470 »······· |
491 » angle = 0.5f * saacos(co); | 471 » angle= 0.5f*saacos(co); |
492 » si = sinf(angle); | 472 » si= (float)sin(angle); |
493 » q[0] = cosf(angle); | 473 » q[0]= (float)cos(angle); |
494 » q[1] = nor[0] * si; | 474 » q[1]= nor[0]*si; |
495 » q[2] = nor[1] * si; | 475 » q[2]= nor[1]*si; |
496 » q[3] = nor[2] * si; | 476 » q[3]= nor[2]*si; |
497 | 477 »······· |
498 » if (axis != upflag) { | 478 » if(axis!=upflag) { |
499 » » float mat[3][3]; | 479 » » quat_to_mat3(mat,q); |
500 » » float q2[4]; | 480 |
501 » » const float *fp = mat[2]; | 481 » » fp= mat[2]; |
502 » » quat_to_mat3(mat, q); | 482 » » if(axis==0) { |
503 | 483 » » » if(upflag==1) angle= (float)(0.5*atan2(fp[2], fp[1])); |
504 » » if (axis == 0) { | 484 » » » else angle= (float)(-0.5*atan2(fp[1], fp[2])); |
505 » » » if (upflag == 1) angle = 0.5f * atan2f(fp[2], fp[1]); | |
506 » » » else angle = -0.5f * atan2f(fp[1], fp[2]); | |
507 } | 485 } |
508 » » else if (axis == 1) { | 486 » » else if(axis==1) { |
509 » » » if (upflag == 0) angle = -0.5f * atan2f(fp[2], fp[0]); | 487 » » » if(upflag==0) angle= (float)(-0.5*atan2(fp[2], fp[0])); |
510 » » » else angle = 0.5f * atan2f(fp[0], fp[2]); | 488 » » » else angle= (float)(0.5*atan2(fp[0], fp[2])); |
511 } | 489 } |
512 else { | 490 else { |
513 » » » if (upflag == 0) angle = 0.5f * atan2f(-fp[1], -fp[0]); | 491 » » » if(upflag==0) angle= (float)(0.5*atan2(-fp[1], -fp[0])); |
514 » » » else angle = -0.5f * atan2f(-fp[0], -fp[1]); | 492 » » » else angle= (float)(-0.5*atan2(-fp[0], -fp[1])); |
515 } | 493 } |
516 | 494 » » » » |
517 » » co = cosf(angle); | 495 » » co= cosf(angle); |
518 » » si = sinf(angle) / len; | 496 » » si= sinf(angle)/len1; |
519 » » q2[0] = co; | 497 » » q2[0]= co; |
520 » » q2[1] = tvec[0] * si; | 498 » » q2[1]= x2*si; |
521 » » q2[2] = tvec[1] * si; | 499 » » q2[2]= y2*si; |
522 » » q2[3] = tvec[2] * si; | 500 » » q2[3]= z2*si; |
523 | 501 » » » |
524 » » mul_qt_qtqt(q, q2, q); | 502 » » mul_qt_qtqt(q,q2,q); |
525 } | 503 } |
526 } | 504 } |
527 | 505 |
528 #if 0 | 506 #if 0 |
529 | |
530 /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */ | 507 /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */ |
531 void QuatInterpolW(float *result, float quat1[4], float quat2[4], float t) | 508 void QuatInterpolW(float *result, float *quat1, float *quat2, float t) |
532 { | 509 { |
533 float omega, cosom, sinom, sc1, sc2; | 510 float omega, cosom, sinom, sc1, sc2; |
534 | 511 |
535 » cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2]
+ quat1[3] * quat2[3]; | 512 » cosom = quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat
1[3]*quat2[3] ; |
536 | 513 » |
537 /* rotate around shortest angle */ | 514 /* rotate around shortest angle */ |
538 if ((1.0f + cosom) > 0.0001f) { | 515 if ((1.0f + cosom) > 0.0001f) { |
539 | 516 » » |
540 if ((1.0f - cosom) > 0.0001f) { | 517 if ((1.0f - cosom) > 0.0001f) { |
541 omega = (float)acos(cosom); | 518 omega = (float)acos(cosom); |
542 » » » sinom = sinf(omega); | 519 » » » sinom = (float)sin(omega); |
543 » » » sc1 = sinf((1.0 - t) * omega) / sinom; | 520 » » » sc1 = (float)sin((1.0 - t) * omega) / sinom; |
544 » » » sc2 = sinf(t * omega) / sinom; | 521 » » » sc2 = (float)sin(t * omega) / sinom; |
545 » » } | 522 » » } |
546 else { | 523 else { |
547 sc1 = 1.0f - t; | 524 sc1 = 1.0f - t; |
548 sc2 = t; | 525 sc2 = t; |
549 } | 526 } |
550 » » result[0] = sc1 * quat1[0] + sc2 * quat2[0]; | 527 » » result[0] = sc1*quat1[0] + sc2*quat2[0]; |
551 » » result[1] = sc1 * quat1[1] + sc2 * quat2[1]; | 528 » » result[1] = sc1*quat1[1] + sc2*quat2[1]; |
552 » » result[2] = sc1 * quat1[2] + sc2 * quat2[2]; | 529 » » result[2] = sc1*quat1[2] + sc2*quat2[2]; |
553 » » result[3] = sc1 * quat1[3] + sc2 * quat2[3]; | 530 » » result[3] = sc1*quat1[3] + sc2*quat2[3]; |
554 » } | 531 » } |
555 else { | 532 else { |
556 result[0] = quat2[3]; | 533 result[0] = quat2[3]; |
557 result[1] = -quat2[2]; | 534 result[1] = -quat2[2]; |
558 result[2] = quat2[1]; | 535 result[2] = quat2[1]; |
559 result[3] = -quat2[0]; | 536 result[3] = -quat2[0]; |
560 | 537 » » |
561 » » sc1 = sinf((1.0 - t) * M_PI_2); | 538 » » sc1 = (float)sin((1.0 - t)*M_PI_2); |
562 » » sc2 = sinf(t * M_PI_2); | 539 » » sc2 = (float)sin(t*M_PI_2); |
563 | 540 » » |
564 » » result[0] = sc1 * quat1[0] + sc2 * result[0]; | 541 » » result[0] = sc1*quat1[0] + sc2*result[0]; |
565 » » result[1] = sc1 * quat1[1] + sc2 * result[1]; | 542 » » result[1] = sc1*quat1[1] + sc2*result[1]; |
566 » » result[2] = sc1 * quat1[2] + sc2 * result[2]; | 543 » » result[2] = sc1*quat1[2] + sc2*result[2]; |
567 » » result[3] = sc1 * quat1[3] + sc2 * result[3]; | 544 » » result[3] = sc1*quat1[3] + sc2*result[3]; |
568 } | 545 } |
569 } | 546 } |
570 #endif | 547 #endif |
571 | 548 |
572 void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4],
const float t) | 549 void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4],
const float t) |
573 { | 550 { |
574 float quat[4], omega, cosom, sinom, sc1, sc2; | 551 float quat[4], omega, cosom, sinom, sc1, sc2; |
575 | 552 |
576 » cosom = dot_qtqt(quat1, quat2); | 553 » cosom = quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat
1[3]*quat2[3] ; |
577 | 554 »······· |
578 /* rotate around shortest angle */ | 555 /* rotate around shortest angle */ |
579 if (cosom < 0.0f) { | 556 if (cosom < 0.0f) { |
580 cosom = -cosom; | 557 cosom = -cosom; |
581 » » negate_v4_v4(quat, quat1); | 558 » » quat[0]= -quat1[0]; |
582 » } | 559 » » quat[1]= -quat1[1]; |
| 560 » » quat[2]= -quat1[2]; |
| 561 » » quat[3]= -quat1[3]; |
| 562 » }· |
583 else { | 563 else { |
584 » » copy_qt_qt(quat, quat1); | 564 » » quat[0]= quat1[0]; |
585 » } | 565 » » quat[1]= quat1[1]; |
586 | 566 » » quat[2]= quat1[2]; |
| 567 » » quat[3]= quat1[3]; |
| 568 » } |
| 569 »······· |
587 if ((1.0f - cosom) > 0.0001f) { | 570 if ((1.0f - cosom) > 0.0001f) { |
588 » » omega = acosf(cosom); | 571 » » omega = (float)acos(cosom); |
589 » » sinom = sinf(omega); | 572 » » sinom = (float)sin(omega); |
590 » » sc1 = sinf((1.0f - t) * omega) / sinom; | 573 » » sc1 = (float)sin((1 - t) * omega) / sinom; |
591 » » sc2 = sinf(t * omega) / sinom; | 574 » » sc2 = (float)sin(t * omega) / sinom; |
592 » } | 575 » } else { |
593 » else { | 576 » » sc1= 1.0f - t; |
594 » » sc1 = 1.0f - t; | 577 » » sc2= t; |
595 » » sc2 = t; | 578 » } |
596 » } | 579 » |
597 | |
598 result[0] = sc1 * quat[0] + sc2 * quat2[0]; | 580 result[0] = sc1 * quat[0] + sc2 * quat2[0]; |
599 result[1] = sc1 * quat[1] + sc2 * quat2[1]; | 581 result[1] = sc1 * quat[1] + sc2 * quat2[1]; |
600 result[2] = sc1 * quat[2] + sc2 * quat2[2]; | 582 result[2] = sc1 * quat[2] + sc2 * quat2[2]; |
601 result[3] = sc1 * quat[3] + sc2 * quat2[3]; | 583 result[3] = sc1 * quat[3] + sc2 * quat2[3]; |
602 } | 584 } |
603 | 585 |
604 void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], co
nst float t) | 586 void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], co
nst float t) |
605 { | 587 { |
606 » result[0] = quat1[0] + t * quat2[0]; | 588 » result[0]= quat1[0] + t*quat2[0]; |
607 » result[1] = quat1[1] + t * quat2[1]; | 589 » result[1]= quat1[1] + t*quat2[1]; |
608 » result[2] = quat1[2] + t * quat2[2]; | 590 » result[2]= quat1[2] + t*quat2[2]; |
609 » result[3] = quat1[3] + t * quat2[3]; | 591 » result[3]= quat1[3] + t*quat2[3]; |
610 } | 592 } |
611 | 593 |
612 /* same as tri_to_quat() but takes pre-computed normal from the triangle | 594 void tri_to_quat(float quat[4], const float v1[3], const float v2[3], const floa
t v3[3]) |
613 * used for ngons when we know their normal */ | |
614 void tri_to_quat_ex(float quat[4], const float v1[3], const float v2[3], const f
loat v3[3], | |
615 const float no_orig[3]) | |
616 { | 595 { |
617 /* imaginary x-axis, y-axis triangle is being rotated */ | 596 /* imaginary x-axis, y-axis triangle is being rotated */ |
618 float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3]; | 597 float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3]; |
619 | 598 » |
620 /* move z-axis to face-normal */ | 599 /* move z-axis to face-normal */ |
621 #if 0 | 600 » normal_tri_v3(vec,v1, v2, v3); |
622 » normal_tri_v3(vec, v1, v2, v3); | 601 |
623 #else | 602 » n[0]= vec[1]; |
624 » copy_v3_v3(vec, no_orig); | 603 » n[1]= -vec[0]; |
625 » (void)v3; | 604 » n[2]= 0.0f; |
626 #endif | |
627 | |
628 » n[0] = vec[1]; | |
629 » n[1] = -vec[0]; | |
630 » n[2] = 0.0f; | |
631 normalize_v3(n); | 605 normalize_v3(n); |
632 | 606 » |
633 » if (n[0] == 0.0f && n[1] == 0.0f) { | 607 » if(n[0]==0.0f && n[1]==0.0f) n[0]= 1.0f; |
634 » » n[0] = 1.0f; | 608 »······· |
635 » } | 609 » angle= -0.5f*(float)saacos(vec[2]); |
636 | 610 » co= (float)cos(angle); |
637 » angle = -0.5f * saacos(vec[2]); | 611 » si= (float)sin(angle); |
638 » co = cosf(angle); | 612 » q1[0]= co; |
639 » si = sinf(angle); | 613 » q1[1]= n[0]*si; |
640 » q1[0] = co; | 614 » q1[2]= n[1]*si; |
641 » q1[1] = n[0] * si; | 615 » q1[3]= 0.0f; |
642 » q1[2] = n[1] * si; | 616 » |
643 » q1[3] = 0.0f; | |
644 | |
645 /* rotate back line v1-v2 */ | 617 /* rotate back line v1-v2 */ |
646 » quat_to_mat3(mat, q1); | 618 » quat_to_mat3(mat,q1); |
647 invert_m3_m3(imat, mat); | 619 invert_m3_m3(imat, mat); |
648 sub_v3_v3v3(vec, v2, v1); | 620 sub_v3_v3v3(vec, v2, v1); |
649 mul_m3_v3(imat, vec); | 621 mul_m3_v3(imat, vec); |
650 | 622 |
651 /* what angle has this line with x-axis? */ | 623 /* what angle has this line with x-axis? */ |
652 » vec[2] = 0.0f; | 624 » vec[2]= 0.0f; |
653 normalize_v3(vec); | 625 normalize_v3(vec); |
654 | 626 |
655 » angle = (float)(0.5 * atan2(vec[1], vec[0])); | 627 » angle= (float)(0.5*atan2(vec[1], vec[0])); |
656 » co = cosf(angle); | 628 » co= (float)cos(angle); |
657 » si = sinf(angle); | 629 » si= (float)sin(angle); |
658 » q2[0] = co; | 630 » q2[0]= co; |
659 » q2[1] = 0.0f; | 631 » q2[1]= 0.0f; |
660 » q2[2] = 0.0f; | 632 » q2[2]= 0.0f; |
661 » q2[3] = si; | 633 » q2[3]= si; |
662 | 634 |
663 mul_qt_qtqt(quat, q1, q2); | 635 mul_qt_qtqt(quat, q1, q2); |
664 } | 636 } |
665 | 637 |
666 void tri_to_quat(float quat[4], const float v1[3], const float v2[3], const floa
t v3[3]) | 638 void print_qt(const char *str, const float q[4]) |
667 { | |
668 » float vec[3]; | |
669 » normal_tri_v3(vec, v1, v2, v3); | |
670 » tri_to_quat_ex(quat, v1, v2, v3, vec); | |
671 } | |
672 | |
673 void print_qt(const char *str, const float q[4]) | |
674 { | 639 { |
675 printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]); | 640 printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]); |
676 } | 641 } |
677 | 642 |
678 /******************************** Axis Angle *********************************/ | 643 /******************************** Axis Angle *********************************/ |
679 | 644 |
680 /* Axis angle to Quaternions */ | 645 /* Axis angle to Quaternions */ |
681 void axis_angle_to_quat(float q[4], const float axis[3], const float angle) | 646 void axis_angle_to_quat(float q[4], const float axis[3], float angle) |
682 { | 647 { |
683 float nor[3]; | 648 float nor[3]; |
684 | 649 » float si; |
685 » if (LIKELY(normalize_v3_v3(nor, axis) != 0.0f)) { | 650 |
686 » » const float phi = angle / 2.0f; | 651 » if(normalize_v3_v3(nor, axis) == 0.0f) { |
687 » » float si; | |
688 » » si = sinf(phi); | |
689 » » q[0] = cosf(phi); | |
690 » » q[1] = nor[0] * si; | |
691 » » q[2] = nor[1] * si; | |
692 » » q[3] = nor[2] * si; | |
693 » } | |
694 » else { | |
695 unit_qt(q); | 652 unit_qt(q); |
696 » } | 653 » » return; |
| 654 » } |
| 655 |
| 656 » angle /= 2; |
| 657 » si = (float)sin(angle); |
| 658 » q[0] = (float)cos(angle); |
| 659 » q[1] = nor[0] * si; |
| 660 » q[2] = nor[1] * si; |
| 661 » q[3] = nor[2] * si;»···· |
697 } | 662 } |
698 | 663 |
699 /* Quaternions to Axis Angle */ | 664 /* Quaternions to Axis Angle */ |
700 void quat_to_axis_angle(float axis[3], float *angle, const float q[4]) | 665 void quat_to_axis_angle(float axis[3], float *angle, const float q[4]) |
701 { | 666 { |
702 float ha, si; | 667 float ha, si; |
703 | 668 |
704 #ifdef DEBUG | 669 #ifdef DEBUG |
705 » if (!((ha = dot_qtqt(q, q)) == 0.0f || (fabsf(ha - 1.0f) < (float)QUAT_E
PSILON))) { | 670 » if(!((ha=dot_qtqt(q, q))==0.0f || (fabsf(ha-1.0f) < (float)QUAT_EPSILON)
)) { |
706 fprintf(stderr, "Warning! quat_to_axis_angle() called with non-n
ormalized: size %.8f *** report a bug ***\n", ha); | 671 fprintf(stderr, "Warning! quat_to_axis_angle() called with non-n
ormalized: size %.8f *** report a bug ***\n", ha); |
707 } | 672 } |
708 #endif | 673 #endif |
709 | 674 |
710 /* calculate angle/2, and sin(angle/2) */ | 675 /* calculate angle/2, and sin(angle/2) */ |
711 » ha = acosf(q[0]); | 676 » ha= (float)acos(q[0]); |
712 » si = sinf(ha); | 677 » si= (float)sin(ha); |
713 | 678 » |
714 /* from half-angle to angle */ | 679 /* from half-angle to angle */ |
715 » *angle = ha * 2; | 680 » *angle= ha * 2; |
716 | 681 » |
717 /* prevent division by zero for axis conversion */ | 682 /* prevent division by zero for axis conversion */ |
718 » if (fabsf(si) < 0.0005f) | 683 » if (fabs(si) < 0.0005) |
719 » » si = 1.0f; | 684 » » si= 1.0f; |
720 | 685 » |
721 » axis[0] = q[1] / si; | 686 » axis[0]= q[1] / si; |
722 » axis[1] = q[2] / si; | 687 » axis[1]= q[2] / si; |
723 » axis[2] = q[3] / si; | 688 » axis[2]= q[3] / si; |
724 } | 689 } |
725 | 690 |
726 /* Axis Angle to Euler Rotation */ | 691 /* Axis Angle to Euler Rotation */ |
727 void axis_angle_to_eulO(float eul[3], const short order, const float axis[3], co
nst float angle) | 692 void axis_angle_to_eulO(float eul[3], const short order, const float axis[3], co
nst float angle) |
728 { | 693 { |
729 float q[4]; | 694 float q[4]; |
730 | 695 » |
731 /* use quaternions as intermediate representation for now... */ | 696 /* use quaternions as intermediate representation for now... */ |
732 axis_angle_to_quat(q, axis, angle); | 697 axis_angle_to_quat(q, axis, angle); |
733 » quat_to_eulO(eul, order, q); | 698 » quat_to_eulO(eul, order,q); |
734 } | 699 } |
735 | 700 |
736 /* Euler Rotation to Axis Angle */ | 701 /* Euler Rotation to Axis Angle */ |
737 void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const s
hort order) | 702 void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const s
hort order) |
738 { | 703 { |
739 float q[4]; | 704 float q[4]; |
740 | 705 » |
741 /* use quaternions as intermediate representation for now... */ | 706 /* use quaternions as intermediate representation for now... */ |
742 » eulO_to_quat(q, eul, order); | 707 » eulO_to_quat(q,eul, order); |
743 » quat_to_axis_angle(axis, angle, q); | 708 » quat_to_axis_angle(axis, angle,q); |
744 } | 709 } |
745 | 710 |
746 /* axis angle to 3x3 matrix - note: requires that axis is normalized */ | 711 /* axis angle to 3x3 matrix - safer version (normalisation of axis performed) */ |
747 void axis_angle_normalized_to_mat3(float mat[3][3], const float nor[3], const fl
oat angle) | 712 void axis_angle_to_mat3(float mat[3][3], const float axis[3], const float angle) |
748 { | 713 { |
749 » float nsi[3], co, si, ico; | 714 » float nor[3], nsi[3], co, si, ico; |
750 | 715 »······· |
751 » BLI_ASSERT_UNIT_V3(nor); | 716 » /* normalise the axis first (to remove unwanted scaling) */ |
752 | 717 » if(normalize_v3_v3(nor, axis) == 0.0f) { |
| 718 » » unit_m3(mat); |
| 719 » » return; |
| 720 » } |
| 721 »······· |
753 /* now convert this to a 3x3 matrix */ | 722 /* now convert this to a 3x3 matrix */ |
754 » co = cosf(angle); | 723 » co= (float)cos(angle);» »······· |
755 » si = sinf(angle); | 724 » si= (float)sin(angle); |
756 | 725 »······· |
757 » ico = (1.0f - co); | 726 » ico= (1.0f - co); |
758 » nsi[0] = nor[0] * si; | 727 » nsi[0]= nor[0]*si; |
759 » nsi[1] = nor[1] * si; | 728 » nsi[1]= nor[1]*si; |
760 » nsi[2] = nor[2] * si; | 729 » nsi[2]= nor[2]*si; |
761 | 730 » |
762 mat[0][0] = ((nor[0] * nor[0]) * ico) + co; | 731 mat[0][0] = ((nor[0] * nor[0]) * ico) + co; |
763 mat[0][1] = ((nor[0] * nor[1]) * ico) + nsi[2]; | 732 mat[0][1] = ((nor[0] * nor[1]) * ico) + nsi[2]; |
764 mat[0][2] = ((nor[0] * nor[2]) * ico) - nsi[1]; | 733 mat[0][2] = ((nor[0] * nor[2]) * ico) - nsi[1]; |
765 mat[1][0] = ((nor[0] * nor[1]) * ico) - nsi[2]; | 734 mat[1][0] = ((nor[0] * nor[1]) * ico) - nsi[2]; |
766 mat[1][1] = ((nor[1] * nor[1]) * ico) + co; | 735 mat[1][1] = ((nor[1] * nor[1]) * ico) + co; |
767 mat[1][2] = ((nor[1] * nor[2]) * ico) + nsi[0]; | 736 mat[1][2] = ((nor[1] * nor[2]) * ico) + nsi[0]; |
768 mat[2][0] = ((nor[0] * nor[2]) * ico) + nsi[1]; | 737 mat[2][0] = ((nor[0] * nor[2]) * ico) + nsi[1]; |
769 mat[2][1] = ((nor[1] * nor[2]) * ico) - nsi[0]; | 738 mat[2][1] = ((nor[1] * nor[2]) * ico) - nsi[0]; |
770 mat[2][2] = ((nor[2] * nor[2]) * ico) + co; | 739 mat[2][2] = ((nor[2] * nor[2]) * ico) + co; |
771 } | 740 } |
772 | 741 |
773 | 742 /* axis angle to 4x4 matrix - safer version (normalisation of axis performed) */ |
774 /* axis angle to 3x3 matrix - safer version (normalization of axis performed) */ | |
775 void axis_angle_to_mat3(float mat[3][3], const float axis[3], const float angle) | |
776 { | |
777 » float nor[3]; | |
778 | |
779 » /* normalize the axis first (to remove unwanted scaling) */ | |
780 » if (normalize_v3_v3(nor, axis) == 0.0f) { | |
781 » » unit_m3(mat); | |
782 » » return; | |
783 » } | |
784 | |
785 » axis_angle_normalized_to_mat3(mat, nor, angle); | |
786 } | |
787 | |
788 /* axis angle to 4x4 matrix - safer version (normalization of axis performed) */ | |
789 void axis_angle_to_mat4(float mat[4][4], const float axis[3], const float angle) | 743 void axis_angle_to_mat4(float mat[4][4], const float axis[3], const float angle) |
790 { | 744 { |
791 float tmat[3][3]; | 745 float tmat[3][3]; |
792 | 746 » |
793 » axis_angle_to_mat3(tmat, axis, angle); | 747 » axis_angle_to_mat3(tmat,axis, angle); |
794 unit_m4(mat); | 748 unit_m4(mat); |
795 copy_m4_m3(mat, tmat); | 749 copy_m4_m3(mat, tmat); |
796 } | 750 } |
797 | 751 |
798 /* 3x3 matrix to axis angle (see Mat4ToVecRot too) */ | 752 /* 3x3 matrix to axis angle (see Mat4ToVecRot too) */ |
799 void mat3_to_axis_angle(float axis[3], float *angle, float mat[3][3]) | 753 void mat3_to_axis_angle(float axis[3], float *angle,float mat[3][3]) |
800 { | 754 { |
801 float q[4]; | 755 float q[4]; |
802 | 756 » |
803 /* use quaternions as intermediate representation */ | 757 /* use quaternions as intermediate representation */ |
804 » /* TODO: it would be nicer to go straight there... */ | 758 » // TODO: it would be nicer to go straight there... |
805 » mat3_to_quat(q, mat); | 759 » mat3_to_quat(q,mat); |
806 » quat_to_axis_angle(axis, angle, q); | 760 » quat_to_axis_angle(axis, angle,q); |
807 } | 761 } |
808 | 762 |
809 /* 4x4 matrix to axis angle (see Mat4ToVecRot too) */ | 763 /* 4x4 matrix to axis angle (see Mat4ToVecRot too) */ |
810 void mat4_to_axis_angle(float axis[3], float *angle, float mat[4][4]) | 764 void mat4_to_axis_angle(float axis[3], float *angle,float mat[4][4]) |
811 { | 765 { |
812 float q[4]; | 766 float q[4]; |
813 | 767 » |
814 /* use quaternions as intermediate representation */ | 768 /* use quaternions as intermediate representation */ |
815 » /* TODO: it would be nicer to go straight there... */ | 769 » // TODO: it would be nicer to go straight there... |
816 » mat4_to_quat(q, mat); | 770 » mat4_to_quat(q,mat); |
817 » quat_to_axis_angle(axis, angle, q); | 771 » quat_to_axis_angle(axis, angle,q); |
818 } | 772 } |
819 | 773 |
820 /* rotation matrix from a single axis */ | 774 |
821 void axis_angle_to_mat3_single(float mat[3][3], const char axis, const float ang
le) | 775 |
822 { | 776 void single_axis_angle_to_mat3(float mat[3][3], const char axis, const float ang
le) |
823 » const float angle_cos = cosf(angle); | 777 { |
824 » const float angle_sin = sinf(angle); | 778 » const float angle_cos= cosf(angle); |
825 | 779 » const float angle_sin= sinf(angle); |
826 » switch (axis) { | 780 |
827 » » case 'X': /* rotation around X */ | 781 » switch(axis) { |
828 » » » mat[0][0] = 1.0f; | 782 » case 'X': /* rotation around X */ |
829 » » » mat[0][1] = 0.0f; | 783 » » mat[0][0] = 1.0f; |
830 » » » mat[0][2] = 0.0f; | 784 » » mat[0][1] = 0.0f; |
831 » » » mat[1][0] = 0.0f; | 785 » » mat[0][2] = 0.0f; |
832 » » » mat[1][1] = angle_cos; | 786 » » mat[1][0] = 0.0f; |
833 » » » mat[1][2] = angle_sin; | 787 » » mat[1][1] = angle_cos; |
834 » » » mat[2][0] = 0.0f; | 788 » » mat[1][2] = angle_sin; |
835 » » » mat[2][1] = -angle_sin; | 789 » » mat[2][0] = 0.0f; |
836 » » » mat[2][2] = angle_cos; | 790 » » mat[2][1] = -angle_sin; |
837 » » » break; | 791 » » mat[2][2] = angle_cos; |
838 » » case 'Y': /* rotation around Y */ | 792 » » break; |
839 » » » mat[0][0] = angle_cos; | 793 » case 'Y': /* rotation around Y */ |
840 » » » mat[0][1] = 0.0f; | 794 » » mat[0][0] = angle_cos; |
841 » » » mat[0][2] = -angle_sin; | 795 » » mat[0][1] = 0.0f; |
842 » » » mat[1][0] = 0.0f; | 796 » » mat[0][2] = -angle_sin; |
843 » » » mat[1][1] = 1.0f; | 797 » » mat[1][0] = 0.0f; |
844 » » » mat[1][2] = 0.0f; | 798 » » mat[1][1] = 1.0f; |
845 » » » mat[2][0] = angle_sin; | 799 » » mat[1][2] = 0.0f; |
846 » » » mat[2][1] = 0.0f; | 800 » » mat[2][0] = angle_sin; |
847 » » » mat[2][2] = angle_cos; | 801 » » mat[2][1] = 0.0f; |
848 » » » break; | 802 » » mat[2][2] = angle_cos; |
849 » » case 'Z': /* rotation around Z */ | 803 » » break; |
850 » » » mat[0][0] = angle_cos; | 804 » case 'Z': /* rotation around Z */ |
851 » » » mat[0][1] = angle_sin; | 805 » » mat[0][0] = angle_cos; |
852 » » » mat[0][2] = 0.0f; | 806 » » mat[0][1] = angle_sin; |
853 » » » mat[1][0] = -angle_sin; | 807 » » mat[0][2] = 0.0f; |
854 » » » mat[1][1] = angle_cos; | 808 » » mat[1][0] = -angle_sin; |
855 » » » mat[1][2] = 0.0f; | 809 » » mat[1][1] = angle_cos; |
856 » » » mat[2][0] = 0.0f; | 810 » » mat[1][2] = 0.0f; |
857 » » » mat[2][1] = 0.0f; | 811 » » mat[2][0] = 0.0f; |
858 » » » mat[2][2] = 1.0f; | 812 » » mat[2][1] = 0.0f; |
859 » » » break; | 813 » » mat[2][2] = 1.0f; |
860 » » default: | 814 » » break; |
861 » » » BLI_assert(0); | 815 » default: |
862 » » » break; | 816 » » assert("invalid axis"); |
863 » } | 817 » } |
864 } | 818 } |
865 | 819 |
866 void angle_to_mat2(float mat[2][2], const float angle) | 820 /****************************** Vector/Rotation ******************************/ |
867 { | 821 /* TODO: the following calls should probably be depreceated sometime */ |
868 » const float angle_cos = cosf(angle); | 822 |
869 » const float angle_sin = sinf(angle); | 823 /* axis angle to 3x3 matrix */ |
870 | 824 void vec_rot_to_mat3(float mat[][3], const float vec[3], const float phi) |
871 » /* 2D rotation matrix */ | 825 { |
872 » mat[0][0] = angle_cos; | 826 » /* rotation of phi radials around vec */ |
873 » mat[0][1] = angle_sin; | 827 » float vx, vx2, vy, vy2, vz, vz2, co, si; |
874 » mat[1][0] = -angle_sin; | 828 »······· |
875 » mat[1][1] = angle_cos; | 829 » vx= vec[0]; |
| 830 » vy= vec[1]; |
| 831 » vz= vec[2]; |
| 832 » vx2= vx*vx; |
| 833 » vy2= vy*vy; |
| 834 » vz2= vz*vz; |
| 835 » co= (float)cos(phi); |
| 836 » si= (float)sin(phi); |
| 837 »······· |
| 838 » mat[0][0]= vx2+co*(1.0f-vx2); |
| 839 » mat[0][1]= vx*vy*(1.0f-co)+vz*si; |
| 840 » mat[0][2]= vz*vx*(1.0f-co)-vy*si; |
| 841 » mat[1][0]= vx*vy*(1.0f-co)-vz*si; |
| 842 » mat[1][1]= vy2+co*(1.0f-vy2); |
| 843 » mat[1][2]= vy*vz*(1.0f-co)+vx*si; |
| 844 » mat[2][0]= vz*vx*(1.0f-co)+vy*si; |
| 845 » mat[2][1]= vy*vz*(1.0f-co)-vx*si; |
| 846 » mat[2][2]= vz2+co*(1.0f-vz2); |
| 847 } |
| 848 |
| 849 /* axis angle to 4x4 matrix */ |
| 850 void vec_rot_to_mat4(float mat[][4], const float vec[3], const float phi) |
| 851 { |
| 852 » float tmat[3][3]; |
| 853 »······· |
| 854 » vec_rot_to_mat3(tmat,vec, phi); |
| 855 » unit_m4(mat); |
| 856 » copy_m4_m3(mat, tmat); |
| 857 } |
| 858 |
| 859 /* axis angle to quaternion */ |
| 860 void vec_rot_to_quat(float *quat, const float vec[3], const float phi) |
| 861 { |
| 862 » /* rotation of phi radials around vec */ |
| 863 » float si; |
| 864 |
| 865 » quat[1]= vec[0]; |
| 866 » quat[2]= vec[1]; |
| 867 » quat[3]= vec[2]; |
| 868 »······· |
| 869 » if(normalize_v3(quat+1) == 0.0f) { |
| 870 » » unit_qt(quat); |
| 871 » } |
| 872 » else { |
| 873 » » quat[0]= (float)cos((double)phi/2.0); |
| 874 » » si= (float)sin((double)phi/2.0); |
| 875 » » quat[1] *= si; |
| 876 » » quat[2] *= si; |
| 877 » » quat[3] *= si; |
| 878 » } |
876 } | 879 } |
877 | 880 |
878 /******************************** XYZ Eulers *********************************/ | 881 /******************************** XYZ Eulers *********************************/ |
879 | 882 |
880 /* XYZ order */ | 883 /* XYZ order */ |
881 void eul_to_mat3(float mat[3][3], const float eul[3]) | 884 void eul_to_mat3(float mat[][3], const float eul[3]) |
882 { | 885 { |
883 double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; | 886 double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; |
884 | 887 » |
885 » ci = cos(eul[0]); | 888 » ci = cos(eul[0]); |
886 » cj = cos(eul[1]); | 889 » cj = cos(eul[1]); |
887 ch = cos(eul[2]); | 890 ch = cos(eul[2]); |
888 » si = sin(eul[0]); | 891 » si = sin(eul[0]); |
889 » sj = sin(eul[1]); | 892 » sj = sin(eul[1]); |
890 sh = sin(eul[2]); | 893 sh = sin(eul[2]); |
891 » cc = ci * ch; | 894 » cc = ci*ch;· |
892 » cs = ci * sh; | 895 » cs = ci*sh;· |
893 » sc = si * ch; | 896 » sc = si*ch;· |
894 » ss = si * sh; | 897 » ss = si*sh; |
895 | 898 |
896 » mat[0][0] = (float)(cj * ch); | 899 » mat[0][0] = (float)(cj*ch);· |
897 » mat[1][0] = (float)(sj * sc - cs); | 900 » mat[1][0] = (float)(sj*sc-cs);· |
898 » mat[2][0] = (float)(sj * cc + ss); | 901 » mat[2][0] = (float)(sj*cc+ss); |
899 » mat[0][1] = (float)(cj * sh); | 902 » mat[0][1] = (float)(cj*sh);· |
900 » mat[1][1] = (float)(sj * ss + cc); | 903 » mat[1][1] = (float)(sj*ss+cc);· |
901 » mat[2][1] = (float)(sj * cs - sc); | 904 » mat[2][1] = (float)(sj*cs-sc); |
902 » mat[0][2] = (float)-sj; | 905 » mat[0][2] = (float)-sj;»· |
903 » mat[1][2] = (float)(cj * si); | 906 » mat[1][2] = (float)(cj*si);···· |
904 » mat[2][2] = (float)(cj * ci); | 907 » mat[2][2] = (float)(cj*ci); |
905 | 908 |
906 } | 909 } |
907 | 910 |
908 /* XYZ order */ | 911 /* XYZ order */ |
909 void eul_to_mat4(float mat[4][4], const float eul[3]) | 912 void eul_to_mat4(float mat[][4], const float eul[3]) |
910 { | 913 { |
911 double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; | 914 double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; |
912 | 915 » |
913 » ci = cos(eul[0]); | 916 » ci = cos(eul[0]); |
914 » cj = cos(eul[1]); | 917 » cj = cos(eul[1]); |
915 ch = cos(eul[2]); | 918 ch = cos(eul[2]); |
916 » si = sin(eul[0]); | 919 » si = sin(eul[0]); |
917 » sj = sin(eul[1]); | 920 » sj = sin(eul[1]); |
918 sh = sin(eul[2]); | 921 sh = sin(eul[2]); |
919 » cc = ci * ch; | 922 » cc = ci*ch;· |
920 » cs = ci * sh; | 923 » cs = ci*sh;· |
921 » sc = si * ch; | 924 » sc = si*ch;· |
922 » ss = si * sh; | 925 » ss = si*sh; |
923 | 926 |
924 » mat[0][0] = (float)(cj * ch); | 927 » mat[0][0] = (float)(cj*ch);· |
925 » mat[1][0] = (float)(sj * sc - cs); | 928 » mat[1][0] = (float)(sj*sc-cs);· |
926 » mat[2][0] = (float)(sj * cc + ss); | 929 » mat[2][0] = (float)(sj*cc+ss); |
927 » mat[0][1] = (float)(cj * sh); | 930 » mat[0][1] = (float)(cj*sh);· |
928 » mat[1][1] = (float)(sj * ss + cc); | 931 » mat[1][1] = (float)(sj*ss+cc);· |
929 » mat[2][1] = (float)(sj * cs - sc); | 932 » mat[2][1] = (float)(sj*cs-sc); |
930 » mat[0][2] = (float)-sj; | 933 » mat[0][2] = (float)-sj;»· |
931 » mat[1][2] = (float)(cj * si); | 934 » mat[1][2] = (float)(cj*si);···· |
932 » mat[2][2] = (float)(cj * ci); | 935 » mat[2][2] = (float)(cj*ci); |
933 | 936 |
934 | 937 |
935 » mat[3][0] = mat[3][1] = mat[3][2] = mat[0][3] = mat[1][3] = mat[2][3] =
0.0f; | 938 » mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f; |
936 » mat[3][3] = 1.0f; | 939 » mat[3][3]= 1.0f; |
937 } | 940 } |
938 | 941 |
939 /* returns two euler calculation methods, so we can pick the best */ | 942 /* returns two euler calculation methods, so we can pick the best */ |
940 | |
941 /* XYZ order */ | 943 /* XYZ order */ |
942 static void mat3_to_eul2(float tmat[3][3], float eul1[3], float eul2[3]) | 944 static void mat3_to_eul2(float tmat[][3], float eul1[3], float eul2[3]) |
943 { | 945 { |
944 float cy, quat[4], mat[3][3]; | 946 float cy, quat[4], mat[3][3]; |
945 | 947 » |
946 » mat3_to_quat(quat, tmat); | 948 » mat3_to_quat(quat,tmat); |
947 » quat_to_mat3(mat, quat); | 949 » quat_to_mat3(mat,quat); |
948 copy_m3_m3(mat, tmat); | 950 copy_m3_m3(mat, tmat); |
949 normalize_m3(mat); | 951 normalize_m3(mat); |
950 | 952 » |
951 » cy = (float)sqrt(mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1]); | 953 » cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]); |
952 | 954 » |
953 » if (cy > 16.0f * FLT_EPSILON) { | 955 » if (cy > 16.0f*FLT_EPSILON) { |
954 | 956 » » |
955 eul1[0] = (float)atan2(mat[1][2], mat[2][2]); | 957 eul1[0] = (float)atan2(mat[1][2], mat[2][2]); |
956 eul1[1] = (float)atan2(-mat[0][2], cy); | 958 eul1[1] = (float)atan2(-mat[0][2], cy); |
957 eul1[2] = (float)atan2(mat[0][1], mat[0][0]); | 959 eul1[2] = (float)atan2(mat[0][1], mat[0][0]); |
958 | 960 » » |
959 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]); | 961 eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]); |
960 eul2[1] = (float)atan2(-mat[0][2], -cy); | 962 eul2[1] = (float)atan2(-mat[0][2], -cy); |
961 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]); | 963 eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]); |
962 | 964 » » |
963 » } | 965 » } else { |
964 » else { | |
965 eul1[0] = (float)atan2(-mat[2][1], mat[1][1]); | 966 eul1[0] = (float)atan2(-mat[2][1], mat[1][1]); |
966 eul1[1] = (float)atan2(-mat[0][2], cy); | 967 eul1[1] = (float)atan2(-mat[0][2], cy); |
967 eul1[2] = 0.0f; | 968 eul1[2] = 0.0f; |
968 | 969 » » |
969 copy_v3_v3(eul2, eul1); | 970 copy_v3_v3(eul2, eul1); |
970 } | 971 } |
971 } | 972 } |
972 | 973 |
973 /* XYZ order */ | 974 /* XYZ order */ |
974 void mat3_to_eul(float *eul, float tmat[3][3]) | 975 void mat3_to_eul(float *eul,float tmat[][3]) |
975 { | 976 { |
976 float eul1[3], eul2[3]; | 977 float eul1[3], eul2[3]; |
977 | 978 » |
978 mat3_to_eul2(tmat, eul1, eul2); | 979 mat3_to_eul2(tmat, eul1, eul2); |
979 | 980 » » |
980 /* return best, which is just the one with lowest values it in */ | 981 /* return best, which is just the one with lowest values it in */ |
981 » if (fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]) > fabsf(eul2[0]) +
fabsf(eul2[1]) + fabsf(eul2[2])) { | 982 » if(fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1
])+fabs(eul2[2])) { |
982 copy_v3_v3(eul, eul2); | 983 copy_v3_v3(eul, eul2); |
983 } | 984 } |
984 else { | 985 else { |
985 copy_v3_v3(eul, eul1); | 986 copy_v3_v3(eul, eul1); |
986 } | 987 } |
987 } | 988 } |
988 | 989 |
989 /* XYZ order */ | 990 /* XYZ order */ |
990 void mat4_to_eul(float *eul, float tmat[4][4]) | 991 void mat4_to_eul(float *eul,float tmat[][4]) |
991 { | 992 { |
992 float tempMat[3][3]; | 993 float tempMat[3][3]; |
993 | 994 |
994 copy_m3_m4(tempMat, tmat); | 995 copy_m3_m4(tempMat, tmat); |
995 normalize_m3(tempMat); | 996 normalize_m3(tempMat); |
996 » mat3_to_eul(eul, tempMat); | 997 » mat3_to_eul(eul,tempMat); |
997 } | 998 } |
998 | 999 |
999 /* XYZ order */ | 1000 /* XYZ order */ |
1000 void quat_to_eul(float *eul, const float quat[4]) | 1001 void quat_to_eul(float *eul, const float quat[4]) |
1001 { | 1002 { |
1002 float mat[3][3]; | 1003 float mat[3][3]; |
1003 | 1004 |
1004 » quat_to_mat3(mat, quat); | 1005 » quat_to_mat3(mat,quat); |
1005 » mat3_to_eul(eul, mat); | 1006 » mat3_to_eul(eul,mat); |
1006 } | 1007 } |
1007 | 1008 |
1008 /* XYZ order */ | 1009 /* XYZ order */ |
1009 void eul_to_quat(float quat[4], const float eul[3]) | 1010 void eul_to_quat(float *quat, const float eul[3]) |
1010 { | 1011 { |
1011 float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; | 1012 float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; |
1012 | 1013 |
1013 » ti = eul[0] * 0.5f; | 1014 » ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f; |
1014 » tj = eul[1] * 0.5f; | 1015 » ci = (float)cos(ti); cj = (float)cos(tj); ch = (float)cos(th); |
1015 » th = eul[2] * 0.5f; | 1016 » si = (float)sin(ti); sj = (float)sin(tj); sh = (float)sin(th); |
1016 » ci = cosf(ti); | 1017 » cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh; |
1017 » cj = cosf(tj); | 1018 »······· |
1018 » ch = cosf(th); | 1019 » quat[0] = cj*cc + sj*ss; |
1019 » si = sinf(ti); | 1020 » quat[1] = cj*sc - sj*cs; |
1020 » sj = sinf(tj); | 1021 » quat[2] = cj*ss + sj*cc; |
1021 » sh = sinf(th); | 1022 » quat[3] = cj*cs - sj*sc; |
1022 » cc = ci * ch; | |
1023 » cs = ci * sh; | |
1024 » sc = si * ch; | |
1025 » ss = si * sh; | |
1026 | |
1027 » quat[0] = cj * cc + sj * ss; | |
1028 » quat[1] = cj * sc - sj * cs; | |
1029 » quat[2] = cj * ss + sj * cc; | |
1030 » quat[3] = cj * cs - sj * sc; | |
1031 } | 1023 } |
1032 | 1024 |
1033 /* XYZ order */ | 1025 /* XYZ order */ |
1034 void rotate_eul(float beul[3], const char axis, const float ang) | 1026 void rotate_eul(float *beul, const char axis, const float ang) |
1035 { | 1027 { |
1036 float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; | 1028 float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; |
1037 | 1029 |
1038 assert(axis >= 'X' && axis <= 'Z'); | 1030 assert(axis >= 'X' && axis <= 'Z'); |
1039 | 1031 |
1040 » eul[0] = eul[1] = eul[2] = 0.0f; | 1032 » eul[0]= eul[1]= eul[2]= 0.0f; |
1041 » if (axis == 'X') eul[0] = ang; | 1033 » if(axis=='X') eul[0]= ang; |
1042 » else if (axis == 'Y') eul[1] = ang; | 1034 » else if(axis=='Y') eul[1]= ang; |
1043 » else eul[2] = ang; | 1035 » else eul[2]= ang; |
1044 | 1036 » |
1045 » eul_to_mat3(mat1, eul); | 1037 » eul_to_mat3(mat1,eul); |
1046 » eul_to_mat3(mat2, beul); | 1038 » eul_to_mat3(mat2,beul); |
1047 | 1039 » |
1048 mul_m3_m3m3(totmat, mat2, mat1); | 1040 mul_m3_m3m3(totmat, mat2, mat1); |
1049 | 1041 » |
1050 » mat3_to_eul(beul, totmat); | 1042 » mat3_to_eul(beul,totmat); |
1051 | 1043 » |
1052 } | 1044 } |
1053 | 1045 |
| 1046 /* exported to transform.c */ |
1054 /* order independent! */ | 1047 /* order independent! */ |
1055 void compatible_eul(float eul[3], const float oldrot[3]) | 1048 void compatible_eul(float eul[3], const float oldrot[3]) |
1056 { | 1049 { |
1057 » /* we could use M_PI as pi_thresh: which is correct but 5.1 gives better
results. | 1050 » float dx, dy, dz; |
1058 » * Checked with baking actions to fcurves - campbell */ | 1051 »······· |
1059 » const float pi_thresh = (5.1f); | |
1060 » const float pi_x2 = (2.0f * (float)M_PI); | |
1061 | |
1062 » float deul[3]; | |
1063 » unsigned int i; | |
1064 | |
1065 /* correct differences of about 360 degrees first */ | 1052 /* correct differences of about 360 degrees first */ |
1066 » for (i = 0; i < 3; i++) { | 1053 » dx= eul[0] - oldrot[0]; |
1067 » » deul[i] = eul[i] - oldrot[i]; | 1054 » dy= eul[1] - oldrot[1]; |
1068 » » if (deul[i] > pi_thresh) { | 1055 » dz= eul[2] - oldrot[2]; |
1069 » » » eul[i] -= floorf(( deul[i] / pi_x2) + 0.5f) * pi_x2; | 1056 »······· |
1070 » » » deul[i] = eul[i] - oldrot[i]; | 1057 » while(fabs(dx) > 5.1) { |
1071 » » } | 1058 » » if(dx > 0.0f) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(fl
oat)M_PI; |
1072 » » else if (deul[i] < -pi_thresh) { | 1059 » » dx= eul[0] - oldrot[0]; |
1073 » » » eul[i] += floorf((-deul[i] / pi_x2) + 0.5f) * pi_x2; | 1060 » } |
1074 » » » deul[i] = eul[i] - oldrot[i]; | 1061 » while(fabs(dy) > 5.1) { |
1075 » » } | 1062 » » if(dy > 0.0f) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(fl
oat)M_PI; |
1076 » } | 1063 » » dy= eul[1] - oldrot[1]; |
1077 | 1064 » } |
1078 » /* is 1 of the axis rotations larger than 180 degrees and the other smal
l? NO ELSE IF!! */ | 1065 » while(fabs(dz) > 5.1) { |
1079 » if (fabsf(deul[0]) > 3.2f && fabsf(deul[1]) < 1.6f && fabsf(deul[2]) < 1
.6f) { | 1066 » » if(dz > 0.0f) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(fl
oat)M_PI; |
1080 » » if (deul[0] > 0.0f) eul[0] -= pi_x2; | 1067 » » dz= eul[2] - oldrot[2]; |
1081 » » else eul[0] += pi_x2; | 1068 » } |
1082 » } | 1069 »······· |
1083 » if (fabsf(deul[1]) > 3.2f && fabsf(deul[2]) < 1.6f && fabsf(deul[0]) < 1
.6f) { | 1070 » /* is 1 of the axis rotations larger than 180 degrees and the other smal
l? NO ELSE IF!! */»····· |
1084 » » if (deul[1] > 0.0f) eul[1] -= pi_x2; | 1071 » if(fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6) { |
1085 » » else eul[1] += pi_x2; | 1072 » » if(dx > 0.0f) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(fl
oat)M_PI; |
1086 » } | 1073 » } |
1087 » if (fabsf(deul[2]) > 3.2f && fabsf(deul[0]) < 1.6f && fabsf(deul[1]) < 1
.6f) { | 1074 » if(fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6) { |
1088 » » if (deul[2] > 0.0f) eul[2] -= pi_x2; | 1075 » » if(dy > 0.0f) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(fl
oat)M_PI; |
1089 » » else eul[2] += pi_x2; | 1076 » } |
1090 » } | 1077 » if(fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6) { |
1091 | 1078 » » if(dz > 0.0f) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(fl
oat)M_PI; |
1092 #undef PI_THRESH | 1079 » } |
1093 #undef PI_2F | 1080 »······· |
| 1081 » /* the method below was there from ancient days... but why! probably bec
ause the code sucks :) |
| 1082 » » */ |
| 1083 #if 0»·· |
| 1084 » /* calc again */ |
| 1085 » dx= eul[0] - oldrot[0]; |
| 1086 » dy= eul[1] - oldrot[1]; |
| 1087 » dz= eul[2] - oldrot[2]; |
| 1088 »······· |
| 1089 » /* special case, tested for x-z */ |
| 1090 »······· |
| 1091 » if((fabs(dx) > 3.1 && fabs(dz) > 1.5) || (fabs(dx) > 1.5 && fabs(dz) > 3
.1)) { |
| 1092 » » if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI; |
| 1093 » » if(eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul
[1]; |
| 1094 » » if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI; |
| 1095 » »······· |
| 1096 » } |
| 1097 » else if((fabs(dx) > 3.1 && fabs(dy) > 1.5) || (fabs(dx) > 1.5 && fabs(dy
) > 3.1)) { |
| 1098 » » if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI; |
| 1099 » » if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI; |
| 1100 » » if(eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul
[2]; |
| 1101 » } |
| 1102 » else if((fabs(dy) > 3.1 && fabs(dz) > 1.5) || (fabs(dy) > 1.5 && fabs(dz
) > 3.1)) { |
| 1103 » » if(eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul
[0]; |
| 1104 » » if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI; |
| 1105 » » if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI; |
| 1106 » } |
| 1107 #endif»· |
1094 } | 1108 } |
1095 | 1109 |
1096 /* uses 2 methods to retrieve eulers, and picks the closest */ | 1110 /* uses 2 methods to retrieve eulers, and picks the closest */ |
1097 | |
1098 /* XYZ order */ | 1111 /* XYZ order */ |
1099 void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[3][3]
) | 1112 void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[][3]) |
1100 { | 1113 { |
1101 float eul1[3], eul2[3]; | 1114 float eul1[3], eul2[3]; |
1102 float d1, d2; | 1115 float d1, d2; |
1103 | 1116 » |
1104 mat3_to_eul2(mat, eul1, eul2); | 1117 mat3_to_eul2(mat, eul1, eul2); |
1105 | 1118 » |
1106 compatible_eul(eul1, oldrot); | 1119 compatible_eul(eul1, oldrot); |
1107 compatible_eul(eul2, oldrot); | 1120 compatible_eul(eul2, oldrot); |
1108 | 1121 » |
1109 » d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul
1[2] - oldrot[2]); | 1122 » d1= (float)fabs(eul1[0]-oldrot[0]) + (float)fabs(eul1[1]-oldrot[1]) + (f
loat)fabs(eul1[2]-oldrot[2]); |
1110 » d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul
2[2] - oldrot[2]); | 1123 » d2= (float)fabs(eul2[0]-oldrot[0]) + (float)fabs(eul2[1]-oldrot[1]) + (f
loat)fabs(eul2[2]-oldrot[2]); |
1111 | 1124 » |
1112 /* return best, which is just the one with lowest difference */ | 1125 /* return best, which is just the one with lowest difference */ |
1113 » if (d1 > d2) { | 1126 » if(d1 > d2) { |
1114 copy_v3_v3(eul, eul2); | 1127 copy_v3_v3(eul, eul2); |
1115 } | 1128 } |
1116 else { | 1129 else { |
1117 copy_v3_v3(eul, eul1); | 1130 copy_v3_v3(eul, eul1); |
1118 } | 1131 } |
1119 | 1132 » |
1120 } | 1133 } |
1121 | 1134 |
1122 /************************** Arbitrary Order Eulers ***************************/ | 1135 /************************** Arbitrary Order Eulers ***************************/ |
1123 | 1136 |
1124 /* Euler Rotation Order Code: | 1137 /* Euler Rotation Order Code: |
1125 * was adapted from | 1138 * was adapted from |
1126 * ANSI C code from the article | 1139 » » ANSI C code from the article |
1127 * "Euler Angle Conversion" | 1140 » » "Euler Angle Conversion" |
1128 * by Ken Shoemake, shoemake@graphics.cis.upenn.edu | 1141 » » by Ken Shoemake, shoemake@graphics.cis.upenn.edu |
1129 * in "Graphics Gems IV", Academic Press, 1994 | 1142 » » in "Graphics Gems IV", Academic Press, 1994 |
1130 * for use in Blender | 1143 * for use in Blender |
1131 */ | 1144 */ |
1132 | 1145 |
1133 /* Type for rotation order info - see wiki for derivation details */ | 1146 /* Type for rotation order info - see wiki for derivation details */ |
1134 typedef struct RotOrderInfo { | 1147 typedef struct RotOrderInfo { |
1135 short axis[3]; | 1148 short axis[3]; |
1136 » short parity; /* parity of axis permutation (even=0, odd=1) - 'n' in ori
ginal code */ | 1149 » short parity;» /* parity of axis permutation (even=0, odd=1) - 'n' in o
riginal code */ |
1137 } RotOrderInfo; | 1150 } RotOrderInfo; |
1138 | 1151 |
1139 /* Array of info for Rotation Order calculations | 1152 /* Array of info for Rotation Order calculations |
1140 * WARNING: must be kept in same order as eEulerRotationOrders | 1153 * WARNING: must be kept in same order as eEulerRotationOrders |
1141 */ | 1154 */ |
1142 static const RotOrderInfo rotOrders[] = { | 1155 static RotOrderInfo rotOrders[]= { |
1143 /* i, j, k, n */ | 1156 /* i, j, k, n */ |
1144 » {{0, 1, 2}, 0}, /* XYZ */ | 1157 » {{0, 1, 2}, 0}, // XYZ |
1145 » {{0, 2, 1}, 1}, /* XZY */ | 1158 » {{0, 2, 1}, 1}, // XZY |
1146 » {{1, 0, 2}, 1}, /* YXZ */ | 1159 » {{1, 0, 2}, 1}, // YXZ |
1147 » {{1, 2, 0}, 0}, /* YZX */ | 1160 » {{1, 2, 0}, 0}, // YZX |
1148 » {{2, 0, 1}, 0}, /* ZXY */ | 1161 » {{2, 0, 1}, 0}, // ZXY |
1149 » {{2, 1, 0}, 1} /* ZYX */ | 1162 » {{2, 1, 0}, 1} // ZYX |
1150 }; | 1163 }; |
1151 | 1164 |
1152 /* Get relevant pointer to rotation order set from the array | 1165 /* Get relevant pointer to rotation order set from the array |
1153 * NOTE: since we start at 1 for the values, but arrays index from 0, | 1166 * NOTE: since we start at 1 for the values, but arrays index from 0, |
1154 * there is -1 factor involved in this process... | 1167 * there is -1 factor involved in this process... |
1155 */ | 1168 */ |
1156 #define GET_ROTATIONORDER_INFO(order) (assert(order >= 0 && order <= 6), (order
< 1) ? &rotOrders[0] : &rotOrders[(order) - 1]) | 1169 #define GET_ROTATIONORDER_INFO(order) (assert(order>=0 && order<=6), (order < 1)
? &rotOrders[0] : &rotOrders[(order)-1]) |
1157 | 1170 |
1158 /* Construct quaternion from Euler angles (in radians). */ | 1171 /* Construct quaternion from Euler angles (in radians). */ |
1159 void eulO_to_quat(float q[4], const float e[3], const short order) | 1172 void eulO_to_quat(float q[4], const float e[3], const short order) |
1160 { | 1173 { |
1161 » const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); | 1174 » RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); |
1162 » short i = R->axis[0], j = R->axis[1], k = R->axis[2]; | 1175 » short i=R->axis[0], j=R->axis[1], » k=R->axis[2]; |
1163 double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; | 1176 double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; |
1164 double a[3]; | 1177 double a[3]; |
1165 | 1178 » |
1166 ti = e[i] * 0.5f; | 1179 ti = e[i] * 0.5f; |
1167 tj = e[j] * (R->parity ? -0.5f : 0.5f); | 1180 tj = e[j] * (R->parity ? -0.5f : 0.5f); |
1168 th = e[k] * 0.5f; | 1181 th = e[k] * 0.5f; |
1169 | 1182 |
1170 » ci = cos(ti); | 1183 » ci = cos(ti); cj = cos(tj); ch = cos(th); |
1171 » cj = cos(tj); | 1184 » si = sin(ti); sj = sin(tj); sh = sin(th); |
1172 » ch = cos(th); | 1185 » |
1173 » si = sin(ti); | 1186 » cc = ci*ch; cs = ci*sh; |
1174 » sj = sin(tj); | 1187 » sc = si*ch; ss = si*sh; |
1175 » sh = sin(th); | 1188 » |
1176 | 1189 » a[i] = cj*sc - sj*cs; |
1177 » cc = ci * ch; | 1190 » a[j] = cj*ss + sj*cc; |
1178 » cs = ci * sh; | 1191 » a[k] = cj*cs - sj*sc; |
1179 » sc = si * ch; | 1192 » |
1180 » ss = si * sh; | 1193 » q[0] = cj*cc + sj*ss; |
1181 | |
1182 » a[i] = cj * sc - sj * cs; | |
1183 » a[j] = cj * ss + sj * cc; | |
1184 » a[k] = cj * cs - sj * sc; | |
1185 | |
1186 » q[0] = cj * cc + sj * ss; | |
1187 q[1] = a[0]; | 1194 q[1] = a[0]; |
1188 q[2] = a[1]; | 1195 q[2] = a[1]; |
1189 q[3] = a[2]; | 1196 q[3] = a[2]; |
1190 | 1197 » |
1191 » if (R->parity) q[j + 1] = -q[j + 1]; | 1198 » if (R->parity) q[j+1] = -q[j+1]; |
1192 } | 1199 } |
1193 | 1200 |
1194 /* Convert quaternion to Euler angles (in radians). */ | 1201 /* Convert quaternion to Euler angles (in radians). */ |
1195 void quat_to_eulO(float e[3], short const order, const float q[4]) | 1202 void quat_to_eulO(float e[3], short const order, const float q[4]) |
1196 { | 1203 { |
1197 float M[3][3]; | 1204 float M[3][3]; |
1198 | 1205 » |
1199 » quat_to_mat3(M, q); | 1206 » quat_to_mat3(M,q); |
1200 » mat3_to_eulO(e, order, M); | 1207 » mat3_to_eulO(e, order,M); |
1201 } | 1208 } |
1202 | 1209 |
1203 /* Construct 3x3 matrix from Euler angles (in radians). */ | 1210 /* Construct 3x3 matrix from Euler angles (in radians). */ |
1204 void eulO_to_mat3(float M[3][3], const float e[3], const short order) | 1211 void eulO_to_mat3(float M[3][3], const float e[3], const short order) |
1205 { | 1212 { |
1206 » const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); | 1213 » RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); |
1207 » short i = R->axis[0], j = R->axis[1], k = R->axis[2]; | 1214 » short i=R->axis[0], j=R->axis[1], » k=R->axis[2]; |
1208 double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; | 1215 double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; |
1209 | 1216 » |
1210 if (R->parity) { | 1217 if (R->parity) { |
1211 » » ti = -e[i]; | 1218 » » ti = -e[i];» tj = -e[j];» th = -e[k]; |
1212 » » tj = -e[j]; | |
1213 » » th = -e[k]; | |
1214 } | 1219 } |
1215 else { | 1220 else { |
1216 » » ti = e[i]; | 1221 » » ti = e[i];» tj = e[j];» th = e[k]; |
1217 » » tj = e[j]; | 1222 » } |
1218 » » th = e[k]; | 1223 »······· |
1219 » } | 1224 » ci = cos(ti); cj = cos(tj); ch = cos(th); |
1220 | 1225 » si = sin(ti); sj = sin(tj); sh = sin(th); |
1221 » ci = cos(ti); | 1226 »······· |
1222 » cj = cos(tj); | 1227 » cc = ci*ch; cs = ci*sh;· |
1223 » ch = cos(th); | 1228 » sc = si*ch; ss = si*sh; |
1224 » si = sin(ti); | 1229 »······· |
1225 » sj = sin(tj); | 1230 » M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss; |
1226 » sh = sin(th); | 1231 » M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc; |
1227 | 1232 » M[i][k] = -sj;» M[j][k] = cj*si;» M[k][k] = cj*ci; |
1228 » cc = ci * ch; | |
1229 » cs = ci * sh; | |
1230 » sc = si * ch; | |
1231 » ss = si * sh; | |
1232 | |
1233 » M[i][i] = cj * ch; | |
1234 » M[j][i] = sj * sc - cs; | |
1235 » M[k][i] = sj * cc + ss; | |
1236 » M[i][j] = cj * sh; | |
1237 » M[j][j] = sj * ss + cc; | |
1238 » M[k][j] = sj * cs - sc; | |
1239 » M[i][k] = -sj; | |
1240 » M[j][k] = cj * si; | |
1241 » M[k][k] = cj * ci; | |
1242 } | 1233 } |
1243 | 1234 |
1244 /* returns two euler calculation methods, so we can pick the best */ | 1235 /* returns two euler calculation methods, so we can pick the best */ |
1245 static void mat3_to_eulo2(float M[3][3], float e1[3], float e2[3], const short o
rder) | 1236 static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, short order) |
1246 { | 1237 { |
1247 » const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); | 1238 » RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); |
1248 » short i = R->axis[0], j = R->axis[1], k = R->axis[2]; | 1239 » short i=R->axis[0], j=R->axis[1], » k=R->axis[2]; |
1249 float m[3][3]; | 1240 float m[3][3]; |
1250 double cy; | 1241 double cy; |
1251 | 1242 » |
1252 /* process the matrix first */ | 1243 /* process the matrix first */ |
1253 copy_m3_m3(m, M); | 1244 copy_m3_m3(m, M); |
1254 normalize_m3(m); | 1245 normalize_m3(m); |
1255 | 1246 » |
1256 » cy = sqrt(m[i][i] * m[i][i] + m[i][j] * m[i][j]); | 1247 » cy= sqrt(m[i][i]*m[i][i] + m[i][j]*m[i][j]); |
1257 | 1248 » |
1258 » if (cy > 16.0 * (double)FLT_EPSILON) { | 1249 » if (cy > 16.0*(double)FLT_EPSILON) { |
1259 e1[i] = atan2(m[j][k], m[k][k]); | 1250 e1[i] = atan2(m[j][k], m[k][k]); |
1260 e1[j] = atan2(-m[i][k], cy); | 1251 e1[j] = atan2(-m[i][k], cy); |
1261 e1[k] = atan2(m[i][j], m[i][i]); | 1252 e1[k] = atan2(m[i][j], m[i][i]); |
1262 | 1253 » » |
1263 e2[i] = atan2(-m[j][k], -m[k][k]); | 1254 e2[i] = atan2(-m[j][k], -m[k][k]); |
1264 e2[j] = atan2(-m[i][k], -cy); | 1255 e2[j] = atan2(-m[i][k], -cy); |
1265 e2[k] = atan2(-m[i][j], -m[i][i]); | 1256 e2[k] = atan2(-m[i][j], -m[i][i]); |
1266 » } | 1257 » } |
1267 else { | 1258 else { |
1268 e1[i] = atan2(-m[k][j], m[j][j]); | 1259 e1[i] = atan2(-m[k][j], m[j][j]); |
1269 e1[j] = atan2(-m[i][k], cy); | 1260 e1[j] = atan2(-m[i][k], cy); |
1270 e1[k] = 0; | 1261 e1[k] = 0; |
1271 | 1262 » » |
1272 copy_v3_v3(e2, e1); | 1263 copy_v3_v3(e2, e1); |
1273 } | 1264 } |
1274 | 1265 » |
1275 if (R->parity) { | 1266 if (R->parity) { |
1276 » » e1[0] = -e1[0]; | 1267 » » e1[0] = -e1[0]; |
1277 » » e1[1] = -e1[1]; | 1268 » » e1[1] = -e1[1]; |
1278 e1[2] = -e1[2]; | 1269 e1[2] = -e1[2]; |
1279 | 1270 » » |
1280 » » e2[0] = -e2[0]; | 1271 » » e2[0] = -e2[0]; |
1281 » » e2[1] = -e2[1]; | 1272 » » e2[1] = -e2[1]; |
1282 e2[2] = -e2[2]; | 1273 e2[2] = -e2[2]; |
1283 } | 1274 } |
1284 } | 1275 } |
1285 | 1276 |
1286 /* Construct 4x4 matrix from Euler angles (in radians). */ | 1277 /* Construct 4x4 matrix from Euler angles (in radians). */ |
1287 void eulO_to_mat4(float M[4][4], const float e[3], const short order) | 1278 void eulO_to_mat4(float M[4][4], const float e[3], const short order) |
1288 { | 1279 { |
1289 float m[3][3]; | 1280 float m[3][3]; |
1290 | 1281 » |
1291 /* for now, we'll just do this the slow way (i.e. copying matrices) */ | 1282 /* for now, we'll just do this the slow way (i.e. copying matrices) */ |
1292 normalize_m3(m); | 1283 normalize_m3(m); |
1293 » eulO_to_mat3(m, e, order); | 1284 » eulO_to_mat3(m,e, order); |
1294 copy_m4_m3(M, m); | 1285 copy_m4_m3(M, m); |
1295 } | 1286 } |
1296 | 1287 |
| 1288 |
1297 /* Convert 3x3 matrix to Euler angles (in radians). */ | 1289 /* Convert 3x3 matrix to Euler angles (in radians). */ |
1298 void mat3_to_eulO(float eul[3], const short order, float M[3][3]) | 1290 void mat3_to_eulO(float eul[3], const short order,float M[3][3]) |
1299 { | 1291 { |
1300 float eul1[3], eul2[3]; | 1292 float eul1[3], eul2[3]; |
1301 | 1293 » |
1302 mat3_to_eulo2(M, eul1, eul2, order); | 1294 mat3_to_eulo2(M, eul1, eul2, order); |
1303 | 1295 » » |
1304 /* return best, which is just the one with lowest values it in */ | 1296 /* return best, which is just the one with lowest values it in */ |
1305 » if (fabsf(eul1[0]) + fabsf(eul1[1]) + fabsf(eul1[2]) > fabsf(eul2[0]) +
fabsf(eul2[1]) + fabsf(eul2[2])) { | 1297 » if(fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1
])+fabs(eul2[2])) { |
1306 copy_v3_v3(eul, eul2); | 1298 copy_v3_v3(eul, eul2); |
1307 } | 1299 } |
1308 else { | 1300 else { |
1309 copy_v3_v3(eul, eul1); | 1301 copy_v3_v3(eul, eul1); |
1310 } | 1302 } |
1311 } | 1303 } |
1312 | 1304 |
1313 /* Convert 4x4 matrix to Euler angles (in radians). */ | 1305 /* Convert 4x4 matrix to Euler angles (in radians). */ |
1314 void mat4_to_eulO(float e[3], const short order, float M[4][4]) | 1306 void mat4_to_eulO(float e[3], const short order,float M[4][4]) |
1315 { | 1307 { |
1316 float m[3][3]; | 1308 float m[3][3]; |
1317 | 1309 » |
1318 /* for now, we'll just do this the slow way (i.e. copying matrices) */ | 1310 /* for now, we'll just do this the slow way (i.e. copying matrices) */ |
1319 copy_m3_m4(m, M); | 1311 copy_m3_m4(m, M); |
1320 normalize_m3(m); | 1312 normalize_m3(m); |
1321 » mat3_to_eulO(e, order, m); | 1313 » mat3_to_eulO(e, order,m); |
1322 } | 1314 } |
1323 | 1315 |
1324 /* uses 2 methods to retrieve eulers, and picks the closest */ | 1316 /* uses 2 methods to retrieve eulers, and picks the closest */ |
1325 void mat3_to_compatible_eulO(float eul[3], float oldrot[3], const short order, f
loat mat[3][3]) | 1317 void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order,float ma
t[3][3]) |
1326 { | 1318 { |
1327 float eul1[3], eul2[3]; | 1319 float eul1[3], eul2[3]; |
1328 float d1, d2; | 1320 float d1, d2; |
1329 | 1321 » |
1330 mat3_to_eulo2(mat, eul1, eul2, order); | 1322 mat3_to_eulo2(mat, eul1, eul2, order); |
1331 | 1323 » |
1332 compatible_eul(eul1, oldrot); | 1324 compatible_eul(eul1, oldrot); |
1333 compatible_eul(eul2, oldrot); | 1325 compatible_eul(eul2, oldrot); |
1334 | 1326 » |
1335 » d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul
1[2] - oldrot[2]); | 1327 » d1= (float)fabs(eul1[0]-oldrot[0]) + (float)fabs(eul1[1]-oldrot[1]) + (f
loat)fabs(eul1[2]-oldrot[2]); |
1336 » d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul
2[2] - oldrot[2]); | 1328 » d2= (float)fabs(eul2[0]-oldrot[0]) + (float)fabs(eul2[1]-oldrot[1]) + (f
loat)fabs(eul2[2]-oldrot[2]); |
1337 | 1329 » |
1338 /* return best, which is just the one with lowest difference */ | 1330 /* return best, which is just the one with lowest difference */ |
1339 if (d1 > d2) | 1331 if (d1 > d2) |
1340 copy_v3_v3(eul, eul2); | 1332 copy_v3_v3(eul, eul2); |
1341 else | 1333 else |
1342 copy_v3_v3(eul, eul1); | 1334 copy_v3_v3(eul, eul1); |
1343 } | 1335 } |
1344 | 1336 |
1345 void mat4_to_compatible_eulO(float eul[3], float oldrot[3], const short order, f
loat M[4][4]) | 1337 void mat4_to_compatible_eulO(float eul[3], float oldrot[3], short order,float M[
4][4]) |
1346 { | 1338 { |
1347 float m[3][3]; | 1339 float m[3][3]; |
1348 | 1340 » |
1349 /* for now, we'll just do this the slow way (i.e. copying matrices) */ | 1341 /* for now, we'll just do this the slow way (i.e. copying matrices) */ |
1350 copy_m3_m4(m, M); | 1342 copy_m3_m4(m, M); |
1351 normalize_m3(m); | 1343 normalize_m3(m); |
1352 mat3_to_compatible_eulO(eul, oldrot, order, m); | 1344 mat3_to_compatible_eulO(eul, oldrot, order, m); |
1353 } | 1345 } |
1354 /* rotate the given euler by the given angle on the specified axis */ | 1346 /* rotate the given euler by the given angle on the specified axis */ |
1355 /* NOTE: is this safe to do with different axis orders? */ | 1347 // NOTE: is this safe to do with different axis orders? |
1356 | 1348 void rotate_eulO(float beul[3], short order, char axis, float ang) |
1357 void rotate_eulO(float beul[3], const short order, char axis, float ang) | |
1358 { | 1349 { |
1359 float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; | 1350 float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; |
1360 | 1351 |
1361 assert(axis >= 'X' && axis <= 'Z'); | 1352 assert(axis >= 'X' && axis <= 'Z'); |
1362 | 1353 |
1363 » eul[0] = eul[1] = eul[2] = 0.0f; | 1354 » eul[0]= eul[1]= eul[2]= 0.0f; |
1364 » if (axis == 'X') | 1355 » if (axis=='X') |
1365 » » eul[0] = ang; | 1356 » » eul[0]= ang; |
1366 » else if (axis == 'Y') | 1357 » else if (axis=='Y') |
1367 » » eul[1] = ang; | 1358 » » eul[1]= ang; |
1368 » else | 1359 » else |
1369 » » eul[2] = ang; | 1360 » » eul[2]= ang; |
1370 | 1361 » |
1371 » eulO_to_mat3(mat1, eul, order); | 1362 » eulO_to_mat3(mat1,eul, order); |
1372 » eulO_to_mat3(mat2, beul, order); | 1363 » eulO_to_mat3(mat2,beul, order); |
1373 | 1364 » |
1374 mul_m3_m3m3(totmat, mat2, mat1); | 1365 mul_m3_m3m3(totmat, mat2, mat1); |
1375 | 1366 » |
1376 » mat3_to_eulO(beul, order, totmat); | 1367 » mat3_to_eulO(beul, order,totmat); |
1377 } | 1368 } |
1378 | 1369 |
1379 /* the matrix is written to as 3 axis vectors */ | 1370 /* the matrix is written to as 3 axis vectors */ |
1380 void eulO_to_gimbal_axis(float gmat[3][3], const float eul[3], const short order
) | 1371 void eulO_to_gimbal_axis(float gmat[][3], const float eul[3], const short order) |
1381 { | 1372 { |
1382 » const RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); | 1373 » RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); |
1383 | 1374 |
1384 float mat[3][3]; | 1375 float mat[3][3]; |
1385 float teul[3]; | 1376 float teul[3]; |
1386 | 1377 |
1387 /* first axis is local */ | 1378 /* first axis is local */ |
1388 » eulO_to_mat3(mat, eul, order); | 1379 » eulO_to_mat3(mat,eul, order); |
1389 copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]); | 1380 copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]); |
1390 | 1381 » |
1391 /* second axis is local minus first rotation */ | 1382 /* second axis is local minus first rotation */ |
1392 copy_v3_v3(teul, eul); | 1383 copy_v3_v3(teul, eul); |
1393 teul[R->axis[0]] = 0; | 1384 teul[R->axis[0]] = 0; |
1394 » eulO_to_mat3(mat, teul, order); | 1385 » eulO_to_mat3(mat,teul, order); |
1395 copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]); | 1386 copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]); |
1396 | 1387 » |
1397 | 1388 » |
1398 /* Last axis is global */ | 1389 /* Last axis is global */ |
1399 gmat[R->axis[2]][0] = 0; | 1390 gmat[R->axis[2]][0] = 0; |
1400 gmat[R->axis[2]][1] = 0; | 1391 gmat[R->axis[2]][1] = 0; |
1401 gmat[R->axis[2]][2] = 0; | 1392 gmat[R->axis[2]][2] = 0; |
1402 gmat[R->axis[2]][R->axis[2]] = 1; | 1393 gmat[R->axis[2]][R->axis[2]] = 1; |
1403 } | 1394 } |
1404 | 1395 |
1405 /******************************* Dual Quaternions ****************************/ | 1396 /******************************* Dual Quaternions ****************************/ |
1406 | 1397 |
1407 /** | 1398 /* |
1408 * Conversion routines between (regular quaternion, translation) and | 1399 Conversion routines between (regular quaternion, translation) and |
1409 * dual quaternion. | 1400 dual quaternion. |
1410 * | 1401 |
1411 * Version 1.0.0, February 7th, 2007 | 1402 Version 1.0.0, February 7th, 2007 |
1412 * | 1403 |
1413 * Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights | 1404 Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights· |
1414 * Reserved | 1405 Reserved |
1415 * | 1406 |
1416 * This software is provided 'as-is', without any express or implied | 1407 This software is provided 'as-is', without any express or implied |
1417 * warranty. In no event will the author(s) be held liable for any damages | 1408 warranty. In no event will the author(s) be held liable for any damages |
1418 * arising from the use of this software. | 1409 arising from the use of this software. |
1419 * | 1410 |
1420 * Permission is granted to anyone to use this software for any purpose, | 1411 Permission is granted to anyone to use this software for any purpose, |
1421 * including commercial applications, and to alter it and redistribute it | 1412 including commercial applications, and to alter it and redistribute it |
1422 * freely, subject to the following restrictions: | 1413 freely, subject to the following restrictions: |
1423 * | 1414 |
1424 * 1. The origin of this software must not be misrepresented; you must not | 1415 1. The origin of this software must not be misrepresented; you must not |
1425 * claim that you wrote the original software. If you use this software | 1416 » claim that you wrote the original software. If you use this software |
1426 * in a product, an acknowledgment in the product documentation would be | 1417 » in a product, an acknowledgment in the product documentation would be |
1427 * appreciated but is not required. | 1418 » appreciated but is not required. |
1428 * 2. Altered source versions must be plainly marked as such, and must not be | 1419 2. Altered source versions must be plainly marked as such, and must not be |
1429 * misrepresented as being the original software. | 1420 » misrepresented as being the original software. |
1430 * 3. This notice may not be removed or altered from any source distribution. | 1421 3. This notice may not be removed or altered from any source distribution. |
1431 * | 1422 |
1432 * \author Ladislav Kavan, kavanl@cs.tcd.ie | 1423 Author: Ladislav Kavan, kavanl@cs.tcd.ie |
1433 * | 1424 |
1434 * Changes for Blender: | 1425 Changes for Blender: |
1435 * - renaming, style changes and optimization's | 1426 - renaming, style changes and optimizations |
1436 * - added support for scaling | 1427 - added support for scaling |
1437 */ | 1428 */ |
1438 | 1429 |
1439 void mat4_to_dquat(DualQuat *dq, float basemat[4][4], float mat[4][4]) | 1430 void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4]) |
1440 { | 1431 { |
1441 float *t, *q, dscale[3], scale[3], basequat[4]; | 1432 float *t, *q, dscale[3], scale[3], basequat[4]; |
1442 float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4]; | 1433 float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4]; |
1443 float R[4][4], S[4][4]; | 1434 float R[4][4], S[4][4]; |
1444 | 1435 |
1445 /* split scaling and rotation, there is probably a faster way to do | 1436 /* split scaling and rotation, there is probably a faster way to do |
1446 » * this, it's done like this now to correctly get negative scaling */ | 1437 » this, it's done like this now to correctly get negative scaling */ |
1447 » mul_m4_m4m4(baseRS, mat, basemat); | 1438 » mul_m4_m4m4(baseRS, basemat, mat); |
1448 » mat4_to_size(scale, baseRS); | 1439 » mat4_to_size(scale,baseRS); |
1449 | 1440 |
1450 » dscale[0] = scale[0] - 1.0f; | 1441 » copy_v3_v3(dscale, scale); |
1451 » dscale[1] = scale[1] - 1.0f; | 1442 » dscale[0] -= 1.0f; dscale[1] -= 1.0f; dscale[2] -= 1.0f; |
1452 » dscale[2] = scale[2] - 1.0f; | 1443 |
1453 | 1444 » if((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) { |
1454 » if ((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) { | |
1455 /* extract R and S */ | 1445 /* extract R and S */ |
1456 float tmp[4][4]; | 1446 float tmp[4][4]; |
1457 | 1447 |
1458 » » /* extra orthogonalize, to avoid flipping with stretched bones *
/ | 1448 » » /* extra orthogonalize, to avoid flipping with stretched bones
*/ |
1459 copy_m4_m4(tmp, baseRS); | 1449 copy_m4_m4(tmp, baseRS); |
1460 orthogonalize_m4(tmp, 1); | 1450 orthogonalize_m4(tmp, 1); |
1461 mat4_to_quat(basequat, tmp); | 1451 mat4_to_quat(basequat, tmp); |
1462 | 1452 |
1463 quat_to_mat4(baseR, basequat); | 1453 quat_to_mat4(baseR, basequat); |
1464 copy_v3_v3(baseR[3], baseRS[3]); | 1454 copy_v3_v3(baseR[3], baseRS[3]); |
1465 | 1455 |
1466 invert_m4_m4(baseinv, basemat); | 1456 invert_m4_m4(baseinv, basemat); |
1467 » » mul_m4_m4m4(R, baseR, baseinv); | 1457 » » mul_m4_m4m4(R, baseinv, baseR); |
1468 | 1458 |
1469 invert_m4_m4(baseRinv, baseR); | 1459 invert_m4_m4(baseRinv, baseR); |
1470 » » mul_m4_m4m4(S, baseRinv, baseRS); | 1460 » » mul_m4_m4m4(S, baseRS, baseRinv); |
1471 | 1461 |
1472 /* set scaling part */ | 1462 /* set scaling part */ |
1473 mul_serie_m4(dq->scale, basemat, S, baseinv, NULL, NULL, NULL, N
ULL, NULL); | 1463 mul_serie_m4(dq->scale, basemat, S, baseinv, NULL, NULL, NULL, N
ULL, NULL); |
1474 » » dq->scale_weight = 1.0f; | 1464 » » dq->scale_weight= 1.0f; |
1475 } | 1465 } |
1476 else { | 1466 else { |
1477 /* matrix does not contain scaling */ | 1467 /* matrix does not contain scaling */ |
1478 copy_m4_m4(R, mat); | 1468 copy_m4_m4(R, mat); |
1479 » » dq->scale_weight = 0.0f; | 1469 » » dq->scale_weight= 0.0f; |
1480 } | 1470 } |
1481 | 1471 |
1482 /* non-dual part */ | 1472 /* non-dual part */ |
1483 » mat4_to_quat(dq->quat, R); | 1473 » mat4_to_quat(dq->quat,R); |
1484 | 1474 |
1485 /* dual part */ | 1475 /* dual part */ |
1486 » t = R[3]; | 1476 » t= R[3]; |
1487 » q = dq->quat; | 1477 » q= dq->quat; |
1488 » dq->trans[0] = -0.5f * ( t[0] * q[1] + t[1] * q[2] + t[2] * q[3]); | 1478 » dq->trans[0]= -0.5f*(t[0]*q[1] + t[1]*q[2] + t[2]*q[3]); |
1489 » dq->trans[1] = 0.5f * ( t[0] * q[0] + t[1] * q[3] - t[2] * q[2]); | 1479 » dq->trans[1]= 0.5f*(t[0]*q[0] + t[1]*q[3] - t[2]*q[2]); |
1490 » dq->trans[2] = 0.5f * (-t[0] * q[3] + t[1] * q[0] + t[2] * q[1]); | 1480 » dq->trans[2]= 0.5f*(-t[0]*q[3] + t[1]*q[0] + t[2]*q[1]); |
1491 » dq->trans[3] = 0.5f * ( t[0] * q[2] - t[1] * q[1] + t[2] * q[0]); | 1481 » dq->trans[3]= 0.5f*(t[0]*q[2] - t[1]*q[1] + t[2]*q[0]); |
1492 } | 1482 } |
1493 | 1483 |
1494 void dquat_to_mat4(float mat[4][4], const DualQuat *dq) | 1484 void dquat_to_mat4(float mat[][4], DualQuat *dq) |
1495 { | 1485 { |
1496 » float len, q0[4]; | 1486 » float len, *t, q0[4]; |
1497 » const float *t; | 1487 »······· |
1498 | |
1499 /* regular quaternion */ | 1488 /* regular quaternion */ |
1500 copy_qt_qt(q0, dq->quat); | 1489 copy_qt_qt(q0, dq->quat); |
1501 | 1490 |
1502 /* normalize */ | 1491 /* normalize */ |
1503 » len = sqrtf(dot_qtqt(q0, q0)); | 1492 » len= (float)sqrt(dot_qtqt(q0, q0)); |
1504 » if (len != 0.0f) | 1493 » if(len != 0.0f) |
1505 » » mul_qt_fl(q0, 1.0f / len); | 1494 » » mul_qt_fl(q0, 1.0f/len); |
1506 | 1495 » |
1507 /* rotation */ | 1496 /* rotation */ |
1508 » quat_to_mat4(mat, q0); | 1497 » quat_to_mat4(mat,q0); |
1509 | 1498 |
1510 /* translation */ | 1499 /* translation */ |
1511 » t = dq->trans; | 1500 » t= dq->trans; |
1512 » mat[3][0] = 2.0f * (-t[0] * q0[1] + t[1] * q0[0] - t[2] * q0[3] + t[3] *
q0[2]); | 1501 » mat[3][0]= 2.0f*(-t[0]*q0[1] + t[1]*q0[0] - t[2]*q0[3] + t[3]*q0[2]); |
1513 » mat[3][1] = 2.0f * (-t[0] * q0[2] + t[1] * q0[3] + t[2] * q0[0] - t[3] *
q0[1]); | 1502 » mat[3][1]= 2.0f*(-t[0]*q0[2] + t[1]*q0[3] + t[2]*q0[0] - t[3]*q0[1]); |
1514 » mat[3][2] = 2.0f * (-t[0] * q0[3] - t[1] * q0[2] + t[2] * q0[1] + t[3] *
q0[0]); | 1503 » mat[3][2]= 2.0f*(-t[0]*q0[3] - t[1]*q0[2] + t[2]*q0[1] + t[3]*q0[0]); |
1515 | 1504 |
1516 /* note: this does not handle scaling */ | 1505 /* note: this does not handle scaling */ |
1517 } | 1506 }» |
1518 | 1507 |
1519 void add_weighted_dq_dq(DualQuat *dqsum, const DualQuat *dq, float weight) | 1508 void add_weighted_dq_dq(DualQuat *dqsum, DualQuat *dq, float weight) |
1520 { | 1509 { |
1521 » int flipped = 0; | 1510 » int flipped= 0; |
1522 | 1511 |
1523 /* make sure we interpolate quats in the right direction */ | 1512 /* make sure we interpolate quats in the right direction */ |
1524 if (dot_qtqt(dq->quat, dqsum->quat) < 0) { | 1513 if (dot_qtqt(dq->quat, dqsum->quat) < 0) { |
1525 » » flipped = 1; | 1514 » » flipped= 1; |
1526 » » weight = -weight; | 1515 » » weight= -weight; |
1527 } | 1516 } |
1528 | 1517 |
1529 /* interpolate rotation and translation */ | 1518 /* interpolate rotation and translation */ |
1530 » dqsum->quat[0] += weight * dq->quat[0]; | 1519 » dqsum->quat[0] += weight*dq->quat[0]; |
1531 » dqsum->quat[1] += weight * dq->quat[1]; | 1520 » dqsum->quat[1] += weight*dq->quat[1]; |
1532 » dqsum->quat[2] += weight * dq->quat[2]; | 1521 » dqsum->quat[2] += weight*dq->quat[2]; |
1533 » dqsum->quat[3] += weight * dq->quat[3]; | 1522 » dqsum->quat[3] += weight*dq->quat[3]; |
1534 | 1523 |
1535 » dqsum->trans[0] += weight * dq->trans[0]; | 1524 » dqsum->trans[0] += weight*dq->trans[0]; |
1536 » dqsum->trans[1] += weight * dq->trans[1]; | 1525 » dqsum->trans[1] += weight*dq->trans[1]; |
1537 » dqsum->trans[2] += weight * dq->trans[2]; | 1526 » dqsum->trans[2] += weight*dq->trans[2]; |
1538 » dqsum->trans[3] += weight * dq->trans[3]; | 1527 » dqsum->trans[3] += weight*dq->trans[3]; |
1539 | 1528 |
1540 /* interpolate scale - but only if needed */ | 1529 /* interpolate scale - but only if needed */ |
1541 if (dq->scale_weight) { | 1530 if (dq->scale_weight) { |
1542 float wmat[4][4]; | 1531 float wmat[4][4]; |
1543 | 1532 » » |
1544 » » if (flipped) /* we don't want negative weights for scaling */ | 1533 » » if(flipped)» /* we don't want negative weights for scaling */ |
1545 » » » weight = -weight; | 1534 » » » weight= -weight; |
1546 | 1535 » » |
1547 » » copy_m4_m4(wmat, (float(*)[4])dq->scale); | 1536 » » copy_m4_m4(wmat, dq->scale); |
1548 mul_m4_fl(wmat, weight); | 1537 mul_m4_fl(wmat, weight); |
1549 add_m4_m4m4(dqsum->scale, dqsum->scale, wmat); | 1538 add_m4_m4m4(dqsum->scale, dqsum->scale, wmat); |
1550 dqsum->scale_weight += weight; | 1539 dqsum->scale_weight += weight; |
1551 } | 1540 } |
1552 } | 1541 } |
1553 | 1542 |
1554 void normalize_dq(DualQuat *dq, float totweight) | 1543 void normalize_dq(DualQuat *dq, float totweight) |
1555 { | 1544 { |
1556 » float scale = 1.0f / totweight; | 1545 » float scale= 1.0f/totweight; |
1557 | 1546 |
1558 mul_qt_fl(dq->quat, scale); | 1547 mul_qt_fl(dq->quat, scale); |
1559 mul_qt_fl(dq->trans, scale); | 1548 mul_qt_fl(dq->trans, scale); |
1560 | 1549 » |
1561 » if (dq->scale_weight) { | 1550 » if(dq->scale_weight) { |
1562 » » float addweight = totweight - dq->scale_weight; | 1551 » » float addweight= totweight - dq->scale_weight; |
1563 | 1552 » » |
1564 » » if (addweight) { | 1553 » » if(addweight) { |
1565 dq->scale[0][0] += addweight; | 1554 dq->scale[0][0] += addweight; |
1566 dq->scale[1][1] += addweight; | 1555 dq->scale[1][1] += addweight; |
1567 dq->scale[2][2] += addweight; | 1556 dq->scale[2][2] += addweight; |
1568 dq->scale[3][3] += addweight; | 1557 dq->scale[3][3] += addweight; |
1569 } | 1558 } |
1570 | 1559 |
1571 mul_m4_fl(dq->scale, scale); | 1560 mul_m4_fl(dq->scale, scale); |
1572 » » dq->scale_weight = 1.0f; | 1561 » » dq->scale_weight= 1.0f; |
1573 » } | 1562 » } |
1574 } | 1563 } |
1575 | 1564 |
1576 void mul_v3m3_dq(float co[3], float mat[3][3], DualQuat *dq) | 1565 void mul_v3m3_dq(float *co, float mat[][3],DualQuat *dq) |
1577 { | 1566 {» |
1578 float M[3][3], t[3], scalemat[3][3], len2; | 1567 float M[3][3], t[3], scalemat[3][3], len2; |
1579 » float w = dq->quat[0], x = dq->quat[1], y = dq->quat[2], z = dq->quat[3]
; | 1568 » float w= dq->quat[0], x= dq->quat[1], y= dq->quat[2], z= dq->quat[3]; |
1580 » float t0 = dq->trans[0], t1 = dq->trans[1], t2 = dq->trans[2], t3 = dq->
trans[3]; | 1569 » float t0= dq->trans[0], t1= dq->trans[1], t2= dq->trans[2], t3= dq->tran
s[3]; |
1581 | 1570 » |
1582 /* rotation matrix */ | 1571 /* rotation matrix */ |
1583 » M[0][0] = w * w + x * x - y * y - z * z; | 1572 » M[0][0]= w*w + x*x - y*y - z*z; |
1584 » M[1][0] = 2 * (x * y - w * z); | 1573 » M[1][0]= 2*(x*y - w*z); |
1585 » M[2][0] = 2 * (x * z + w * y); | 1574 » M[2][0]= 2*(x*z + w*y); |
1586 | 1575 |
1587 » M[0][1] = 2 * (x * y + w * z); | 1576 » M[0][1]= 2*(x*y + w*z); |
1588 » M[1][1] = w * w + y * y - x * x - z * z; | 1577 » M[1][1]= w*w + y*y - x*x - z*z; |
1589 » M[2][1] = 2 * (y * z - w * x); | 1578 » M[2][1]= 2*(y*z - w*x);· |
1590 | 1579 |
1591 » M[0][2] = 2 * (x * z - w * y); | 1580 » M[0][2]= 2*(x*z - w*y); |
1592 » M[1][2] = 2 * (y * z + w * x); | 1581 » M[1][2]= 2*(y*z + w*x); |
1593 » M[2][2] = w * w + z * z - x * x - y * y; | 1582 » M[2][2]= w*w + z*z - x*x - y*y; |
1594 | 1583 »······· |
1595 » len2 = dot_qtqt(dq->quat, dq->quat); | 1584 » len2= dot_qtqt(dq->quat, dq->quat); |
1596 » if (len2 > 0.0f) | 1585 » if(len2 > 0.0f) |
1597 » » len2 = 1.0f / len2; | 1586 » » len2= 1.0f/len2; |
1598 | 1587 »······· |
1599 /* translation */ | 1588 /* translation */ |
1600 » t[0] = 2 * (-t0 * x + w * t1 - t2 * z + y * t3); | 1589 » t[0]= 2*(-t0*x + w*t1 - t2*z + y*t3); |
1601 » t[1] = 2 * (-t0 * y + t1 * z - x * t3 + w * t2); | 1590 » t[1]= 2*(-t0*y + t1*z - x*t3 + w*t2); |
1602 » t[2] = 2 * (-t0 * z + x * t2 + w * t3 - t1 * y); | 1591 » t[2]= 2*(-t0*z + x*t2 + w*t3 - t1*y); |
1603 | 1592 |
1604 /* apply scaling */ | 1593 /* apply scaling */ |
1605 » if (dq->scale_weight) | 1594 » if(dq->scale_weight) |
1606 mul_m4_v3(dq->scale, co); | 1595 mul_m4_v3(dq->scale, co); |
1607 | 1596 » |
1608 /* apply rotation and translation */ | 1597 /* apply rotation and translation */ |
1609 mul_m3_v3(M, co); | 1598 mul_m3_v3(M, co); |
1610 » co[0] = (co[0] + t[0]) * len2; | 1599 » co[0]= (co[0] + t[0])*len2; |
1611 » co[1] = (co[1] + t[1]) * len2; | 1600 » co[1]= (co[1] + t[1])*len2; |
1612 » co[2] = (co[2] + t[2]) * len2; | 1601 » co[2]= (co[2] + t[2])*len2; |
1613 | 1602 |
1614 /* compute crazyspace correction mat */ | 1603 /* compute crazyspace correction mat */ |
1615 » if (mat) { | 1604 » if(mat) { |
1616 » » if (dq->scale_weight) { | 1605 » » if(dq->scale_weight) { |
1617 copy_m3_m4(scalemat, dq->scale); | 1606 copy_m3_m4(scalemat, dq->scale); |
1618 mul_m3_m3m3(mat, M, scalemat); | 1607 mul_m3_m3m3(mat, M, scalemat); |
1619 } | 1608 } |
1620 else | 1609 else |
1621 copy_m3_m3(mat, M); | 1610 copy_m3_m3(mat, M); |
1622 mul_m3_fl(mat, len2); | 1611 mul_m3_fl(mat, len2); |
1623 } | 1612 } |
1624 } | 1613 } |
1625 | 1614 |
1626 void copy_dq_dq(DualQuat *dq1, const DualQuat *dq2) | 1615 void copy_dq_dq(DualQuat *dq1, DualQuat *dq2) |
1627 { | 1616 { |
1628 memcpy(dq1, dq2, sizeof(DualQuat)); | 1617 memcpy(dq1, dq2, sizeof(DualQuat)); |
1629 } | 1618 } |
1630 | 1619 |
1631 /* axis matches eTrackToAxis_Modes */ | 1620 /* axis matches eTrackToAxis_Modes */ |
1632 void quat_apply_track(float quat[4], short axis, short upflag) | 1621 void quat_apply_track(float quat[4], short axis, short upflag) |
1633 { | 1622 {» |
1634 /* rotations are hard coded to match vec_to_quat */ | 1623 /* rotations are hard coded to match vec_to_quat */ |
1635 » const float quat_track[][4] = { | 1624 » const float quat_track[][4]= {{0.70710676908493, 0.0, -0.70710676908493,
0.0}, /* pos-y90 */· |
1636 » » {M_SQRT1_2, 0.0, -M_SQRT1_2, 0.0}, /* pos-y90 */ | 1625 » {0.5, 0.5, 0.5, 0.5}, /* Quaternion((1,0,
0), radians(90)) * Quaternion((0,1,0), radians(90)) */· |
1637 » » {0.5, 0.5, 0.5, 0.5}, /* Quaternion((1,0,0), radians(90)) * Quat
ernion((0,1,0), radians(90)) */ | 1626 » {0.70710676908493, 0.0, 0.0, 0.70710676908
493}, /* pos-z90 */· |
1638 » » {M_SQRT1_2, 0.0, 0.0, M_SQRT1_2}, /* pos-z90 */ | 1627 » {0.70710676908493, 0.0, 0.70710676908493,
0.0}, /* neg-y90 */· |
1639 » » {M_SQRT1_2, 0.0, M_SQRT1_2, 0.0}, /* neg-y90 */ | 1628 » {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0
,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */· |
1640 » » {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0,0), radians(-90)) * Q
uaternion((0,1,0), radians(-90)) */ | 1629 » {-3.0908619663705394e-08, 0.70710676908493
, 0.70710676908493, 3.0908619663705394e-08}}; /* no rotation */ |
1641 » » {0.0, M_SQRT1_2, M_SQRT1_2, 0.0} /* no rotation */ | |
1642 » }; | |
1643 | 1630 |
1644 assert(axis >= 0 && axis <= 5); | 1631 assert(axis >= 0 && axis <= 5); |
1645 assert(upflag >= 0 && upflag <= 2); | 1632 assert(upflag >= 0 && upflag <= 2); |
1646 | 1633 » |
1647 mul_qt_qtqt(quat, quat, quat_track[axis]); | 1634 mul_qt_qtqt(quat, quat, quat_track[axis]); |
1648 | 1635 |
1649 » if (axis > 2) | 1636 » if(axis>2) |
1650 » » axis = axis - 3; | 1637 » » axis= axis-3; |
1651 | 1638 |
1652 /* there are 2 possible up-axis for each axis used, the 'quat_track' app
lies so the first | 1639 /* there are 2 possible up-axis for each axis used, the 'quat_track' app
lies so the first |
1653 » * up axis is used X->Y, Y->X, Z->X, if this first up axis isn't used th
en rotate 90d | 1640 » * up axis is used X->Y, Y->X, Z->X, if this first up axis isn used then
rotate 90d |
1654 * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */ | 1641 * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */ |
1655 » if (upflag != (2 - axis) >> 1) { | 1642 » if(upflag != (2-axis)>>1) { |
1656 » » float q[4] = {M_SQRT1_2, 0.0, 0.0, 0.0}; /* assign 90d rotation
axis */ | 1643 » » float q[4]= {0.70710676908493, 0.0, 0.0, 0.0}; /* assign 90d rot
ation axis */ |
1657 » » q[axis + 1] = ((axis == 1)) ? M_SQRT1_2 : -M_SQRT1_2; /* flip no
n Y axis */ | 1644 » » q[axis+1] = ((axis==1)) ? 0.70710676908493 : -0.70710676908493;
/* flip non Y axis */ |
1658 mul_qt_qtqt(quat, quat, q); | 1645 mul_qt_qtqt(quat, quat, q); |
1659 } | 1646 } |
1660 } | 1647 } |
1661 | 1648 |
| 1649 |
1662 void vec_apply_track(float vec[3], short axis) | 1650 void vec_apply_track(float vec[3], short axis) |
1663 { | 1651 { |
1664 float tvec[3]; | 1652 float tvec[3]; |
1665 | 1653 |
1666 assert(axis >= 0 && axis <= 5); | 1654 assert(axis >= 0 && axis <= 5); |
1667 | 1655 » |
1668 copy_v3_v3(tvec, vec); | 1656 copy_v3_v3(tvec, vec); |
1669 | 1657 |
1670 » switch (axis) { | 1658 » switch(axis) { |
1671 » » case 0: /* pos-x */ | 1659 » case 0: /* pos-x */ |
1672 » » » /* vec[0] = 0.0; */ | 1660 » » /* vec[0]= 0.0; */ |
1673 » » » vec[1] = tvec[2]; | 1661 » » vec[1]= tvec[2]; |
1674 » » » vec[2] = -tvec[1]; | 1662 » » vec[2]= -tvec[1]; |
1675 » » » break; | 1663 » » break; |
1676 » » case 1: /* pos-y */ | 1664 » case 1: /* pos-y */ |
1677 » » » /* vec[0] = tvec[0]; */ | 1665 » » /* vec[0]= tvec[0]; */ |
1678 » » » /* vec[1] = 0.0; */ | 1666 » » /* vec[1]= 0.0; */ |
1679 » » » /* vec[2] = tvec[2]; */ | 1667 » » /* vec[2]= tvec[2]; */· |
1680 » » » break; | 1668 » » break; |
1681 » » case 2: /* pos-z */ | 1669 » case 2: /* pos-z */ |
1682 » » » /* vec[0] = tvec[0]; */ | 1670 » » /* vec[0]= tvec[0]; */ |
1683 » » » /* vec[1] = tvec[1]; */ | 1671 » » /* vec[1]= tvec[1]; */ |
1684 » » » /* vec[2] = 0.0; */ | 1672 » » // vec[2]= 0.0; */ |
1685 » » » break; | 1673 » » break; |
1686 » » case 3: /* neg-x */ | 1674 » case 3: /* neg-x */ |
1687 » » » /* vec[0] = 0.0; */ | 1675 » » /* vec[0]= 0.0; */ |
1688 » » » vec[1] = tvec[2]; | 1676 » » vec[1]= tvec[2]; |
1689 » » » vec[2] = -tvec[1]; | 1677 » » vec[2]= -tvec[1]; |
| 1678 » » break; |
| 1679 » case 4: /* neg-y */ |
| 1680 » » vec[0]= -tvec[2]; |
| 1681 » » /* vec[1]= 0.0; */ |
| 1682 » » vec[2]= tvec[0]; |
| 1683 » » break; |
| 1684 » case 5: /* neg-z */ |
| 1685 » » vec[0]= -tvec[0]; |
| 1686 » » vec[1]= -tvec[1]; |
| 1687 » » /* vec[2]= 0.0; */ |
| 1688 » » break; |
| 1689 » } |
1690 } | 1690 } |
1691 | 1691 |
1692 /* lens/angle conversion (radians) */ | 1692 /* lens/angle conversion (radians) */ |
1693 float lens_to_angle(float lens) | 1693 float focallength_to_hfov(float focal_length, float sensor_x) |
1694 { | 1694 { |
1695 » return 2.0f * atanf(16.0f/lens); | 1695 » return 2.0f * atanf((sensor_x/2.0f) / focal_length); |
1696 } | 1696 } |
1697 | 1697 |
1698 float angle_to_lens(float angle) | 1698 float hfov_to_focallength(float hfov, float sensor_x) |
1699 { | 1699 { |
1700 » return 16.0f / tanf(angle * 0.5f); | 1700 » return (sensor_x/2.0f) / tanf(hfov * 0.5f); |
1701 } | 1701 } |
1702 | 1702 |
1703 /* 'mod_inline(-3,4)= 1', 'fmod(-3,4)= -3' */ | 1703 /* 'mod_inline(-3,4)= 1', 'fmod(-3,4)= -3' */ |
| 1704 static float mod_inline(float a, float b) |
| 1705 { |
| 1706 return a - (b * floorf(a / b)); |
| 1707 } |
| 1708 |
| 1709 float angle_wrap_rad(float angle) |
| 1710 { |
| 1711 return mod_inline(angle + (float)M_PI, (float)M_PI*2.0f) - (float)M_PI; |
| 1712 } |
| 1713 |
| 1714 float angle_wrap_deg(float angle) |
| 1715 { |
| 1716 return mod_inline(angle + 180.0f, 360.0f) - 180.0f; |
| 1717 } |
LEFT | RIGHT |