00001
00008 #ifndef _MATH3D_H
00009 #define _MATH3D_H
00010
00011 #include <math.h>
00012
00013
00014
00015
00016
00017
00018
00019
00020 #define PI_OVER_180 0.017453292
00021
00022 #ifndef finitef
00023
00024 #endif
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058 typedef float MATRIX[4*4];
00059
00060
00061
00062 #define X 0
00063 #define Y 1
00064 #define Z 2
00065 #define W 3
00066 #define D 3
00067
00068
00069
00070
00071
00072
00073
00074
00075 #define ppercent(zero_pc, hundred_pc, p_pc) (zero_pc + ((hundred_pc)-(zero_pc))*(p_pc)/100 )
00076
00077 #define inside(min, v, max) ( ( (min) <= (v) ) && ( (v) <= (max) ) )
00078
00079 #define setflag(a, b) (a) = ( FLAGS((a)) | FLAGS((b)) )
00080
00081 #define resetflag(a, b) (a) = ( FLAGS((a)) & ( ~ FLAGS((b)) ) )
00082
00083
00084
00085
00086 #define xsqr(x) ( (x) * (x) )
00087
00088 #define xsgn(v) (((v)<0)?(-1):(((v)>0)?(+1):(0)))
00089
00090 #define xabs(v) (((v)<0)?-(v):(v))
00091
00092 #define xmod(a, b) ((a) -= long((b)) * (long( (a) ) / long( (b) )))
00093
00094 #define xmax(v1,v2) (((v1)>(v2))?(v1):(v2))
00095
00096 #define xmin(v1,v2) (((v1)<(v2))?(v1):(v2))
00097
00098 #define xminmax(v, min, max) ( xmin(xmax(v, max), min) )
00099
00100
00101
00102
00103
00104
00105 #define spline_b1(u) ((u) * ((u) - 1) * ((u) - 2) / (-6.0F))
00106
00107 #define spline_b2(u) (((u) + 1) * ((u) - 1) * ((u) - 2) / (2.0F))
00108
00109 #define spline_b3(u) (((u) + 1) * (u) * ((u) - 2) / (-2.0F))
00110
00111 #define spline_b4(u) (((u) + 1) * (u) * ((u) - 1) / (6.0F))
00112
00113 #define spline_func(a, b, c, d, u) ((a) * spline_b1((u)) + (b) * spline_b2((u)) + (c) * spline_b3((u)) + (d) * spline_b4((u)))
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126 #define calcT(n, p, v, d) (-(n[X]*p[X]+n[Y]*p[Y]+n[Z]*p[Z]+d)/(n[X]*v[X]+n[Y]*v[Y]+n[Z]*v[Z]))
00127
00128 #define calcP(nrml, pnt, d) (nrml[X]*pnt[X]+nrml[Y]*pnt[Y]+nrml[Z]*pnt[Z]+d)
00129
00130 #define calcA(nrml, pnt, d) (-(nrml[Y]*pnt[Y]+nrml[Z]*pnt[Z]+d)/pnt[X])
00131
00132 #define calcB(nrml, pnt, d) (-(nrml[X]*pnt[X]+nrml[Z]*pnt[Z]+d)/pnt[Y])
00133
00134 #define calcC(nrml, pnt, d) (-(nrml[X]*pnt[X]+nrml[Y]*pnt[Y]+d)/pnt[Z])
00135
00136 #define calcD(nrml, pnt) (-(nrml[X]*pnt[X]+nrml[Y]*pnt[Y]+nrml[Z]*pnt[Z]))
00137
00138 #define calcX(nrml, pnt, d) (-(nrml[Y]*pnt[Y]+nrml[Z]*pnt[Z]+d)/nrml[X])
00139
00140 #define calcY(nrml, pnt, d) (-(nrml[X]*pnt[X]+nrml[Z]*pnt[Z]+d)/nrml[Y])
00141
00142 #define calcZ(nrml, pnt, d) (-(nrml[X]*pnt[X]+nrml[Y]*pnt[Y]+d)/nrml[Z])
00143
00144 #define clip(a1, b1, a2, b2, clp) ( (a1) - ( (a1) - (a2) ) / ( (b1) - (b2) ) * ( (b1) - (clp) ) )
00145
00146 #define clif(b1, b2, f) ( (b1) - ( (b1) - (b2) ) * (f) )
00147
00148 #define clic(b1, b2, f, d) ( (b1) - ( (b1) - (b2) ) * (f) / (d) )
00149
00150
00151
00152 #define plane_lineintersect(result, normal, d, p0, p1, resultp) \
00153 { \
00154 (result) = 0 \
00155 float d0 = calcP((normal), (p0), (d)); \
00156 float d1 = calcP((normal), (p1), (d)); \
00157 if ( xsgn(d0) != xsgn(d1) ) { \
00158 float dd = d0 / (d0 - d1); \
00159 (resultp)[X] = (p0)[X] - (((p0)[X] - (p1)[X]) * dd); \
00160 (resultp)[Y] = (p0)[Y] - (((p0)[Y] - (p1)[Y]) * dd); \
00161 (resultp)[Z] = (p0)[Z] - (((p0)[Z] - (p1)[Z]) * dd); \
00162 result = 1; \
00163 } \
00164 }
00165
00166
00167 #define normal_threepnt(result, p0, p1, p2) \
00168 { \
00169 float v0[3]; \
00170 float v1[3]; \
00171 vector_sub(v0, (p0), (p1)); \
00172 vector_sub(v1, (p1), (p2)); \
00173 vector_cross((result), v0, v1); \
00174 }
00175
00176
00177
00178
00179
00180
00181 #define matrix_print(glMatrix) \
00182 { \
00183 printf("/ %3.2f %3.2f %3.2f %3.2f \\\n", (glMatrix)[0], (glMatrix)[4], (glMatrix)[ 8], (glMatrix)[12]); \
00184 printf("| %3.2f %3.2f %3.2f %3.2f |\n", (glMatrix)[1], (glMatrix)[5], (glMatrix)[ 9], (glMatrix)[13]); \
00185 printf("| %3.2f %3.2f %3.2f %3.2f |\n", (glMatrix)[2], (glMatrix)[6], (glMatrix)[10], (glMatrix)[14]); \
00186 printf("\\ %3.2f %3.2f %3.2f %3.2f /\n", (glMatrix)[3], (glMatrix)[7], (glMatrix)[11], (glMatrix)[15]); \
00187 }
00188
00189
00190 #define matrix_transpose(m) { float tmp; loopi(4) { loopj(i) { tmp = m[4*i+j]; m[4*i+j] = m[4*j+i]; m[4*j+i] = tmp; } } }
00191
00192
00193 #define matrix_set(result, a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4) \
00194 { \
00195 (result)[ 0]=(a1); (result)[ 1]=(a2); (result)[ 2]=(a3); (result)[ 3]=(a4); \
00196 (result)[ 4]=(b1); (result)[ 5]=(b2); (result)[ 6]=(b3); (result)[ 7]=(b4); \
00197 (result)[ 8]=(c1); (result)[ 9]=(c2); (result)[10]=(c3); (result)[11]=(c4); \
00198 (result)[12]=(d1); (result)[13]=(d2); (result)[14]=(d3); (result)[15]=(d4); \
00199 }
00200
00201
00202 #define matrix_identity(result) \
00203 { \
00204 matrix_set((result), 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1); \
00205 }
00206
00207
00208
00209 #define matrix_apply2(M, V) \
00210 { \
00211 float pre[] = { (V)[0], (V)[1], (V)[2] }; \
00212 (V)[0] = (M)[0]*(pre)[0] + (M)[4]*(pre)[1] + (M)[ 8]*(pre)[2] + (M)[12]; \
00213 (V)[1] = (M)[1]*(pre)[0] + (M)[5]*(pre)[1] + (M)[ 9]*(pre)[2] + (M)[13]; \
00214 (V)[2] = (M)[2]*(pre)[0] + (M)[6]*(pre)[1] + (M)[10]*(pre)[2] + (M)[14]; \
00215 }
00216
00217
00218
00219 #define matrix_apply3(result, M, V) \
00220 { \
00221 (result)[X] = ((V)[0]*(M)[0]) + ((V)[1]*(M)[4]) + ((V)[2]*(M)[ 8]) + ((M)[12]); \
00222 (result)[Y] = ((V)[0]*(M)[1]) + ((V)[1]*(M)[5]) + ((V)[2]*(M)[ 9]) + ((M)[13]); \
00223 (result)[Z] = ((V)[0]*(M)[2]) + ((V)[1]*(M)[6]) + ((V)[2]*(M)[10]) + ((M)[14]); \
00224 }
00225
00226
00227
00228 #define matrix_mult2(M, N) \
00229 { \
00230 MATRIX tmpM; \
00231 loopj(4) { \
00232 loopi(4) { \
00233 tmpM[i+4*j] = ((N)[i+4*0]*(M)[0+4*j]) + ((N)[i+4*1]*(M)[1+4*j])+ \
00234 ((N)[i+4*2]*(M)[2+4*j]) + ((N)[i+4*3]*(M)[3+4*j]); \
00235 }; \
00236 }; \
00237 memcpy((M), tmpM, sizeof(MATRIX)); \
00238 }
00239
00240
00241
00242 #define matrix_add2(M, N) \
00243 { \
00244 loopi(16) \
00245 (M)[i] += (N)[i]; \
00246 }
00247
00248
00249
00250 #define matrix_rotx(M, x) \
00251 { \
00252 MATRIX N; \
00253 float c = cos((x)*PI_OVER_180); \
00254 float s = sin((x)*PI_OVER_180); \
00255 matrix_set(N, 1, 0, 0, 0, \
00256 0, c, s, 0, \
00257 0,-s, c, 0, \
00258 0, 0, 0, 1); \
00259 matrix_mult2((M), N); \
00260 }
00261
00262
00263
00264 #define matrix_roty(M, y) \
00265 { \
00266 MATRIX N; \
00267 float c = cos((y)*PI_OVER_180); \
00268 float s = sin((y)*PI_OVER_180); \
00269 matrix_set(N, c, 0,-s, 0, \
00270 0, 1, 0, 0, \
00271 s, 0, c, 0, \
00272 0, 0, 0, 1); \
00273 matrix_mult2((M), N); \
00274 }
00275
00276
00277
00278 #define matrix_rotz(M, z) \
00279 { \
00280 MATRIX N; \
00281 float c = cos((z)*PI_OVER_180); \
00282 float s = sin((z)*PI_OVER_180); \
00283 matrix_set(N, c, s, 0, 0, \
00284 -s, c, 0, 0, \
00285 0, 0, 1, 0, \
00286 0, 0, 0, 1); \
00287 matrix_mult2((M), N); \
00288 }
00289
00290
00291
00292 #define matrix_translate(M, x, y, z) \
00293 { \
00294 (M)[12] += x; \
00295 (M)[13] += y; \
00296 (M)[14] += z; \
00297 }
00298
00299
00300
00301 #define matrix_scale(M, x, y, z) \
00302 { \
00303 MATRIX N; \
00304 matrix_set(N, (x), 0, 0, 0, \
00305 0,(y), 0, 0, \
00306 0, 0,(z), 0, \
00307 0, 0, 0, 1); \
00308 matrix_mult2(M, N); \
00309 }
00310
00311
00312
00313
00314
00315
00316 #define quat_print(q0) \
00317 { \
00318 printf( "< %2.4f %2.4f %2.4f %2.4f >\n", (q0)[0], (q0)[1], (q0)[2], (q0)[3] ); \
00319 }
00320
00321
00322 #define quat_print_pos(q0) \
00323 { \
00324 if ((q0)[3] > 0) printf( "< %2.4f %2.4f %2.4f %2.4f >\n", (q0)[0], (q0)[1], (q0)[2], (q0)[3] ); \
00325 else printf( "< %2.4f %2.4f %2.4f %2.4f >\n", -(q0)[0], -(q0)[1], -(q0)[2], -(q0)[3] ); \
00326 }
00327
00328
00329 #define quat_rectify_pos(q0) \
00330 { \
00331 if ((q0)[3] < 0) { (q0)[0] = -(q0)[0]; (q0)[1] = -(q0)[1]; (q0)[2] = -(q0)[2]; (q0)[3] = -(q0)[3]; } \
00332 }
00333
00334
00335 #define quat_rectify_neg(q0) \
00336 { \
00337 if ((q0)[3] > 0) { (q0)[0] = -(q0)[0]; (q0)[1] = -(q0)[1]; (q0)[2] = -(q0)[2]; (q0)[3] = -(q0)[3]; } \
00338 }
00339
00340
00341 #define quat_set(result, x, y, z, w) \
00342 { \
00343 (result)[0] = (x); \
00344 (result)[1] = (y); \
00345 (result)[2] = (z); \
00346 (result)[3] = (w); \
00347 }
00348
00349 #define quat_expand(q_) \
00350 { \
00351 const float w2 = 1.0f - ( (q_[0]*q_[0]) + ((q_)[1]*)q_)[1]) + ((q_)[2]*(q_)[2]) ); \
00352 q_[3] = (w2 < 0) ? 0.0f : -sqrt(w2); \
00353 }
00354
00355 #define quat_cpy(result, q0) \
00356 { \
00357 (result)[0] = (q0)[0]; \
00358 (result)[1] = (q0)[1]; \
00359 (result)[2] = (q0)[2]; \
00360 (result)[3] = (q0)[3]; \
00361 }
00362
00363 #define quat_rotaxis(result, radtheta, vnorm) \
00364 { \
00365 float radtheta_half = radtheta * 0.5f; \
00366 float sinhalf = sin(radtheta_half); \
00367 (result)[0] = (vnorm)[0] * sinhalf; \
00368 (result)[1] = (vnorm)[1] * sinhalf; \
00369 (result)[2] = (vnorm)[2] * sinhalf; \
00370 (result)[3] = cos(radtheta_half); \
00371 }
00372
00373 #define quat_apply(result, q0, v) \
00374 { \
00375 const float _v4f_[] = { (v)[0], (v)[1], (v)[2], 0.0f }; \
00376 const float _q_conj_[] = { -(q0)[0], -(q0)[1], -(q0)[2], (q0)[3] }; \
00377 float _temp0_[4]; \
00378 float _temp1_[4]; \
00379 quat_mul(_temp0_, (q0), _v4f_); \
00380 quat_mul(_temp1_, _temp0_, _q_conj_); \
00381 vector_set((result), _temp1_[0],_temp1_[1],_temp1_[2]); \
00382 }
00383
00384 #define quat_nlerp(result, q0, q1, delta) \
00385 { \
00386 float _q0_part_[4]; \
00387 float _q1_part_[4]; \
00388 float _unnorm_[4]; \
00389 quat_scale(_q0_part_, (q0), (1.0f - (delta))); \
00390 quat_scale(_q1_part_, (q1), delta); \
00391 quat_add(_unnorm_, _q0_part_, _q1_part_); \
00392 const float mag_inv = 1.0f / quat_mag(_unnorm_); \
00393 quat_scale((result), _unnorm_, mag_inv); \
00394 }
00395
00396 #define quat_mag(q0) \
00397 ( \
00398 sqrt( (q0)[0] * (q0)[0] + (q0)[1] * (q0)[1] + (q0)[2] * (q0)[2] + (q0)[3] * (q0)[3] ) \
00399 )
00400
00401 #define quat_mag_inv(q0) \
00402 ( \
00403 1.0 / quat_mag(q0) \
00404 )
00405
00406 #define quat_conj(q_) \
00407 { \
00408 (q_)[0] = -(q_)[0]; \
00409 (q_)[1] = -(q_)[1]; \
00410 (q_)[2] = -(q_)[2]; \
00411 (q_)[3] = +(q_)[3]; \
00412 }
00413
00414 #define quat_scale(q_, q0, scale) \
00415 { \
00416 (q_)[0] = (q0)[0] * (scale); \
00417 (q_)[1] = (q0)[1] * (scale); \
00418 (q_)[2] = (q0)[2] * (scale); \
00419 (q_)[3] = (q0)[3] * (scale); \
00420 }
00421
00422 #define quat_add(q_, q0, q1) \
00423 { \
00424 (q_)[0] = (q0)[0] + (q1)[0]; \
00425 (q_)[1] = (q0)[1] + (q1)[1]; \
00426 (q_)[2] = (q0)[2] + (q1)[2]; \
00427 (q_)[3] = (q0)[3] + (q1)[3]; \
00428 }
00429
00430 #define quat_mul_(q_, q0, q1) \
00431 { \
00432 (q_)[0] = (q0)[3] * (q1)[0] + (q0)[0] * (q1)[3] + (q0)[1] * (q1)[2] - (q0)[2] * (q1)[1]; \
00433 (q_)[1] = (q0)[3] * (q1)[1] + (q0)[1] * (q1)[3] + (q0)[2] * (q1)[0] - (q0)[0] * (q1)[2]; \
00434 (q_)[2] = (q0)[3] * (q1)[2] + (q0)[2] * (q1)[3] + (q0)[0] * (q1)[1] - (q0)[1] * (q1)[0]; \
00435 (q_)[3] = (q0)[3] * (q1)[3] - (q0)[0] * (q1)[0] - (q0)[1] * (q1)[1] - (q0)[2] * (q1)[2]; \
00436 }
00437
00438 #define quat_mul(q_, q0, q1) \
00439 { \
00440 const float _a_ = ((q0)[3] + (q0)[0]) * ((q1)[3] + (q1)[0]); \
00441 const float _b_ = ((q0)[2] - (q0)[1]) * ((q1)[1] - (q1)[2]); \
00442 const float _c_ = ((q0)[3] - (q0)[0]) * ((q1)[1] + (q1)[2]); \
00443 const float _d_ = ((q0)[1] + (q0)[2]) * ((q1)[3] - (q1)[0]); \
00444 const float _e_ = ((q0)[0] + (q0)[2]) * ((q1)[0] + (q1)[1]); \
00445 const float _f_ = ((q0)[0] - (q0)[2]) * ((q1)[0] - (q1)[1]); \
00446 const float _g_ = ((q0)[3] + (q0)[1]) * ((q1)[3] - (q1)[2]); \
00447 const float _h_ = ((q0)[3] - (q0)[1]) * ((q1)[3] + (q1)[2]); \
00448 (q_)[0] = _a_ - ( _e_ + _f_ + _g_ + _h_) * 0.5f; \
00449 (q_)[1] = _c_ + ( _e_ - _f_ + _g_ - _h_) * 0.5f; \
00450 (q_)[2] = _d_ + ( _e_ - _f_ - _g_ + _h_) * 0.5f; \
00451 (q_)[3] = _b_ + (-_e_ - _f_ + _g_ + _h_) * 0.5f; \
00452 }
00453
00454
00455
00456
00457
00458 #define vector_print(A) \
00459 { \
00460 printf("< %3.4f %3.4f %3.4f >\n", (A)[0], (A)[1], (A)[2]); \
00461 }
00462
00463 #define vector_cpy(result, A) { (result)[X] = (A)[X]; (result)[Y] = (A)[Y]; (result)[Z] = (A)[Z]; }
00464
00465 #define vector_set(result, x, y, z) { (result)[X] = x; (result)[Y] = y; (result)[Z] = z; }
00466
00467 #define vector_set3i(result, fun_of_i) { loop3i(result[i] = fun_of_i;) }
00468
00469 #define vector_norm(result, A) { typeof(A[0]) inv = ((typeof(A[0]))1)/vector_mag(A); vector_scale( (result), (A), inv); }
00470
00471 #define vector_mag(A) ( sqrt( xsqr((A)[X]) + xsqr((A)[Y]) + xsqr((A)[Z]) ) )
00472
00473 #define vector_dot(A, B) ((A)[X] * (B)[X] + (A)[Y] * (B)[Y] + (A)[Z] * (B)[Z] )
00474
00475 #define vector_neg(result, A) { (result)[X] = -(A)[X]; (result)[Y] = -(A)[Y]; (result)[Z] = -(A)[Z]; }
00476
00477 #define vector_cross(result, A, B) { (result)[X] = (A)[Y] * (B)[Z] - (A)[Z] * (B)[Y]; (result)[Y] = (A)[Z] * (B)[X] - (A)[X] * (B)[Z]; (result)[Z] = (A)[X] * (B)[Y] - (A)[Y] * (B)[X]; }
00478
00479 #define vector_add(result, A, B) { (result)[X] = (A)[X] + (B)[X]; (result)[Y] = (A)[Y] + (B)[Y]; (result)[Z] = (A)[Z] + (B)[Z]; }
00480
00481 #define vector_sub(result, A, B) { (result)[X] = (A)[X] - (B)[X]; (result)[Y] = (A)[Y] - (B)[Y]; (result)[Z] = (A)[Z] - (B)[Z]; }
00482
00483 #define vector_scale(result, A, B) { (result)[X] = (A)[X] * (B); (result)[Y] = (A)[Y] * (B); (result)[Z] = (A)[Z] * (B); }
00484
00485 #define vector_sqrdist(A, B) ( ((A)[X]-(B)[X]) * ((A)[X]-(B)[X]) + ((A)[Y]-(B)[Y]) * ((A)[Y]-(B)[Y]) + ((A)[Z]-(B)[Z]) * ((A)[Z]-(B)[Z]) )
00486
00487 #define vector_distance(A, B) ( sqrt( ((A)[X]-(B)[X]) * ((A)[X]-(B)[X]) + ((A)[Y]-(B)[Y]) * ((A)[Y]-(B)[Y]) + ((A)[Z]-(B)[Z]) * ((A)[Z]-(B)[Z]) ) )
00488
00489 #define vector_manhattan(A, B) ( xabs((A)[X]-(B)[X])) + xabs((A)[Y]-(B)[Y])) + xabs((A)[Z]-(B)[Z])) )
00490
00491 #define vector_rotx(result, radx) \
00492 { \
00493 MATRIX M; \
00494 float c = cos(radx); \
00495 float s = sin(radx); \
00496 matrix_set(M, 1, 0, 0, 0, \
00497 0, c, s, 0, \
00498 0,-s, c, 0, \
00499 0, 0, 0, 1); \
00500 matrix_apply2(M, (result)); \
00501 }
00502
00503
00504 #define vector_roty(result, rady) \
00505 { \
00506 MATRIX M; \
00507 float c = cos(rady); \
00508 float s = sin(rady); \
00509 matrix_set(M, c, 0,-s, 0, \
00510 0, 1, 0, 0, \
00511 s, 0, c, 0, \
00512 0, 0, 0, 1); \
00513 matrix_apply2(M, (result)); \
00514 }
00515
00516
00517 #define vector_rotz(result, radz) \
00518 { \
00519 MATRIX M; \
00520 float c = cos(radz); \
00521 float s = sin(radz); \
00522 matrix_set(M, c, s, 0, 0, \
00523 -s, c, 0, 0, \
00524 0, 0, 1, 0, \
00525 0, 0, 0, 1); \
00526 matrix_apply2(M, (result)); \
00527 }
00528
00529
00530
00531 #endif