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 #define quat_set(result, x, y, z, w) \
00322 { \
00323 (result)[X] = x; \
00324 (result)[Y] = y; \
00325 (result)[Z] = z; \
00326 (result)[W] = w; \
00327 }
00328
00329 #define quat_expand(q_) \
00330 { \
00331 float w2 = 1.0f - ( q_[0]*q_[0] + q_[1]*q_[1] + q_[2]*q_[2] ); \
00332 q_[3] = (w2 < 0) ? 0.0f : -sqrt(w2); \
00333 }
00334
00335 #define quat_cpy(result, q0) \
00336 { \
00337 (result)[X] = (q0)[X]; \
00338 (result)[Y] = (q0)[Y]; \
00339 (result)[Z] = (q0)[Z]; \
00340 (result)[W] = (q0)[W]; \
00341 }
00342
00343 #define quat_rotaxis(result, radtheta, vnorm) \
00344 { \
00345 float radtheta_half = radtheta * 0.5f; \
00346 float sinhalf = sin(radtheta_half); \
00347 (result)[0] = (vnorm)[0] * sinhalf; \
00348 (result)[1] = (vnorm)[1] * sinhalf; \
00349 (result)[2] = (vnorm)[2] * sinhalf; \
00350 (result)[3] = cos(radtheta_half); \
00351 }
00352
00353 #define quat_apply(result, q0, v) \
00354 { \
00355 float _v4f_[] = { v[0], v[1], v[2], 0.0f }; \
00356 float _q_conj_[] = { -q0[0], -q0[1], -q0[2], q0[3] }; \
00357 float _temp0_[4]; \
00358 float _temp1_[4]; \
00359 quat_mul(_temp0_, q0, _v4f_); \
00360 quat_mul(_temp1_, _temp0_, _q_conj_); \
00361 vector_set(result, _temp1_[0],_temp1_[1],_temp1_[2]); \
00362 }
00363
00364 #define quat_nlerp(result, q0, q1, delta) \
00365 { \
00366 float _q0_part_[4]; \
00367 float _q1_part_[4]; \
00368 float _unnorm_[4]; \
00369 quat_scale(_q0_part_, q0, (1.0f - (delta))); \
00370 quat_scale(_q1_part_, q1, delta); \
00371 quat_add(_unnorm_, _q0_part_, _q1_part_); \
00372 float mag_inv = 1.0f / quat_mag(_unnorm_); \
00373 quat_scale(result, _unnorm_, mag_inv); \
00374 }
00375
00376 #define quat_mag(q0) \
00377 ( \
00378 sqrt( (q0)[0] * (q0)[0] + (q0)[1] * (q0)[1] + (q0)[2] * (q0)[3] + (q0)[3] * (q0)[3] ) \
00379 )
00380
00381 #define quat_mag_inv(q0) \
00382 ( \
00383 1.0 / quat_norm(q0) \
00384 )
00385
00386 #define quat_conj(q_) \
00387 { \
00388 (q_)[0] = -(q_)[0]; \
00389 (q_)[1] = -(q_)[1]; \
00390 (q_)[2] = -(q_)[2]; \
00391 (q_)[3] = +(q_)[3]; \
00392 }
00393
00394 #define quat_scale(q_, q0, scale) \
00395 { \
00396 (q_)[0] = (q0)[0] * (scale); \
00397 (q_)[1] = (q0)[1] * (scale); \
00398 (q_)[2] = (q0)[2] * (scale); \
00399 (q_)[3] = (q0)[3] * (scale); \
00400 }
00401
00402 #define quat_add(q_, q0, q1) \
00403 { \
00404 (q_)[0] = (q0)[0] + (q1)[0]; \
00405 (q_)[1] = (q0)[1] + (q1)[1]; \
00406 (q_)[2] = (q0)[2] + (q1)[2]; \
00407 (q_)[3] = (q0)[3] + (q1)[3]; \
00408 }
00409
00410 #define quat_mul_(q_, q0, q1) \
00411 { \
00412 (q_)[0] = (q0)[3] * (q1)[0] + (q0)[0] * (q1)[3] + (q0)[1] * (q1)[2] - (q0)[2] * (q1)[1]; \
00413 (q_)[1] = (q0)[3] * (q1)[1] + (q0)[1] * (q1)[3] + (q0)[2] * (q1)[0] - (q0)[0] * (q1)[2]; \
00414 (q_)[2] = (q0)[3] * (q1)[2] + (q0)[2] * (q1)[3] + (q0)[0] * (q1)[1] - (q0)[1] * (q1)[0]; \
00415 (q_)[3] = (q0)[3] * (q1)[3] - (q0)[0] * (q1)[0] - (q0)[1] * (q1)[1] - (q0)[2] * (q1)[2]; \
00416 }
00417
00418 #define quat_mul(q_, q0, q1) \
00419 { \
00420 float _a_ = (q0[3] + q0[0]) * (q1[3] + q1[0]); \
00421 float _b_ = (q0[2] - q0[1]) * (q1[1] - q1[2]); \
00422 float _c_ = (q0[3] - q0[0]) * (q1[1] + q1[2]); \
00423 float _d_ = (q0[1] + q0[2]) * (q1[3] - q1[0]); \
00424 float _e_ = (q0[0] + q0[2]) * (q1[0] + q1[1]); \
00425 float _f_ = (q0[0] - q0[2]) * (q1[0] - q1[1]); \
00426 float _g_ = (q0[3] + q0[1]) * (q1[3] - q1[2]); \
00427 float _h_ = (q0[3] - q0[1]) * (q1[3] + q1[2]); \
00428 q_[0] = _a_ - ( _e_ + _f_ + _g_ + _h_) / 2; \
00429 q_[1] = _c_ + ( _e_ - _f_ + _g_ - _h_) / 2; \
00430 q_[2] = _d_ + ( _e_ - _f_ - _g_ + _h_) / 2; \
00431 q_[3] = _b_ + (-_e_ - _f_ + _g_ + _h_) / 2; \
00432 }
00433
00434
00435
00436
00437
00438 #define vector_print(A) \
00439 { \
00440 printf("< %3.4f %3.4f %3.4f >\n", (A)[0], (A)[1], (A)[2]); \
00441 }
00442
00443 #define vector_cpy(result, A) { (result)[X] = (A)[X]; (result)[Y] = (A)[Y]; (result)[Z] = (A)[Z]; }
00444
00445 #define vector_set(result, x, y, z) { (result)[X] = x; (result)[Y] = y; (result)[Z] = z; }
00446
00447 #define vector_set3i(result, fun_of_i) { loop3i(result[i] = fun_of_i;) }
00448
00449 #define vector_norm(result, A) { typeof(A[0]) inv = ((typeof(A[0]))1)/vector_mag(A); vector_scale( (result), (A), inv); }
00450
00451 #define vector_mag(A) ( sqrt( xsqr((A)[X]) + xsqr((A)[Y]) + xsqr((A)[Z]) ) )
00452
00453 #define vector_dot(A, B) ((A)[X] * (B)[X] + (A)[Y] * (B)[Y] + (A)[Z] * (B)[Z] )
00454
00455 #define vector_neg(result, A) { (result)[X] = -(A)[X]; (result)[Y] = -(A)[Y]; (result)[Z] = -(A)[Z]; }
00456
00457 #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]; }
00458
00459 #define vector_add(result, A, B) { (result)[X] = (A)[X] + (B)[X]; (result)[Y] = (A)[Y] + (B)[Y]; (result)[Z] = (A)[Z] + (B)[Z]; }
00460
00461 #define vector_sub(result, A, B) { (result)[X] = (A)[X] - (B)[X]; (result)[Y] = (A)[Y] - (B)[Y]; (result)[Z] = (A)[Z] - (B)[Z]; }
00462
00463 #define vector_scale(result, A, B) { (result)[X] = (A)[X] * (B); (result)[Y] = (A)[Y] * (B); (result)[Z] = (A)[Z] * (B); }
00464
00465 #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]) )
00466
00467 #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]) ) )
00468
00469 #define vector_manhattan(A, B) ( xabs((A)[X]-(B)[X])) + xabs((A)[Y]-(B)[Y])) + xabs((A)[Z]-(B)[Z])) )
00470
00471 #define vector_rotx(result, radx) \
00472 { \
00473 MATRIX M; \
00474 float c = cos(radx); \
00475 float s = sin(radx); \
00476 matrix_set(M, 1, 0, 0, 0, \
00477 0, c, s, 0, \
00478 0,-s, c, 0, \
00479 0, 0, 0, 1); \
00480 matrix_apply2(M, (result)); \
00481 }
00482
00483
00484 #define vector_roty(result, rady) \
00485 { \
00486 MATRIX M; \
00487 float c = cos(rady); \
00488 float s = sin(rady); \
00489 matrix_set(M, c, 0,-s, 0, \
00490 0, 1, 0, 0, \
00491 s, 0, c, 0, \
00492 0, 0, 0, 1); \
00493 matrix_apply2(M, (result)); \
00494 }
00495
00496
00497 #define vector_rotz(result, radz) \
00498 { \
00499 MATRIX M; \
00500 float c = cos(radz); \
00501 float s = sin(radz); \
00502 matrix_set(M, c, s, 0, 0, \
00503 -s, c, 0, 0, \
00504 0, 0, 1, 0, \
00505 0, 0, 0, 1); \
00506 matrix_apply2(M, (result)); \
00507 }
00508
00509
00510
00511 #endif