00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #ifndef __GVECTOR_H__
00017 #define __GVECTOR_H__
00018
00019 #include <math.h>
00020 #include "Shared/Util.h"
00021
00022 #define V3COMP(p) p.x,p.y,p.z
00023 #define V2COMP(p) p.x,p.y
00024
00025 namespace GVector {
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 template <class num>
00041 class vector3d{
00042 public:
00043 num x,y,z;
00044
00045 vector3d() : x(),y(),z()
00046 {}
00047 vector3d(num nx,num ny,num nz) : x(nx),y(ny),z(nz)
00048 {}
00049
00050 void set(num nx,num ny,num nz)
00051 {x=nx; y=ny; z=nz;}
00052 void set(vector3d<num> p)
00053 {x=p.x; y=p.y; z=p.z;}
00054
00055 vector3d<num> &operator=(const vector3d<num> p)
00056 {set(p); return(*this);}
00057
00058 num length() const;
00059 num sqlength() const;
00060 vector3d<num> norm() const;
00061 void normalize();
00062
00063 num dot(const vector3d<num> p) const;
00064 vector3d<num> cross(const vector3d<num> p) const;
00065
00066 vector3d<num> operator+=(const vector3d<num> p);
00067 vector3d<num> operator-=(const vector3d<num> p);
00068 vector3d<num> operator*=(const vector3d<num> p);
00069 vector3d<num> operator/=(const vector3d<num> p);
00070
00071 vector3d<num> operator+(const vector3d<num> p) const;
00072 vector3d<num> operator-(const vector3d<num> p) const;
00073 vector3d<num> operator*(const vector3d<num> p) const;
00074 vector3d<num> operator/(const vector3d<num> p) const;
00075
00076 vector3d<num> operator*(num f) const;
00077 vector3d<num> operator/(num f) const;
00078 vector3d<num> operator*=(num f);
00079 vector3d<num> operator/=(num f);
00080
00081 vector3d<num> operator-() const;
00082
00083 bool operator==(const vector3d<num> p) const;
00084 bool operator!=(const vector3d<num> p) const;
00085 bool operator< (const vector3d<num> p) const;
00086 bool operator> (const vector3d<num> p) const;
00087 bool operator<=(const vector3d<num> p) const;
00088 bool operator>=(const vector3d<num> p) const;
00089
00090 vector3d<num> rotate_x(const double a) const;
00091 vector3d<num> rotate_y(const double a) const;
00092 vector3d<num> rotate_z(const double a) const;
00093 };
00094
00095 template <class num>
00096 num vector3d<num>::length() const
00097 {
00098 return(sqrt(x*x + y*y + z*z));
00099 }
00100
00101 template <class num>
00102 num vector3d<num>::sqlength() const
00103 {
00104 return(x*x + y*y + z*z);
00105 }
00106
00107 template <class num>
00108 vector3d<num> vector3d<num>::norm() const
00109 {
00110 vector3d<num> p;
00111 num l;
00112
00113 l = sqrt(x*x + y*y + z*z);
00114 p.x = x / l;
00115 p.y = y / l;
00116 p.z = z / l;
00117
00118 return(p);
00119 }
00120
00121 template <class num>
00122 void vector3d<num>::normalize()
00123 {
00124 num l;
00125
00126 l = sqrt(x*x + y*y + z*z);
00127 x /= l;
00128 y /= l;
00129 z /= l;
00130 }
00131
00132 template <class num>
00133 num vector3d<num>::dot(const vector3d<num> p) const
00134 {
00135 return(x*p.x + y*p.y + z*p.z);
00136 }
00137
00138 template <class num>
00139 num dot(const vector3d<num> a,const vector3d<num> b)
00140 {
00141 return(a.x*b.x + a.y*b.y + a.z*b.z);
00142 }
00143
00144 template <class num>
00145 vector3d<num> vector3d<num>::cross(const vector3d<num> p) const
00146 {
00147 vector3d<num> r;
00148
00149
00150 r.x = y*p.z - z*p.y;
00151 r.y = z*p.x - x*p.z;
00152 r.z = x*p.y - y*p.x;
00153
00154 return(r);
00155 }
00156
00157 template <class num>
00158 vector3d<num> cross(const vector3d<num> a,const vector3d<num> b)
00159 {
00160 vector3d<num> r;
00161
00162
00163 r.x = a.y*b.z - a.z*b.y;
00164 r.y = a.z*b.x - a.x*b.z;
00165 r.z = a.x*b.y - a.y*b.x;
00166
00167 return(r);
00168 }
00169
00170 #define VECTOR3D_EQUAL_BINARY_OPERATOR(opr) \
00171 template <class num> \
00172 vector3d<num> vector3d<num>::operator opr (const vector3d<num> p) \
00173 { \
00174 x = x opr p.x; \
00175 y = y opr p.y; \
00176 z = z opr p.z; \
00177 return(*this); \
00178 }
00179
00180 VECTOR3D_EQUAL_BINARY_OPERATOR(+=)
00181 VECTOR3D_EQUAL_BINARY_OPERATOR(-=)
00182 VECTOR3D_EQUAL_BINARY_OPERATOR(*=)
00183 VECTOR3D_EQUAL_BINARY_OPERATOR(/=)
00184
00185 #define VECTOR3D_BINARY_OPERATOR(opr) \
00186 template <class num> \
00187 vector3d<num> vector3d<num>::operator opr (const vector3d<num> p) const \
00188 { \
00189 vector3d<num> r; \
00190 r.x = x opr p.x; \
00191 r.y = y opr p.y; \
00192 r.z = z opr p.z; \
00193 return(r); \
00194 }
00195
00196 VECTOR3D_BINARY_OPERATOR(+)
00197 VECTOR3D_BINARY_OPERATOR(-)
00198 VECTOR3D_BINARY_OPERATOR(*)
00199 VECTOR3D_BINARY_OPERATOR(/)
00200
00201 #define VECTOR3D_SCALAR_OPERATOR(opr) \
00202 template <class num> \
00203 vector3d<num> vector3d<num>::operator opr (const num f) const \
00204 { \
00205 vector3d<num> r; \
00206 r.x = x opr f; \
00207 r.y = y opr f; \
00208 r.z = z opr f; \
00209 return(r); \
00210 }
00211
00212 VECTOR3D_SCALAR_OPERATOR(*)
00213 VECTOR3D_SCALAR_OPERATOR(/)
00214
00215 #define VECTOR3D_EQUAL_SCALAR_OPERATOR(opr) \
00216 template <class num> \
00217 vector3d<num> vector3d<num>::operator opr (num f) \
00218 { \
00219 x = x opr f; \
00220 y = y opr f; \
00221 z = z opr f; \
00222 return(*this); \
00223 }
00224
00225 VECTOR3D_EQUAL_SCALAR_OPERATOR(*=)
00226 VECTOR3D_EQUAL_SCALAR_OPERATOR(/=)
00227
00228 #define VECTOR3D_LOGIC_OPERATOR(opr,combine) \
00229 template <class num> \
00230 bool vector3d<num>::operator opr (const vector3d<num> p) const \
00231 { \
00232 return((x opr p.x) combine \
00233 (y opr p.y) combine \
00234 (z opr p.z)); \
00235 }
00236
00237 VECTOR3D_LOGIC_OPERATOR(==,&&)
00238 VECTOR3D_LOGIC_OPERATOR(!=,||)
00239
00240 VECTOR3D_LOGIC_OPERATOR(< ,&&)
00241 VECTOR3D_LOGIC_OPERATOR(> ,&&)
00242 VECTOR3D_LOGIC_OPERATOR(<=,&&)
00243 VECTOR3D_LOGIC_OPERATOR(>=,&&)
00244
00245 template <class num>
00246 vector3d<num> vector3d<num>::operator-() const
00247 {
00248 vector3d<num> r;
00249 r.x = -x;
00250 r.y = -y;
00251 r.z = -z;
00252 return(r);
00253 }
00254
00255
00256 template <class num>
00257 vector3d<num> vector3d<num>::rotate_x(const double a) const
00258 {
00259 vector3d<num> q;
00260 double s,c;
00261
00262 s = sin(a);
00263 c = cos(a);
00264
00265 q.x = x;
00266 q.y = c*y + -s*z;
00267 q.z = s*y + c*z;
00268
00269 return(q);
00270 }
00271
00272
00273 template <class num>
00274 vector3d<num> vector3d<num>::rotate_y(const double a) const
00275 {
00276 vector3d<num> q;
00277 double s,c;
00278
00279 s = sin(a);
00280 c = cos(a);
00281
00282 q.x = c*x + s*z;
00283 q.y = y;
00284 q.z = -s*x + c*z;
00285
00286 return(q);
00287 }
00288
00289
00290 template <class num>
00291 vector3d<num> vector3d<num>::rotate_z(const double a) const
00292 {
00293 vector3d<num> q;
00294 double s,c;
00295
00296 s = sin(a);
00297 c = cos(a);
00298
00299 q.x = c*x + -s*y;
00300 q.y = s*x + c*y;
00301 q.z = z;
00302
00303 return(q);
00304 }
00305
00306
00307 template <class num>
00308 num distance(const vector3d<num> a,const vector3d<num> b)
00309 {
00310 num dx,dy,dz;
00311
00312 dx = a.x - b.x;
00313 dy = a.y - b.y;
00314 dz = a.z - b.z;
00315
00316 return(sqrt(dx*dx + dy*dy + dz*dz));
00317 }
00318
00319
00320 template <class num>
00321 num sdistance(const vector3d<num> a,const vector3d<num> b)
00322 {
00323 num dx,dy,dz;
00324
00325 dx = a.x - b.x;
00326 dy = a.y - b.y;
00327 dz = a.z - b.z;
00328
00329 return(dx*dx + dy*dy + dz*dz);
00330 }
00331
00332
00333 template <class num>
00334 num distance_to_line(const vector3d<num> x0,const vector3d<num> x1,const vector3d<num> p)
00335 {
00336 vector3d<num> x;
00337 num t;
00338
00339 t = ((p.x - x0.x) + (p.y - x0.y) + (p.z - x0.z)) / (x1.x + x1.y + x1.z);
00340 x = x0 + (x1 - x0) * t;
00341
00342 return(distance(x,p));
00343 }
00344
00345
00346
00347
00348
00349
00350 template <class num>
00351 class vector2d{
00352 public:
00353 num x,y;
00354
00355 vector2d() : x(), y()
00356 {}
00357 vector2d(num nx,num ny) : x(nx), y(ny)
00358 {}
00359
00360 void set(num nx,num ny)
00361 {x=nx; y=ny;}
00362 void set(vector2d<num> p)
00363 {x=p.x; y=p.y;}
00364 vector2d<num> &operator=(vector2d<num> p)
00365 {set(p); return(*this);}
00366
00367 num length() const;
00368 num sqlength() const;
00369 num angle() const
00370 {return(atan2a(y,x));}
00371
00372 vector2d<num> norm() const;
00373 void normalize();
00374
00375 num dot(const vector2d<num> p) const;
00376 vector2d<num> cross(const vector2d<num> p) const;
00377
00378 vector2d<num> operator+=(const vector2d<num> p);
00379 vector2d<num> operator-=(const vector2d<num> p);
00380 vector2d<num> operator*=(const vector2d<num> p);
00381 vector2d<num> operator/=(const vector2d<num> p);
00382
00383 vector2d<num> operator+(const vector2d<num> p) const;
00384 vector2d<num> operator-(const vector2d<num> p) const;
00385 vector2d<num> operator*(const vector2d<num> p) const;
00386 vector2d<num> operator/(const vector2d<num> p) const;
00387
00388 vector2d<num> operator*(const num f) const;
00389 vector2d<num> operator/(const num f) const;
00390 vector2d<num> operator*=(num f);
00391 vector2d<num> operator/=(num f);
00392
00393 vector2d<num> operator-() const;
00394
00395 bool operator==(const vector2d<num> p) const;
00396 bool operator!=(const vector2d<num> p) const;
00397 bool operator< (const vector2d<num> p) const;
00398 bool operator> (const vector2d<num> p) const;
00399 bool operator<=(const vector2d<num> p) const;
00400 bool operator>=(const vector2d<num> p) const;
00401
00402 vector2d<num> rotate(const num a) const;
00403 };
00404
00405 template <class num>
00406 num vector2d<num>::length() const
00407 {
00408 return(sqrt(x*x + y*y));
00409 }
00410
00411 template <class num>
00412 num vector2d<num>::sqlength() const
00413 {
00414 return(x*x + y*y);
00415 }
00416
00417 template <class num>
00418 vector2d<num> vector2d<num>::norm() const
00419 {
00420 vector2d<num> p;
00421 num l;
00422
00423 l = sqrt(x*x + y*y);
00424 p.x = x / l;
00425 p.y = y / l;
00426
00427 return(p);
00428 }
00429
00430 template <class num>
00431 void vector2d<num>::normalize()
00432 {
00433 num l;
00434
00435 l = sqrt(x*x + y*y);
00436 x /= l;
00437 y /= l;
00438 }
00439
00440 template <class num>
00441 num vector2d<num>::dot(const vector2d<num> p) const
00442 {
00443 return(x*p.x + y*p.y);
00444 }
00445
00446 template <class num>
00447 num dot(const vector2d<num> a,const vector2d<num> b)
00448 {
00449 return(a.x*b.x + a.y*b.y);
00450 }
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466 #define VECTOR2D_EQUAL_BINARY_OPERATOR(opr) \
00467 template <class num> \
00468 vector2d<num> vector2d<num>::operator opr (const vector2d<num> p) \
00469 { \
00470 x = x opr p.x; \
00471 y = y opr p.y; \
00472 return(*this); \
00473 }
00474
00475 VECTOR2D_EQUAL_BINARY_OPERATOR(+=)
00476 VECTOR2D_EQUAL_BINARY_OPERATOR(-=)
00477 VECTOR2D_EQUAL_BINARY_OPERATOR(*=)
00478 VECTOR2D_EQUAL_BINARY_OPERATOR(/=)
00479
00480 #define VECTOR2D_BINARY_OPERATOR(opr) \
00481 template <class num> \
00482 vector2d<num> vector2d<num>::operator opr (const vector2d<num> p) const \
00483 { \
00484 vector2d<num> r; \
00485 r.x = x opr p.x; \
00486 r.y = y opr p.y; \
00487 return(r); \
00488 }
00489
00490 VECTOR2D_BINARY_OPERATOR(+)
00491 VECTOR2D_BINARY_OPERATOR(-)
00492 VECTOR2D_BINARY_OPERATOR(*)
00493 VECTOR2D_BINARY_OPERATOR(/)
00494
00495 #define VECTOR2D_SCALAR_OPERATOR(opr) \
00496 template <class num> \
00497 vector2d<num> vector2d<num>::operator opr (const num f) const \
00498 { \
00499 vector2d<num> r; \
00500 r.x = x opr f; \
00501 r.y = y opr f; \
00502 return(r); \
00503 }
00504
00505 VECTOR2D_SCALAR_OPERATOR(*)
00506 VECTOR2D_SCALAR_OPERATOR(/)
00507
00508 #define VECTOR2D_EQUAL_SCALAR_OPERATOR(opr) \
00509 template <class num> \
00510 vector2d<num> vector2d<num>::operator opr (num f) \
00511 { \
00512 x = x opr f; \
00513 y = y opr f; \
00514 return(*this); \
00515 }
00516
00517 VECTOR2D_EQUAL_SCALAR_OPERATOR(*=)
00518 VECTOR2D_EQUAL_SCALAR_OPERATOR(/=)
00519
00520 #define VECTOR2D_LOGIC_OPERATOR(opr,combine) \
00521 template <class num> \
00522 bool vector2d<num>::operator opr (const vector2d<num> p) const \
00523 { \
00524 return((x opr p.x) combine \
00525 (y opr p.y)); \
00526 }
00527
00528 VECTOR2D_LOGIC_OPERATOR(==,&&)
00529 VECTOR2D_LOGIC_OPERATOR(!=,||)
00530
00531 VECTOR2D_LOGIC_OPERATOR(< ,&&)
00532 VECTOR2D_LOGIC_OPERATOR(> ,&&)
00533 VECTOR2D_LOGIC_OPERATOR(<=,&&)
00534 VECTOR2D_LOGIC_OPERATOR(>=,&&)
00535
00536
00537 template <class num>
00538 vector2d<num> vector2d<num>::operator-() const
00539 {
00540 vector2d<num> r;
00541 r.x = -x;
00542 r.y = -y;
00543 return(r);
00544 }
00545
00546 template <class num>
00547 vector2d<num> vector2d<num>::rotate(const num a) const {
00548 vector2d<num> q;
00549 double s,c;
00550
00551 s = sin(a);
00552 c = cos(a);
00553
00554 q.x = c*x + -s*y;
00555 q.y = s*x + c*y;
00556
00557 return(q);
00558 }
00559
00560 template <class num>
00561 num distance(const vector2d<num> a,const vector2d<num> b)
00562 {
00563 num dx,dy;
00564
00565 dx = a.x - b.x;
00566 dy = a.y - b.y;
00567
00568 return(sqrt(dx*dx + dy*dy));
00569 }
00570
00571
00572 template <class num>
00573 num sdistance(const vector2d<num> a,const vector2d<num> b)
00574 {
00575 num dx,dy;
00576
00577 dx = a.x - b.x;
00578 dy = a.y - b.y;
00579
00580 return(dx*dx + dy*dy);
00581 }
00582
00583
00584 template <class num>
00585 num distance_to_line(const vector2d<num> x0,const vector2d<num> x1,const vector2d<num> p)
00586 {
00587 vector2d<num> x;
00588 num t;
00589
00590 t = ((p.x - x0.x) + (p.y - x0.y)) / (x1.x + x1.y);
00591 x = x0 + (x1 - x0) * t;
00592
00593
00594
00595 return(distance(x,p));
00596 }
00597
00598
00599 template <class num>
00600 num offset_to_line(const vector2d<num> x0,const vector2d<num> x1,const vector2d<num> p)
00601 {
00602 vector2d<num> n,v;
00603
00604
00605 n = x1 - x0;
00606 n.set(n.y,-n.x);
00607
00608 v = p - x0;
00609
00610 return(n.dot(v));
00611 }
00612
00613
00614
00615
00616
00617 template <class vector>
00618 vector point_on_segment(const vector x0,const vector x1,const vector p)
00619 {
00620 vector q,dx;
00621 double f,l;
00622
00623 dx = x1 - x0;
00624 f = dot(dx,p-x0);
00625 l = dx.sqlength();
00626
00627 if(f <= 0.0) return(x0);
00628 if(f > l) return(x1);
00629
00630 q = x0 + dx * (f / sqrt(l));
00631
00632 return(q);
00633 }
00634
00635
00636
00637
00638
00639
00640 template <class vector>
00641 bool intersect_ray_plane(const vector r0, const vector rd, const vector p0, const vector pn, vector &result) {
00642 double t;
00643 double d;
00644
00645 if(rd.dot(pn)==0.0)
00646 return false;
00647
00648 d = -p0.dot(pn);
00649
00650 t = -(r0.dot(pn) + d) / (rd.dot(pn));
00651
00652 result = r0 + rd * t;
00653
00654 return true;
00655 }
00656
00657 }
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686 #endif
00687