00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef __math3d_hpp__
00022 #define __math3d_hpp__
00023
00024 #include <math.h>
00025 #include <stdlib.h>
00026
00027 #if defined(_WIN32) || defined(_WINDOWS)
00028 template <class T>
00029 inline long int lround(T x)
00030 {
00031 if ( x >= 0 )
00032 return (long int)(x + 0.5);
00033 else
00034 return (long int)(x - 0.5);
00035 }
00036 #endif
00037
00038 #if defined(USE_ERRORPREC) || defined(USE_GMP)
00039 #include <gmp.h>
00040 #ifndef GMP_MPF_BITS
00041 #define GMP_MPF_BITS 256
00042 #endif
00043 #endif
00044
00045 typedef int int32;
00046 typedef unsigned int uint32;
00047
00048
00049
00050 #if defined(USE_ERRORPREC)
00051 struct prec_t
00052 {
00053 inline prec_t ()
00054 {
00055 mpf_init2(v2, GMP_MPF_BITS);
00056 }
00057
00058 inline prec_t(double v) : v1(v)
00059 {
00060 mpf_init2 (v2, GMP_MPF_BITS);
00061 mpf_set_d (v2, v);
00062 }
00063
00064 inline prec_t(const prec_t& c) : v1(c.v1)
00065 {
00066 mpf_init2 (v2, GMP_MPF_BITS);
00067 mpf_set (v2, c.v2);
00068 }
00069
00070 inline prec_t& operator= (const prec_t& c)
00071 {
00072 v1 = c.v1;
00073 mpf_set (v2, c.v2);
00074 return *this;
00075 }
00076
00077 inline prec_t& operator= (double v)
00078 {
00079 v1 = v;
00080 mpf_set_d (v2, v);
00081 return *this;
00082 }
00083
00084 inline ~prec_t ()
00085 {
00086 mpf_clear (v2);
00087 }
00088
00089 friend inline bool operator== (const prec_t& l, const prec_t& r) { return l.v1 == r.v1; }
00090 friend inline bool operator!= (const prec_t& l, const prec_t& r) { return l.v1 != r.v1; }
00091 friend inline bool operator< (const prec_t& l, const prec_t& r) { return l.v1 < r.v1; }
00092 friend inline bool operator<= (const prec_t& l, const prec_t& r) { return l.v1 <= r.v1; }
00093 friend inline bool operator> (const prec_t& l, const prec_t& r) { return l.v1 > r.v1; }
00094 friend inline bool operator>= (const prec_t& l, const prec_t& r) { return l.v1 >= r.v1; }
00095
00096 inline prec_t& operator += (const prec_t& r) { v1 += r.v1; mpf_add (v2, v2, r.v2); return *this; }
00097 inline prec_t& operator -= (const prec_t& r) { v1 -= r.v1; mpf_sub (v2, v2, r.v2); return *this; }
00098 inline prec_t& operator *= (const prec_t& r) { v1 *= r.v1; mpf_mul (v2, v2, r.v2); return *this; }
00099 inline prec_t& operator /= (const prec_t& r)
00100 {
00101 v1 /= r.v1;
00102 if ( mpf_cmp_d (r.v2, 0) != 0 )
00103 {
00104 mpf_div (v2, v2, r.v2);
00105 }
00106 else
00107 {
00108 mpf_set_d (v2, 0);
00109 }
00110 return *this;
00111 }
00112
00113 inline prec_t operator- () const
00114 {
00115 prec_t ret;
00116 ret.v1 = -v1;
00117 mpf_neg (ret.v2, v2);
00118 return ret;
00119 }
00120
00121 inline friend prec_t operator+ (const prec_t& l, const prec_t& r)
00122 {
00123 prec_t ret;
00124 ret.v1 = l.v1 + r.v1;
00125 mpf_add (ret.v2, l.v2, r.v2);
00126 return ret;
00127 }
00128
00129 inline friend prec_t operator- (const prec_t& l, const prec_t& r)
00130 {
00131 prec_t ret;
00132 ret.v1 = l.v1 - r.v1;
00133 mpf_sub (ret.v2, l.v2, r.v2);
00134 return ret;
00135 }
00136
00137 inline friend prec_t operator* (const prec_t& l, const prec_t& r)
00138 {
00139 prec_t ret;
00140 ret.v1 = l.v1 * r.v1;
00141 mpf_mul (ret.v2, l.v2, r.v2);
00142 return ret;
00143 }
00144
00145 inline friend prec_t operator/ (const prec_t& l, const prec_t& r)
00146 {
00147 prec_t ret;
00148 ret.v1 = l.v1 / r.v1;
00149 if ( mpf_cmp_d (r.v2, 0) != 0 )
00150 {
00151 mpf_div (ret.v2, l.v2, r.v2);
00152 }
00153 else
00154 {
00155 mpf_set_d (ret.v2, 0);
00156 }
00157 return ret;
00158 }
00159
00160 inline prec_t sqrt () const
00161 {
00162 prec_t ret;
00163 ret.v1 = ::sqrt(v1);
00164 mpf_sqrt (ret.v2, v2);
00165 return ret;
00166 }
00167
00168 inline prec_t abs () const
00169 {
00170 prec_t ret;
00171 ret.v1 = fabs(v1);
00172 mpf_abs (ret.v2, v2);
00173 return ret;
00174 }
00175
00176
00177 double error () const
00178 {
00179 if ( mpf_cmp_d(v2, 0) == 0 )
00180 return v1 == 0.0 ? 0 : 9e9;
00181 mpf_t e;
00182 mpf_init2 (e, MPF_BITS_USE);
00183 mpf_set_d (e, v1);
00184 mpf_div (e, e, v2);
00185 double ret = fabs(100.0 * mpf_get_d(e) - 100.0);
00186 mpf_clear(e);
00187 return ret;
00188 }
00189
00190 inline double d() const { return v1; }
00191 inline double rd() const { return mpf_get_d(v2); }
00192 inline void correct () { v1 = mpf_get_d(v2); }
00193
00194 private:
00195 double v1;
00196 mpf_t v2;
00197 };
00198
00199 inline prec_t prec_t_sqrt (const prec_t& v) { return v.sqrt(); }
00200 inline double prec2double (const prec_t& p) { return p.d(); }
00201 inline size_t prec2size_t (const prec_t& p) { return (size_t)p.d(); }
00202
00203 inline double cos (const prec_t& v) { return cos(v.d()); }
00204 inline double acos (const prec_t& v) { return acos(v.d()); }
00205 inline double sin (const prec_t& v) { return sin(v.d()); }
00206 inline double asin (const prec_t& v) { return asin(v.d()); }
00207 inline double tan (const prec_t& v) { return tan(v.d()); }
00208 inline double atan (const prec_t& v) { return atan(v.d()); }
00209 inline double floor (const prec_t& v) { return floor(v.d()); }
00210 inline double ceil (const prec_t& v) { return ceil(v.d()); }
00211 inline double round (const prec_t& v) { return round(v.d()); }
00212 inline double lround (const prec_t& v) { return lround(v.d()); }
00213 inline double prec_t_pow (const prec_t& v, const prec_t& f) { return pow(v.d(), f.d()); }
00214 inline double prec_t_log (const prec_t& v) { return log(v.d()); }
00215 inline prec_t fabs (const prec_t& v) { return v.abs(); }
00216
00217
00218 inline void do_prec_checkerror(const char *path, int lineno, const prec_t& v, const char *name, prec_t level = 10.0)
00219 {
00220 double e = v.error();
00221 if ( e > level )
00222 printf ("%s:%u: %s off by %.2e%%\n", path, lineno, name, e);
00223 }
00224
00225 #define prec_checkerror(a,b,c) do_prec_checkerror(__FILE__,__LINE__,a,b,c)
00226 inline void prec_clearerror(prec_t& v) { v = prec2double(v); }
00227 inline void prec_correcterror(prec_t& v) { v.correct(); }
00228
00229 #define PREC_ERROR
00230 #elif defined(USE_FLOAT)
00231 typedef float prec_t;
00232 #define prec_t_sqrt sqrtf
00233 #define prec_t_log logf
00234 #define prec_t_pow powf
00235 #define prec2double(v) ((double)(v))
00236 #define prec2size_t(v) ((size_t)(v))
00237 #define prec_checkerror(a,b,c)
00238 #define prec_clearerror(a)
00239 #define prec_correcterror(a)
00240 #define PREC_FLOAT
00241 #elif defined(USE_DOUBLEDOUBLE)
00242 typedef long double prec_t;
00243 #define prec_t_sqrt sqrtl
00244 #define prec_t_log logl
00245 #define prec_t_pow powl
00246 #define prec2double(v) ((double)(v))
00247 #define prec2size_t(v) ((size_t)(v))
00248 #define prec_checkerror(a,b,c)
00249 #define prec_clearerror(a)
00250 #define prec_correcterror(a)
00251 #define PREC_DOUBLEDOUBLE
00252 #elif defined(USE_GMP)
00253 struct prec_t
00254 {
00255 inline prec_t ()
00256 {
00257 mpf_init2(v2, GMP_MPF_BITS);
00258 }
00259
00260 inline prec_t(double v)
00261 {
00262 mpf_init2 (v2, GMP_MPF_BITS);
00263 mpf_set_d (v2, v);
00264 }
00265
00266 inline prec_t(const prec_t& c)
00267 {
00268 mpf_init2 (v2, GMP_MPF_BITS);
00269 mpf_set (v2, c.v2);
00270 }
00271
00272 inline prec_t& operator= (const prec_t& c)
00273 {
00274 mpf_set (v2, c.v2);
00275 return *this;
00276 }
00277
00278 inline prec_t& operator= (double v)
00279 {
00280 mpf_set_d (v2, v);
00281 return *this;
00282 }
00283
00284 inline ~prec_t ()
00285 {
00286 mpf_clear (v2);
00287 }
00288
00289 friend inline bool operator== (const prec_t& l, const prec_t& r) { return mpf_cmp(l.v2, r.v2) == 0; }
00290 friend inline bool operator!= (const prec_t& l, const prec_t& r) { return mpf_cmp(l.v2, r.v2) != 0; }
00291 friend inline bool operator< (const prec_t& l, const prec_t& r) { return mpf_cmp(l.v2, r.v2) < 0; }
00292 friend inline bool operator<= (const prec_t& l, const prec_t& r) { return mpf_cmp(l.v2, r.v2) <= 0; }
00293 friend inline bool operator> (const prec_t& l, const prec_t& r) { return mpf_cmp(l.v2, r.v2) > 0; }
00294 friend inline bool operator>= (const prec_t& l, const prec_t& r) { return mpf_cmp(l.v2, r.v2) >= 0; }
00295
00296 inline prec_t& operator += (const prec_t& r) { mpf_add (v2, v2, r.v2); return *this; }
00297 inline prec_t& operator -= (const prec_t& r) { mpf_sub (v2, v2, r.v2); return *this; }
00298 inline prec_t& operator *= (const prec_t& r) { mpf_mul (v2, v2, r.v2); return *this; }
00299 inline prec_t& operator /= (const prec_t& r)
00300 {
00301 if ( mpf_cmp_d (r.v2, 0) != 0 )
00302 {
00303 mpf_div (v2, v2, r.v2);
00304 }
00305 else
00306 {
00307 mpf_set_d (v2, 0);
00308 }
00309 return *this;
00310 }
00311
00312 inline prec_t operator- () const
00313 {
00314 prec_t ret;
00315 mpf_neg (ret.v2, v2);
00316 return ret;
00317 }
00318
00319 inline friend prec_t operator+ (const prec_t& l, const prec_t& r)
00320 {
00321 prec_t ret;
00322 mpf_add (ret.v2, l.v2, r.v2);
00323 return ret;
00324 }
00325
00326 inline friend prec_t operator- (const prec_t& l, const prec_t& r)
00327 {
00328 prec_t ret;
00329 mpf_sub (ret.v2, l.v2, r.v2);
00330 return ret;
00331 }
00332
00333 inline friend prec_t operator* (const prec_t& l, const prec_t& r)
00334 {
00335 prec_t ret;
00336 mpf_mul (ret.v2, l.v2, r.v2);
00337 return ret;
00338 }
00339
00340 inline friend prec_t operator/ (const prec_t& l, const prec_t& r)
00341 {
00342 prec_t ret;
00343 if ( mpf_cmp_d (r.v2, 0) != 0 )
00344 {
00345 mpf_div (ret.v2, l.v2, r.v2);
00346 }
00347 else
00348 {
00349 mpf_set_d (ret.v2, 0);
00350 }
00351 return ret;
00352 }
00353
00354 inline prec_t sqrt () const
00355 {
00356 prec_t ret;
00357 mpf_sqrt (ret.v2, v2);
00358 return ret;
00359 }
00360
00361 inline prec_t abs () const
00362 {
00363 prec_t ret;
00364 mpf_abs (ret.v2, v2);
00365 return ret;
00366 }
00367
00368 inline double d() const { return mpf_get_d(v2); }
00369
00370 private:
00371 mpf_t v2;
00372 };
00373 inline prec_t prec_t_sqrt (const prec_t& v) { return v.sqrt(); }
00374 inline double prec2double (const prec_t& p) { return p.d(); }
00375 inline size_t prec2size_t (const prec_t& p) { return (size_t)p.d(); }
00376 inline double cos (const prec_t& v) { return cos(v.d()); }
00377 inline double acos (const prec_t& v) { return acos(v.d()); }
00378 inline double sin (const prec_t& v) { return sin(v.d()); }
00379 inline double asin (const prec_t& v) { return asin(v.d()); }
00380 inline double tan (const prec_t& v) { return tan(v.d()); }
00381 inline double atan (const prec_t& v) { return atan(v.d()); }
00382 inline double floor (const prec_t& v) { return floor(v.d()); }
00383 inline double ceil (const prec_t& v) { return ceil(v.d()); }
00384 inline double round (const prec_t& v) { return round(v.d()); }
00385 inline double lround (const prec_t& v) { return lround(v.d()); }
00386 inline double prec_t_pow (const prec_t& v, const prec_t& f) { return pow(v.d(), f.d()); }
00387 inline double prec_t_log (const prec_t& v) { return log(v.d()); }
00388 inline prec_t fabs (const prec_t& v) { return v.abs(); }
00389 #else
00390 typedef double prec_t;
00391 #define prec_t_sqrt sqrt
00392 #define prec_t_log log
00393 #define prec_t_pow pow
00394 #define prec2double(v) ((double)(v))
00395 #define prec2size_t(v) ((size_t)(v))
00396 #define prec_checkerror(a,b,c)
00397 #define prec_clearerror(a)
00398 #define prec_correcterror(a)
00399 #define PREC_DOUBLE
00400 #endif
00401
00402 #if defined(_WIN32) || defined(_WINDOWS)
00403 #define ATTRIBUTE_PACKED
00404 #define ATTRIBUTE_ALIGNED16
00405
00406 __forceinline double prec_t_round (double v)
00407 {
00408 int result;
00409 __asm
00410 {
00411 fld v
00412 fistp result
00413 }
00414 return result;
00415 }
00416
00417 __forceinline long int prec_t_lrand ()
00418 {
00419 return rand();
00420 }
00421
00422 __forceinline prec_t prec_t_drand ()
00423 {
00424 return (prec_t)rand()/(prec_t)RAND_MAX;
00425 }
00426 #else
00427 #define ATTRIBUTE_PACKED __attribute__ ((packed))
00428 #define ATTRIBUTE_ALIGNED16 __attribute__ ((aligned (16)))
00429 #define prec_t_round round
00430 #define prec_t_lrand lrand48
00431 #define prec_t_drand drand48
00432 #endif
00433
00434 #define PREC_PI 3.141592653589793238462643383279502884197169399375105820974944592304
00435 #define PREC_SQRT2 1.414213562373095048801688724209698078569671875376948073176679737990
00436
00438 struct vect3d
00439 {
00440 prec_t x, y, z;
00441
00442 inline vect3d() { }
00443 inline vect3d(prec_t a, prec_t b, prec_t c) : x(a), y(b), z(c) { }
00444
00445 inline void clear () { x = 0.0; y = 0.0; z = 0.0; }
00446 inline void add (const vect3d& a) { x += a.x; y += a.y; z += a.z; }
00447 inline void normalize () { *this /= length(); }
00449 inline vect3d normal () const { vect3d ret = *this; ret.normalize(); return ret; }
00451 inline prec_t magnitude () const { return *this * *this; }
00453 inline prec_t length () const { return prec_t_sqrt (magnitude()); }
00454
00455 inline void operator*= (prec_t f) { x *= f; y *= f; z *= f; }
00456 inline void operator+= (const vect3d& v) { add(v); }
00457 inline void operator-= (const vect3d& v) { add(-v); }
00458 inline void operator/= (prec_t f) { x /= f; y /= f; z /= f; }
00459
00460 inline void operator= (prec_t f) { x = f; y = f; z = f; }
00461
00462 friend inline vect3d operator* (prec_t f, const vect3d& v)
00463 { return vect3d(v.x * f, v.y * f, v.z * f); }
00464
00465 friend inline vect3d operator* (const vect3d& v, prec_t f)
00466 { return vect3d(v.x * f, v.y * f, v.z * f); }
00467
00468 friend inline vect3d operator/ (const vect3d& v, prec_t f)
00469 { return vect3d(v.x / f, v.y / f, v.z / f); }
00470
00471 friend inline vect3d operator+ (const vect3d& l, const vect3d& r)
00472 { return vect3d(l.x + r.x, l.y + r.y, l.z + r.z); }
00473
00474 friend inline vect3d operator- (const vect3d& l, const vect3d& r)
00475 { return vect3d(l.x - r.x, l.y - r.y, l.z - r.z); }
00476
00478 friend inline vect3d operator% (const vect3d& l, const vect3d& r)
00479 { return vect3d(l.y * r.z - l.z * r.y, l.z * r.x - l.x * r.z, l.x * r.y - l.y * r.x); }
00480
00482 friend inline prec_t operator* (const vect3d& l, const vect3d& r)
00483 { return l.x * r.x + l.y * r.y + l.z * r.z; }
00484
00485 friend inline bool operator== (const vect3d& l, const vect3d& r)
00486 { return l.x == r.x && l.y == r.y && l.z == r.z; }
00487
00488 friend inline bool operator!= (const vect3d& l, const vect3d& r)
00489 { return !(l == r); }
00490
00491 friend inline bool operator< (const vect3d& l, const vect3d& r)
00492 {
00493 if ( l.x < r.x )
00494 return true;
00495 else if ( l.x == r.x && l.y < r.y )
00496 return true;
00497 else
00498 return l.x == r.x && l.y == r.y && l.z < r.z;
00499 }
00500
00501 inline vect3d operator- () const { return -1.0 * *this; }
00502
00503 inline vect3d recip () const { return vect3d (1.0 / x, 1.0 / y, 1.0 / z); }
00504
00506 void across (vect3d& out);
00507
00509 void print(const char *name = 0) const;
00511 char * sprint(char *buf) const;
00512 };
00513
00515 struct transf3d
00516 {
00517 prec_t a, b, c;
00518 prec_t d, e, f;
00519 prec_t g, h, i;
00520
00521 transf3d () { }
00522
00523 transf3d (const vect3d& r1, const vect3d& r2, const vect3d& r3)
00524 {
00525 a = r1.x; b = r1.y; c = r1.z;
00526 d = r2.x; e = r2.y; f = r2.z;
00527 g = r3.x; h = r3.y; i = r3.z;
00528 }
00529
00530 transf3d (prec_t f1, prec_t f2, prec_t f3, prec_t f4, prec_t f5, prec_t f6, prec_t f7, prec_t f8, prec_t f9)
00531 {
00532 a = f1; b = f2; c = f3;
00533 d = f4; e = f5; f = f6;
00534 g = f7; h = f8; i = f9;
00535 }
00536
00538 void print(const char *name = 0) const;
00539
00541 void identity ()
00542 {
00543 a = e = i = 1;
00544 b = c = d = f = g = h = 0;
00545 }
00546
00548 void reflect (const vect3d& n)
00549 {
00550 a = 1 - 2 * n.x * n.x; b = -2 * n.x * n.y; c = -2 * n.x * n.z;
00551 d = -2 * n.x * n.y; e = 1 - 2 * n.y * n.y; f = -2 * n.y * n.z;
00552 g = -2 * n.x * n.z; h = -2 * n.y * n.z; i = 1 - 2 * n.z * n.z;
00553 }
00554
00556 vect3d transform(const vect3d& v) const
00557 {
00558 return vect3d (a * v.x + b * v.y + c * v.z,
00559 d * v.x + e * v.y + f * v.z,
00560 g * v.x + h * v.y + i * v.z);
00561 }
00562
00564 prec_t det() const
00565 {
00566 return a * e * i + c * d * h + b * f * g -
00567 a * f * h - b * d * i - c * e * g;
00568 }
00569
00571 transf3d transpose () const
00572 {
00573 return transf3d (
00574 a, d, g,
00575 b, e, h,
00576 c, f, i);
00577 }
00578
00580 transf3d adj () const
00581 {
00582 return transf3d (
00583 e*i-f*h, -(b*i-c*h), b*f-c*e,
00584 -(d*i-f*g), a*i-c*g, -(a*f-c*d),
00585 d*h-e*g, -(a*h-b*g), a*e-b*d
00586 );
00587 }
00588
00589 transf3d inverse () const
00590 {
00591 return adj() / det();
00592 }
00593
00594 friend transf3d operator/ (const transf3d& t, prec_t f)
00595 {
00596 return transf3d (
00597 t.a / f, t.b / f, t.c / f,
00598 t.d / f, t.e / f, t.f / f,
00599 t.g / f, t.h / f, t.i / f);
00600 }
00601
00602 friend transf3d operator* (const transf3d& t, prec_t f)
00603 {
00604 return transf3d (
00605 t.a*f, t.b*f, t.c*f,
00606 t.d*f, t.e*f, t.f*f,
00607 t.g*f, t.h*f, t.i*f);
00608 }
00609
00610 friend transf3d operator* (prec_t f, const transf3d& t)
00611 {
00612 return transf3d (
00613 t.a*f, t.b*f, t.c*f,
00614 t.d*f, t.e*f, t.f*f,
00615 t.g*f, t.h*f, t.i*f);
00616 }
00617
00618 friend vect3d operator* (const transf3d& t, const vect3d& v)
00619 {
00620 return t.transform(v);
00621 }
00622
00623 friend transf3d operator * (const transf3d& l, const transf3d r)
00624 {
00625 transf3d ret;
00626 ret.a = l.a * r.a + l.b * r.d + l.c * r.g;
00627 ret.b = l.a * r.b + l.b * r.e + l.c * r.h;
00628 ret.c = l.a * r.c + l.b * r.f + l.c * r.i;
00629 ret.d = l.d * r.a + l.e * r.d + l.f * r.g;
00630 ret.e = l.d * r.b + l.e * r.e + l.f * r.h;
00631 ret.f = l.d * r.c + l.e * r.f + l.f * r.i;
00632 ret.g = l.g * r.a + l.h * r.d + l.i * r.g;
00633 ret.h = l.g * r.b + l.h * r.e + l.i * r.h;
00634 ret.i = l.g * r.c + l.h * r.f + l.i * r.i;
00635 return ret;
00636 }
00637 };
00638
00639 struct transf3d_pair
00640 {
00641 transf3d f;
00642 transf3d b;
00643 };
00644
00646 extern prec_t ellipse1(prec_t kk);
00648 extern prec_t ellipse2(prec_t kk);
00652 extern prec_t ellipse2(prec_t kk, prec_t e1v);
00653
00654 extern prec_t prec_t_sub (prec_t a, prec_t b);
00655
00656 #endif // !__math3d_hpp__
00657