[8] | 1 | |
---|
| 2 | |
---|
| 3 | |
---|
| 4 | |
---|
| 5 | |
---|
| 6 | |
---|
[151] | 7 | |
---|
| 8 | |
---|
[8] | 9 | |
---|
| 10 | |
---|
| 11 | |
---|
| 12 | |
---|
[151] | 13 | |
---|
[8] | 14 | |
---|
| 15 | |
---|
| 16 | |
---|
| 17 | |
---|
| 18 | |
---|
| 19 | |
---|
| 20 | |
---|
| 21 | |
---|
| 22 | #define LIB3DS_EXPORT |
---|
[1563] | 23 | #include "quat.h" |
---|
[8] | 24 | #include <math.h> |
---|
| 25 | |
---|
[151] | 26 | |
---|
[8] | 27 | |
---|
| 28 | |
---|
| 29 | |
---|
| 30 | |
---|
| 31 | |
---|
| 32 | |
---|
| 33 | |
---|
| 34 | |
---|
| 35 | |
---|
| 36 | |
---|
[151] | 37 | |
---|
[8] | 38 | |
---|
| 39 | |
---|
| 40 | |
---|
| 41 | void |
---|
| 42 | lib3ds_quat_zero(Lib3dsQuat c) |
---|
| 43 | { |
---|
[151] | 44 | c[0]=c[1]=c[2]=c[3]=0.0f; |
---|
[8] | 45 | } |
---|
| 46 | |
---|
| 47 | |
---|
| 48 | |
---|
| 49 | |
---|
| 50 | |
---|
| 51 | void |
---|
| 52 | lib3ds_quat_identity(Lib3dsQuat c) |
---|
| 53 | { |
---|
[151] | 54 | c[0]=c[1]=c[2]=0.0f; |
---|
| 55 | c[3]=1.0f; |
---|
[8] | 56 | } |
---|
| 57 | |
---|
| 58 | |
---|
| 59 | |
---|
| 60 | |
---|
| 61 | |
---|
[151] | 62 | void |
---|
[8] | 63 | lib3ds_quat_copy(Lib3dsQuat dest, Lib3dsQuat src) |
---|
| 64 | { |
---|
[151] | 65 | int i; |
---|
| 66 | for (i=0; i<4; ++i) { |
---|
| 67 | dest[i]=src[i]; |
---|
| 68 | } |
---|
[8] | 69 | } |
---|
| 70 | |
---|
| 71 | |
---|
| 72 | |
---|
| 73 | |
---|
| 74 | |
---|
| 75 | void |
---|
| 76 | lib3ds_quat_axis_angle(Lib3dsQuat c, Lib3dsVector axis, Lib3dsFloat angle) |
---|
| 77 | { |
---|
[151] | 78 | Lib3dsDouble omega,s; |
---|
| 79 | Lib3dsDouble l; |
---|
[8] | 80 | |
---|
[151] | 81 | l=sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); |
---|
| 82 | if (l<LIB3DS_EPSILON) { |
---|
| 83 | c[0]=c[1]=c[2]=0.0f; |
---|
| 84 | c[3]=1.0f; |
---|
| 85 | } |
---|
| 86 | else { |
---|
| 87 | omega=-0.5*angle; |
---|
| 88 | s=sin(omega)/l; |
---|
| 89 | c[0]=(Lib3dsFloat)s*axis[0]; |
---|
| 90 | c[1]=(Lib3dsFloat)s*axis[1]; |
---|
| 91 | c[2]=(Lib3dsFloat)s*axis[2]; |
---|
| 92 | c[3]=(Lib3dsFloat)cos(omega); |
---|
| 93 | } |
---|
[8] | 94 | } |
---|
| 95 | |
---|
| 96 | |
---|
| 97 | |
---|
| 98 | |
---|
| 99 | |
---|
| 100 | void |
---|
| 101 | lib3ds_quat_neg(Lib3dsQuat c) |
---|
| 102 | { |
---|
[151] | 103 | int i; |
---|
| 104 | for (i=0; i<4; ++i) { |
---|
| 105 | c[i]=-c[i]; |
---|
| 106 | } |
---|
[8] | 107 | } |
---|
| 108 | |
---|
| 109 | |
---|
| 110 | |
---|
| 111 | |
---|
| 112 | |
---|
| 113 | void |
---|
| 114 | lib3ds_quat_abs(Lib3dsQuat c) |
---|
| 115 | { |
---|
[151] | 116 | int i; |
---|
| 117 | for (i=0; i<4; ++i) { |
---|
| 118 | c[i]=(Lib3dsFloat)fabs(c[i]); |
---|
| 119 | } |
---|
[8] | 120 | } |
---|
| 121 | |
---|
| 122 | |
---|
| 123 | |
---|
| 124 | |
---|
| 125 | |
---|
| 126 | void |
---|
| 127 | lib3ds_quat_cnj(Lib3dsQuat c) |
---|
| 128 | { |
---|
[151] | 129 | int i; |
---|
| 130 | for (i=0; i<3; ++i) { |
---|
| 131 | c[i]=-c[i]; |
---|
| 132 | } |
---|
[8] | 133 | } |
---|
| 134 | |
---|
| 135 | |
---|
| 136 | |
---|
| 137 | |
---|
| 138 | |
---|
| 139 | void |
---|
| 140 | lib3ds_quat_mul(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b) |
---|
| 141 | { |
---|
[151] | 142 | c[0]=a[3]*b[0] + a[0]*b[3] + a[1]*b[2] - a[2]*b[1]; |
---|
| 143 | c[1]=a[3]*b[1] + a[1]*b[3] + a[2]*b[0] - a[0]*b[2]; |
---|
| 144 | c[2]=a[3]*b[2] + a[2]*b[3] + a[0]*b[1] - a[1]*b[0]; |
---|
| 145 | c[3]=a[3]*b[3] - a[0]*b[0] - a[1]*b[1] - a[2]*b[2]; |
---|
[8] | 146 | } |
---|
| 147 | |
---|
| 148 | |
---|
| 149 | |
---|
| 150 | |
---|
| 151 | |
---|
| 152 | void |
---|
| 153 | lib3ds_quat_scalar(Lib3dsQuat c, Lib3dsFloat k) |
---|
| 154 | { |
---|
[151] | 155 | int i; |
---|
| 156 | for (i=0; i<4; ++i) { |
---|
| 157 | c[i]*=k; |
---|
| 158 | } |
---|
[8] | 159 | } |
---|
| 160 | |
---|
| 161 | |
---|
| 162 | |
---|
| 163 | |
---|
| 164 | |
---|
| 165 | void |
---|
| 166 | lib3ds_quat_normalize(Lib3dsQuat c) |
---|
| 167 | { |
---|
[151] | 168 | Lib3dsDouble l,m; |
---|
[8] | 169 | |
---|
[151] | 170 | l=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]); |
---|
| 171 | if (fabs(l)<LIB3DS_EPSILON) { |
---|
| 172 | c[0]=c[1]=c[2]=0.0f; |
---|
| 173 | c[3]=1.0f; |
---|
| 174 | } |
---|
| 175 | else { |
---|
| 176 | int i; |
---|
| 177 | m=1.0f/l; |
---|
| 178 | for (i=0; i<4; ++i) { |
---|
| 179 | c[i]=(Lib3dsFloat)(c[i]*m); |
---|
[8] | 180 | } |
---|
[151] | 181 | } |
---|
[8] | 182 | } |
---|
| 183 | |
---|
| 184 | |
---|
| 185 | |
---|
| 186 | |
---|
| 187 | |
---|
| 188 | void |
---|
| 189 | lib3ds_quat_inv(Lib3dsQuat c) |
---|
| 190 | { |
---|
[151] | 191 | Lib3dsDouble l,m; |
---|
[8] | 192 | |
---|
[151] | 193 | l=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]); |
---|
| 194 | if (fabs(l)<LIB3DS_EPSILON) { |
---|
| 195 | c[0]=c[1]=c[2]=0.0f; |
---|
| 196 | c[3]=1.0f; |
---|
| 197 | } |
---|
| 198 | else { |
---|
| 199 | m=1.0f/l; |
---|
| 200 | c[0]=(Lib3dsFloat)(-c[0]*m); |
---|
| 201 | c[1]=(Lib3dsFloat)(-c[1]*m); |
---|
| 202 | c[2]=(Lib3dsFloat)(-c[2]*m); |
---|
| 203 | c[3]=(Lib3dsFloat)(c[3]*m); |
---|
| 204 | } |
---|
[8] | 205 | } |
---|
| 206 | |
---|
| 207 | |
---|
| 208 | |
---|
| 209 | |
---|
| 210 | |
---|
| 211 | Lib3dsFloat |
---|
| 212 | lib3ds_quat_dot(Lib3dsQuat a, Lib3dsQuat b) |
---|
| 213 | { |
---|
[151] | 214 | return(a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]); |
---|
[8] | 215 | } |
---|
| 216 | |
---|
| 217 | |
---|
| 218 | |
---|
| 219 | |
---|
| 220 | |
---|
| 221 | Lib3dsFloat |
---|
| 222 | lib3ds_quat_squared(Lib3dsQuat c) |
---|
| 223 | { |
---|
[151] | 224 | return(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]); |
---|
[8] | 225 | } |
---|
| 226 | |
---|
| 227 | |
---|
| 228 | |
---|
| 229 | |
---|
| 230 | |
---|
| 231 | Lib3dsFloat |
---|
| 232 | lib3ds_quat_length(Lib3dsQuat c) |
---|
| 233 | { |
---|
[151] | 234 | return((Lib3dsFloat)sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3])); |
---|
[8] | 235 | } |
---|
| 236 | |
---|
| 237 | |
---|
| 238 | |
---|
| 239 | |
---|
| 240 | |
---|
| 241 | void |
---|
| 242 | lib3ds_quat_ln(Lib3dsQuat c) |
---|
| 243 | { |
---|
[151] | 244 | Lib3dsDouble om,s,t; |
---|
[8] | 245 | |
---|
[151] | 246 | s=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]); |
---|
[202] | 247 | om=atan2(s,(Lib3dsDouble)c[3]); |
---|
[151] | 248 | if (fabs(s)<LIB3DS_EPSILON) { |
---|
| 249 | t=0.0f; |
---|
| 250 | } |
---|
| 251 | else { |
---|
| 252 | t=om/s; |
---|
| 253 | } |
---|
| 254 | { |
---|
| 255 | int i; |
---|
| 256 | for (i=0; i<3; ++i) { |
---|
| 257 | c[i]=(Lib3dsFloat)(c[i]*t); |
---|
[8] | 258 | } |
---|
[151] | 259 | c[3]=0.0f; |
---|
| 260 | } |
---|
[8] | 261 | } |
---|
| 262 | |
---|
| 263 | |
---|
| 264 | |
---|
| 265 | |
---|
| 266 | |
---|
| 267 | void |
---|
| 268 | lib3ds_quat_ln_dif(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b) |
---|
| 269 | { |
---|
[151] | 270 | Lib3dsQuat invp; |
---|
[8] | 271 | |
---|
[151] | 272 | lib3ds_quat_copy(invp, a); |
---|
| 273 | lib3ds_quat_inv(invp); |
---|
| 274 | lib3ds_quat_mul(c, invp, b); |
---|
| 275 | lib3ds_quat_ln(c); |
---|
[8] | 276 | } |
---|
| 277 | |
---|
| 278 | |
---|
| 279 | |
---|
| 280 | |
---|
| 281 | |
---|
| 282 | void |
---|
| 283 | lib3ds_quat_exp(Lib3dsQuat c) |
---|
| 284 | { |
---|
[151] | 285 | Lib3dsDouble om,sinom; |
---|
[8] | 286 | |
---|
[151] | 287 | om=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]); |
---|
| 288 | if (fabs(om)<LIB3DS_EPSILON) { |
---|
| 289 | sinom=1.0f; |
---|
| 290 | } |
---|
| 291 | else { |
---|
| 292 | sinom=sin(om)/om; |
---|
| 293 | } |
---|
| 294 | { |
---|
| 295 | int i; |
---|
| 296 | for (i=0; i<3; ++i) { |
---|
| 297 | c[i]=(Lib3dsFloat)(c[i]*sinom); |
---|
[8] | 298 | } |
---|
[151] | 299 | c[3]=(Lib3dsFloat)cos(om); |
---|
| 300 | } |
---|
[8] | 301 | } |
---|
| 302 | |
---|
| 303 | |
---|
| 304 | |
---|
| 305 | |
---|
| 306 | |
---|
| 307 | void |
---|
| 308 | lib3ds_quat_slerp(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b, Lib3dsFloat t) |
---|
| 309 | { |
---|
[151] | 310 | Lib3dsDouble l; |
---|
| 311 | Lib3dsDouble om,sinom; |
---|
| 312 | Lib3dsDouble sp,sq; |
---|
| 313 | Lib3dsQuat q; |
---|
[8] | 314 | |
---|
[151] | 315 | l=a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]; |
---|
| 316 | if ((1.0+l)>LIB3DS_EPSILON) { |
---|
| 317 | if (fabs(l)>1.0f) l/=fabs(l); |
---|
| 318 | om=acos(l); |
---|
| 319 | sinom=sin(om); |
---|
| 320 | if (fabs(sinom)>LIB3DS_EPSILON) { |
---|
| 321 | sp=sin((1.0f-t)*om)/sinom; |
---|
| 322 | sq=sin(t*om)/sinom; |
---|
[8] | 323 | } |
---|
[151] | 324 | else { |
---|
| 325 | sp=1.0f-t; |
---|
| 326 | sq=t; |
---|
[8] | 327 | } |
---|
[151] | 328 | c[0]=(Lib3dsFloat)(sp*a[0] + sq*b[0]); |
---|
| 329 | c[1]=(Lib3dsFloat)(sp*a[1] + sq*b[1]); |
---|
| 330 | c[2]=(Lib3dsFloat)(sp*a[2] + sq*b[2]); |
---|
| 331 | c[3]=(Lib3dsFloat)(sp*a[3] + sq*b[3]); |
---|
| 332 | } |
---|
| 333 | else { |
---|
| 334 | q[0]=-a[1]; |
---|
| 335 | q[1]=a[0]; |
---|
| 336 | q[2]=-a[3]; |
---|
| 337 | q[3]=a[2]; |
---|
| 338 | sp=sin((1.0-t)*LIB3DS_HALFPI); |
---|
| 339 | sq=sin(t*LIB3DS_HALFPI); |
---|
| 340 | c[0]=(Lib3dsFloat)(sp*a[0] + sq*q[0]); |
---|
| 341 | c[1]=(Lib3dsFloat)(sp*a[1] + sq*q[1]); |
---|
| 342 | c[2]=(Lib3dsFloat)(sp*a[2] + sq*q[2]); |
---|
| 343 | c[3]=(Lib3dsFloat)(sp*a[3] + sq*q[3]); |
---|
| 344 | } |
---|
[8] | 345 | } |
---|
| 346 | |
---|
| 347 | |
---|
| 348 | |
---|
| 349 | |
---|
| 350 | |
---|
| 351 | void |
---|
| 352 | lib3ds_quat_squad(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat p, Lib3dsQuat q, |
---|
[151] | 353 | Lib3dsQuat b, Lib3dsFloat t) |
---|
[8] | 354 | { |
---|
[151] | 355 | Lib3dsQuat ab; |
---|
| 356 | Lib3dsQuat pq; |
---|
[8] | 357 | |
---|
[151] | 358 | lib3ds_quat_slerp(ab,a,b,t); |
---|
| 359 | lib3ds_quat_slerp(pq,p,q,t); |
---|
| 360 | lib3ds_quat_slerp(c,ab,pq,2*t*(1-t)); |
---|
[8] | 361 | } |
---|
| 362 | |
---|
| 363 | |
---|
| 364 | |
---|
| 365 | |
---|
| 366 | |
---|
| 367 | void |
---|
| 368 | lib3ds_quat_tangent(Lib3dsQuat c, Lib3dsQuat p, Lib3dsQuat q, Lib3dsQuat n) |
---|
| 369 | { |
---|
[151] | 370 | Lib3dsQuat dn,dp,x; |
---|
| 371 | int i; |
---|
[8] | 372 | |
---|
[151] | 373 | lib3ds_quat_ln_dif(dn, q, n); |
---|
| 374 | lib3ds_quat_ln_dif(dp, q, p); |
---|
[8] | 375 | |
---|
[151] | 376 | for (i=0; i<4; i++) { |
---|
| 377 | x[i]=-1.0f/4.0f*(dn[i]+dp[i]); |
---|
| 378 | } |
---|
| 379 | lib3ds_quat_exp(x); |
---|
| 380 | lib3ds_quat_mul(c,q,x); |
---|
[8] | 381 | } |
---|
| 382 | |
---|
| 383 | |
---|
| 384 | |
---|
| 385 | |
---|
| 386 | |
---|
| 387 | void |
---|
| 388 | lib3ds_quat_dump(Lib3dsQuat q) |
---|
| 389 | { |
---|
[151] | 390 | printf("%f %f %f %f\n", q[0], q[1], q[2], q[3]); |
---|
[8] | 391 | } |
---|