00001 #ifndef CD_VECTOR_H
00002
00003 #define CD_VECTOR_H
00004
00061
00062
00063
00064
00065
00066
00067
00068
00069 #pragma warning(disable:4786)
00070
00071 #include <math.h>
00072 #include <float.h>
00073 #include <vector>
00074
00075 namespace ConvexDecomposition
00076 {
00077
00078
00079 const double DEG_TO_RAD = ((2.0f * 3.14152654f) / 360.0f);
00080 const double RAD_TO_DEG = (360.0f / (2.0f * 3.141592654f));
00081
00082 template <class Type> class Vector3d
00083 {
00084 public:
00085 Vector3d(void) { };
00086
00087 Vector3d(const Vector3d &a)
00088 {
00089 x = a.x;
00090 y = a.y;
00091 z = a.z;
00092 };
00093
00094 Vector3d(Type a,Type b,Type c)
00095 {
00096 x = a;
00097 y = b;
00098 z = c;
00099 };
00100
00101 Vector3d(const double *t)
00102 {
00103 x = t[0];
00104 y = t[1];
00105 z = t[2];
00106 };
00107
00108 Vector3d(const int *t)
00109 {
00110 x = t[0];
00111 y = t[1];
00112 z = t[2];
00113 };
00114
00115 bool operator==(const Vector3d<Type> &a) const
00116 {
00117 return( a.x == x && a.y == y && a.z == z );
00118 };
00119
00120 bool operator!=(const Vector3d<Type> &a) const
00121 {
00122 return( a.x != x || a.y != y || a.z != z );
00123 };
00124
00125
00126 Vector3d& operator = (const Vector3d& A)
00127 { x=A.x; y=A.y; z=A.z;
00128 return(*this); };
00129
00130 Vector3d operator + (const Vector3d& A) const
00131 { Vector3d Sum(x+A.x, y+A.y, z+A.z);
00132 return(Sum); };
00133
00134 Vector3d operator - (const Vector3d& A) const
00135 { Vector3d Diff(x-A.x, y-A.y, z-A.z);
00136 return(Diff); };
00137
00138 Vector3d operator * (const double s) const
00139 { Vector3d Scaled(x*s, y*s, z*s);
00140 return(Scaled); };
00141
00142
00143 Vector3d operator + (const double s) const
00144 { Vector3d Scaled(x+s, y+s, z+s);
00145 return(Scaled); };
00146
00147
00148
00149
00150 Vector3d operator / (const double s) const
00151 {
00152 double r = 1.0f / s;
00153 Vector3d Scaled(x*r, y*r, z*r);
00154 return(Scaled);
00155 };
00156
00157 void operator /= (Type A)
00158 { x/=A; y/=A; z/=A; };
00159
00160 void operator += (const Vector3d A)
00161 { x+=A.x; y+=A.y; z+=A.z; };
00162 void operator -= (const Vector3d A)
00163 { x-=A.x; y-=A.y; z-=A.z; };
00164 void operator *= (const double s)
00165 {x*=s; y*=s; z*=s;}
00166
00167 void operator += (const double A)
00168 { x+=A; y+=A; z+=A; };
00169
00170
00171 Vector3d operator - (void) const
00172 { Vector3d Negated(-x, -y, -z);
00173 return(Negated); };
00174
00175 Type operator [] (const int i) const
00176 { return( (i==0)?x:((i==1)?y:z) ); };
00177 Type & operator [] (const int i)
00178 { return( (i==0)?x:((i==1)?y:z) ); };
00179
00180
00181
00182 Type GetX(void) const { return x; };
00183 Type GetY(void) const { return y; };
00184 Type GetZ(void) const { return z; };
00185
00186 Type X(void) const { return x; };
00187 Type Y(void) const { return y; };
00188 Type Z(void) const { return z; };
00189
00190 void SetX(Type t) { x = t; };
00191 void SetY(Type t) { y = t; };
00192 void SetZ(Type t) { z = t; };
00193
00194 bool IsSame(const Vector3d<double> &v,double epsilon) const
00195 {
00196 double dx = fabsf( x - v.x );
00197 if ( dx > epsilon ) return false;
00198 double dy = fabsf( y - v.y );
00199 if ( dy > epsilon ) return false;
00200 double dz = fabsf( z - v.z );
00201 if ( dz > epsilon ) return false;
00202 return true;
00203 }
00204
00205
00206 double ComputeNormal(const Vector3d<double> &A,
00207 const Vector3d<double> &B,
00208 const Vector3d<double> &C)
00209 {
00210 double vx,vy,vz,wx,wy,wz,vw_x,vw_y,vw_z,mag;
00211
00212 vx = (B.x - C.x);
00213 vy = (B.y - C.y);
00214 vz = (B.z - C.z);
00215
00216 wx = (A.x - B.x);
00217 wy = (A.y - B.y);
00218 wz = (A.z - B.z);
00219
00220 vw_x = vy * wz - vz * wy;
00221 vw_y = vz * wx - vx * wz;
00222 vw_z = vx * wy - vy * wx;
00223
00224 mag = sqrtf((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
00225
00226 if ( mag < 0.000001f )
00227 {
00228 mag = 0;
00229 }
00230 else
00231 {
00232 mag = 1.0f/mag;
00233 }
00234
00235 x = vw_x * mag;
00236 y = vw_y * mag;
00237 z = vw_z * mag;
00238
00239 return mag;
00240 }
00241
00242
00243 void ScaleSumScale(double c0,double c1,const Vector3d<double> &pos)
00244 {
00245 x = (x*c0) + (pos.x*c1);
00246 y = (y*c0) + (pos.y*c1);
00247 z = (z*c0) + (pos.z*c1);
00248 }
00249
00250 void SwapYZ(void)
00251 {
00252 double t = y;
00253 y = z;
00254 z = t;
00255 };
00256
00257 void Get(Type *v) const
00258 {
00259 v[0] = x;
00260 v[1] = y;
00261 v[2] = z;
00262 };
00263
00264 void Set(const int *p)
00265 {
00266 x = (Type) p[0];
00267 y = (Type) p[1];
00268 z = (Type) p[2];
00269 }
00270
00271 void Set(const double *p)
00272 {
00273 x = (Type) p[0];
00274 y = (Type) p[1];
00275 z = (Type) p[2];
00276 }
00277
00278
00279 void Set(Type a,Type b,Type c)
00280 {
00281 x = a;
00282 y = b;
00283 z = c;
00284 };
00285
00286 void Zero(void)
00287 {
00288 x = y = z = 0;
00289 };
00290
00291 const Type* Ptr() const { return &x; }
00292 Type* Ptr() { return &x; }
00293
00294
00295
00296 Vector3d negative(void) const
00297 {
00298 Vector3d result;
00299 result.x = -x;
00300 result.y = -y;
00301 result.z = -z;
00302 return result;
00303 }
00304
00305 Type Magnitude(void) const
00306 {
00307 return Type(sqrt(x * x + y * y + z * z));
00308 };
00309
00310 Type FastMagnitude(void) const
00311 {
00312 return Type(sqrt(x * x + y * y + z * z));
00313 };
00314
00315 Type FasterMagnitude(void) const
00316 {
00317 return Type(sqrt(x * x + y * y + z * z));
00318 };
00319
00320 void Lerp(const Vector3d<Type>& from,const Vector3d<Type>& to,double slerp)
00321 {
00322 x = ((to.x - from.x) * slerp) + from.x;
00323 y = ((to.y - from.y) * slerp) + from.y;
00324 z = ((to.z - from.z) * slerp) + from.z;
00325 };
00326
00327
00328
00329
00330
00331
00332 void Interpolate(const Vector3d<double> &from,const Vector3d<double> &to,double offset)
00333 {
00334 x = to.x-from.x;
00335 y = to.y-from.y;
00336 z = to.z-from.z;
00337 double d = sqrtf( x*x + y*y + z*z );
00338 double recip = 1.0f / d;
00339 x*=recip;
00340 y*=recip;
00341 z*=recip;
00342 d+=offset;
00343 x = x*d + from.x;
00344 y = y*d + from.y;
00345 z = z*d + from.z;
00346 };
00347
00348 bool BinaryEqual(const Vector3d<double> &p) const
00349 {
00350 const int *source = (const int *) &x;
00351 const int *dest = (const int *) &p.x;
00352
00353 if ( source[0] == dest[0] &&
00354 source[1] == dest[1] &&
00355 source[2] == dest[2] ) return true;
00356
00357 return false;
00358 };
00359
00360 bool BinaryEqual(const Vector3d<int> &p) const
00361 {
00362 if ( x == p.x && y == p.y && z == p.z ) return true;
00363 return false;
00364 }
00365
00366
00368 void Reflection(const Vector3d<Type> &a,const Vector3d<Type> &b)
00369 {
00370 Vector3d<double> c;
00371 Vector3d<double> d;
00372
00373 double dot = a.Dot(b) * 2.0f;
00374
00375 c = b * dot;
00376
00377 d = c - a;
00378
00379 x = -d.x;
00380 y = -d.y;
00381 z = -d.z;
00382 };
00383
00384 void AngleAxis(Type angle,const Vector3d<Type>& axis)
00385 {
00386 x = axis.x*angle;
00387 y = axis.y*angle;
00388 z = axis.z*angle;
00389 };
00390
00391 Type Length(void) const
00392 {
00393 return Type(sqrt( x*x + y*y + z*z ));
00394 };
00395
00396
00397 double ComputePlane(const Vector3d<double> &A,
00398 const Vector3d<double> &B,
00399 const Vector3d<double> &C)
00400 {
00401 double vx,vy,vz,wx,wy,wz,vw_x,vw_y,vw_z,mag;
00402
00403 vx = (B.x - C.x);
00404 vy = (B.y - C.y);
00405 vz = (B.z - C.z);
00406
00407 wx = (A.x - B.x);
00408 wy = (A.y - B.y);
00409 wz = (A.z - B.z);
00410
00411 vw_x = vy * wz - vz * wy;
00412 vw_y = vz * wx - vx * wz;
00413 vw_z = vx * wy - vy * wx;
00414
00415 mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
00416
00417 if ( mag < 0.000001f )
00418 {
00419 mag = 0;
00420 }
00421 else
00422 {
00423 mag = 1.0f/mag;
00424 }
00425
00426 x = vw_x * mag;
00427 y = vw_y * mag;
00428 z = vw_z * mag;
00429
00430
00431 double D = 0.0f - ((x*A.x)+(y*A.y)+(z*A.z));
00432
00433 return D;
00434 }
00435
00436
00437 Type FastLength(void) const
00438 {
00439 return Type(sqrt( x*x + y*y + z*z ));
00440 };
00441
00442
00443 Type FasterLength(void) const
00444 {
00445 return Type(sqrt( x*x + y*y + z*z ));
00446 };
00447
00448 Type Length2(void) const
00449 {
00450 Type l2 = x*x+y*y+z*z;
00451 return l2;
00452 };
00453
00454 Type Distance(const Vector3d<Type> &a) const
00455 {
00456 Vector3d<Type> d(a.x-x,a.y-y,a.z-z);
00457 return d.Length();
00458 }
00459
00460 Type FastDistance(const Vector3d<Type> &a) const
00461 {
00462 Vector3d<Type> d(a.x-x,a.y-y,a.z-z);
00463 return d.FastLength();
00464 }
00465
00466 Type FasterDistance(const Vector3d<Type> &a) const
00467 {
00468 Vector3d<Type> d(a.x-x,a.y-y,a.z-z);
00469 return d.FasterLength();
00470 }
00471
00472
00473 Type DistanceXY(const Vector3d<Type> &a) const
00474 {
00475 double dx = a.x - x;
00476 double dy = a.y - y;
00477 double dist = dx*dx + dy*dy;
00478 return dist;
00479 }
00480
00481 Type Distance2(const Vector3d<Type> &a) const
00482 {
00483 double dx = a.x - x;
00484 double dy = a.y - y;
00485 double dz = a.z - z;
00486 return dx*dx + dy*dy + dz*dz;
00487 };
00488
00489 Type Partial(const Vector3d<Type> &p) const
00490 {
00491 return (x*p.y) - (p.x*y);
00492 }
00493
00494 Type Area(const Vector3d<Type> &p1,const Vector3d<Type> &p2) const
00495 {
00496 Type A = Partial(p1);
00497 A+= p1.Partial(p2);
00498 A+= p2.Partial(*this);
00499 return A*0.5f;
00500 }
00501
00502 inline double Normalize(void)
00503 {
00504 double d = sqrtf( static_cast< double >( x*x + y*y + z*z ) );
00505 if ( d > 0 )
00506 {
00507 double r = 1.0f / d;
00508 x *= r;
00509 y *= r;
00510 z *= r;
00511 }
00512 else
00513 {
00514 x = y = z = 1;
00515 }
00516 return d;
00517 };
00518
00519 inline double FastNormalize(void)
00520 {
00521 double d = sqrt( static_cast< double >( x*x + y*y + z*z ) );
00522 if ( d > 0 )
00523 {
00524 double r = 1.0f / d;
00525 x *= r;
00526 y *= r;
00527 z *= r;
00528 }
00529 else
00530 {
00531 x = y = z = 1;
00532 }
00533 return d;
00534 };
00535
00536 inline double FasterNormalize(void)
00537 {
00538 double d = sqrt( static_cast< double >( x*x + y*y + z*z ) );
00539 if ( d > 0 )
00540 {
00541 double r = 1.0f / d;
00542 x *= r;
00543 y *= r;
00544 z *= r;
00545 }
00546 else
00547 {
00548 x = y = z = 1;
00549 }
00550 return d;
00551 };
00552
00553
00554
00555
00556 Type Dot(const Vector3d<Type> &a) const
00557 {
00558 return (x * a.x + y * a.y + z * a.z );
00559 };
00560
00561
00562 Vector3d<Type> Cross( const Vector3d<Type>& other ) const
00563 {
00564 Vector3d<Type> result( y*other.z - z*other.y, z*other.x - x*other.z, x*other.y - y*other.x );
00565
00566 return result;
00567 }
00568
00569 void Cross(const Vector3d<Type> &a,const Vector3d<Type> &b)
00570 {
00571 x = a.y*b.z - a.z*b.y;
00572 y = a.z*b.x - a.x*b.z;
00573 z = a.x*b.y - a.y*b.x;
00574 };
00575
00576
00577
00578
00579
00580
00581
00582 bool Concave(const Vector3d<double>& a,const Vector3d<double>& b)
00583 {
00584 double vx,vy,vz,wx,wy,wz,vw_x,vw_y,vw_z,mag,nx,ny,nz,mag_a,mag_b;
00585
00586 wx = b.x - a.x;
00587 wy = b.y - a.y;
00588 wz = b.z - a.z;
00589
00590 mag_a = (double) sqrtf((wx * wx) + (wy * wy) + (wz * wz));
00591
00592 vx = x - b.x;
00593 vy = y - b.y;
00594 vz = z - b.z;
00595
00596 mag_b = (double) sqrtf((vx * vx) + (vy * vy) + (vz * vz));
00597
00598 vw_x = (vy * wz) - (vz * wy);
00599 vw_y = (vz * wx) - (vx * wz);
00600 vw_z = (vx * wy) - (vy * wx);
00601
00602 mag = (double) sqrtf((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612 if (mag/(mag_a*mag_b) <= 0.0f ) return true;
00613
00614 mag = 1.0f / mag;
00615
00616 nx = vw_x * mag;
00617 ny = vw_y * mag;
00618 nz = vw_z * mag;
00619
00620
00621
00622
00623
00624
00625 mag = ( x * nx) + ( y * ny) + ( z * nz);
00626
00627 if (mag > 0.0f ) return false;
00628
00629 return(true);
00630 };
00631
00632 bool PointTestXY(const Vector3d<double> &i,const Vector3d<double> &j) const
00633 {
00634 if (((( i.y <= y ) && ( y < j.y )) ||
00635 (( j.y <= y ) && ( y < i.y ))) &&
00636 ( x < (j.x - i.x) * (y - i.y) / (j.y - i.y) + i.x)) return true;
00637 return false;
00638 }
00639
00640
00641
00642 bool PointInTriXY(const Vector3d<double> &p1,
00643 const Vector3d<double> &p2,
00644 const Vector3d<double> &p3) const
00645 {
00646 double ax = p3.x - p2.x;
00647 double ay = p3.y - p2.y;
00648 double bx = p1.x - p3.x;
00649 double by = p1.y - p3.y;
00650 double cx = p2.x - p1.x;
00651 double cy = p2.y - p1.y;
00652 double apx = x - p1.x;
00653 double apy = y - p1.y;
00654 double bpx = x - p2.x;
00655 double bpy = y - p2.y;
00656 double cpx = x - p3.x;
00657 double cpy = y - p3.y;
00658
00659 double aCROSSbp = ax*bpy - ay*bpx;
00660 double cCROSSap = cx*apy - cy*apx;
00661 double bCROSScp = bx*cpy - by*cpx;
00662
00663 return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
00664 };
00665
00666
00667
00668 bool PointInTriYZ(const Vector3d<double> &p1,
00669 const Vector3d<double> &p2,
00670 const Vector3d<double> &p3) const
00671 {
00672 double ay = p3.y - p2.y;
00673 double az = p3.z - p2.z;
00674 double by = p1.y - p3.y;
00675 double bz = p1.z - p3.z;
00676 double cy = p2.y - p1.y;
00677 double cz = p2.z - p1.z;
00678 double apy = y - p1.y;
00679 double apz = z - p1.z;
00680 double bpy = y - p2.y;
00681 double bpz = z - p2.z;
00682 double cpy = y - p3.y;
00683 double cpz = z - p3.z;
00684
00685 double aCROSSbp = ay*bpz - az*bpy;
00686 double cCROSSap = cy*apz - cz*apy;
00687 double bCROSScp = by*cpz - bz*cpy;
00688
00689 return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
00690 };
00691
00692
00693
00694
00695 bool PointInTriXZ(const Vector3d<double> &p1,
00696 const Vector3d<double> &p2,
00697 const Vector3d<double> &p3) const
00698 {
00699 double az = p3.z - p2.z;
00700 double ax = p3.x - p2.x;
00701 double bz = p1.z - p3.z;
00702 double bx = p1.x - p3.x;
00703 double cz = p2.z - p1.z;
00704 double cx = p2.x - p1.x;
00705 double apz = z - p1.z;
00706 double apx = x - p1.x;
00707 double bpz = z - p2.z;
00708 double bpx = x - p2.x;
00709 double cpz = z - p3.z;
00710 double cpx = x - p3.x;
00711
00712 double aCROSSbp = az*bpx - ax*bpz;
00713 double cCROSSap = cz*apx - cx*apz;
00714 double bCROSScp = bz*cpx - bx*cpz;
00715
00716 return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
00717 };
00718
00719
00720
00721 void NearestPointInLine(const Vector3d<Type> &point,
00722 const Vector3d<Type> &line0,
00723 const Vector3d<Type> &line1)
00724 {
00725 Vector3d<Type> &nearestPoint = *this;
00726 Vector3d<Type> lineDelta = line1 - line0;
00727
00728
00729 if ( lineDelta == Vector3d<Type>(0, 0, 0) )
00730 {
00731 nearestPoint = line0;
00732 }
00733 else
00734 {
00735 double delta = (point-line0).Dot(lineDelta) / (lineDelta).Dot(lineDelta);
00736 nearestPoint = line0 + delta*lineDelta;
00737 }
00738 }
00739
00740
00741
00742 void NearestPointInLineSegment(const Vector3d<Type> &point,
00743 const Vector3d<Type> &line0,
00744 const Vector3d<Type> &line1)
00745 {
00746 Vector3d<Type> &nearestPoint = *this;
00747 Vector3d<Type> lineDelta = line1 - line0;
00748
00749
00750 if ( lineDelta == Vector3d<double>(0, 0, 0) )
00751 {
00752 nearestPoint = line0;
00753 }
00754 else
00755 {
00756 double delta = (point-line0).Dot(lineDelta) / (lineDelta).Dot(lineDelta);
00757
00758
00759 if ( delta < 0 )
00760 delta = 0;
00761 else if ( delta > 1 )
00762 delta = 1;
00763
00764 nearestPoint = line0 + delta*lineDelta;
00765 }
00766 }
00767
00768
00769
00770 void NearestPointInPlane(const Vector3d<Type> &point,
00771 const Vector3d<Type> &triangle0,
00772 const Vector3d<Type> &triangle1,
00773 const Vector3d<Type> &triangle2)
00774 {
00775 Vector3d<Type> &nearestPoint = *this;
00776 Vector3d<Type> lineDelta0 = triangle1 - triangle0;
00777 Vector3d<Type> lineDelta1 = triangle2 - triangle0;
00778 Vector3d<Type> pointDelta = point - triangle0;
00779 Vector3d<Type> normal;
00780
00781
00782 normal.Cross(lineDelta0, lineDelta1);
00783
00784 double delta = normal.Dot(pointDelta) / normal.Dot(normal);
00785 nearestPoint = point - delta*normal;
00786 }
00787
00788
00789
00790 void NearestPointInPlane(const Vector3d<Type> &point,
00791 const Vector3d<Type> &planePoint,
00792 const Vector3d<Type> &planeNormal)
00793 {
00794 Vector3d<Type> &nearestPoint = *this;
00795 Vector3d<Type> pointDelta = point - planePoint;
00796
00797 double delta = planeNormal.Dot(pointDelta) / planeNormal.Dot(planeNormal);
00798 nearestPoint = point - delta*planeNormal;
00799 }
00800
00801
00802
00803 void NearestPointInTriangle(const Vector3d<Type> &point,
00804 const Vector3d<Type> &triangle0,
00805 const Vector3d<Type> &triangle1,
00806 const Vector3d<Type> &triangle2)
00807 {
00808 static const Vector3d<Type> zeroVector(0, 0, 0);
00809
00810 Vector3d<Type> &nearestPoint = *this;
00811
00812 Vector3d<Type> lineDelta0 = triangle1 - triangle0;
00813 Vector3d<Type> lineDelta1 = triangle2 - triangle0;
00814
00815
00816 if ( (lineDelta0 == zeroVector) || (lineDelta1 == zeroVector) )
00817 {
00818 nearestPoint.NearestPointInLineSegment(point, triangle1, triangle2);
00819 }
00820 else if ( lineDelta0 == lineDelta1 )
00821 {
00822 nearestPoint.NearestPointInLineSegment(point, triangle0, triangle1);
00823 }
00824
00825 else
00826 {
00827 static Vector3d<Type> axis[3];
00828 axis[0].NearestPointInLine(triangle0, triangle1, triangle2);
00829 axis[1].NearestPointInLine(triangle1, triangle0, triangle2);
00830 axis[2].NearestPointInLine(triangle2, triangle0, triangle1);
00831
00832 Type axisDot[3];
00833 axisDot[0] = (triangle0-axis[0]).Dot(point-axis[0]);
00834 axisDot[1] = (triangle1-axis[1]).Dot(point-axis[1]);
00835 axisDot[2] = (triangle2-axis[2]).Dot(point-axis[2]);
00836
00837 bool bForce = true;
00838 Type bestMagnitude2 = 0;
00839 Type closeMagnitude2;
00840 Vector3d<double> closePoint;
00841
00842 if ( axisDot[0] < 0 )
00843 {
00844 closePoint.NearestPointInLineSegment(point, triangle1, triangle2);
00845 closeMagnitude2 = point.Distance2(closePoint);
00846 if ( bForce || (bestMagnitude2 > closeMagnitude2) )
00847 {
00848 bForce = false;
00849 bestMagnitude2 = closeMagnitude2;
00850 nearestPoint = closePoint;
00851 }
00852 }
00853 if ( axisDot[1] < 0 )
00854 {
00855 closePoint.NearestPointInLineSegment(point, triangle0, triangle2);
00856 closeMagnitude2 = point.Distance2(closePoint);
00857 if ( bForce || (bestMagnitude2 > closeMagnitude2) )
00858 {
00859 bForce = false;
00860 bestMagnitude2 = closeMagnitude2;
00861 nearestPoint = closePoint;
00862 }
00863 }
00864 if ( axisDot[2] < 0 )
00865 {
00866 closePoint.NearestPointInLineSegment(point, triangle0, triangle1);
00867 closeMagnitude2 = point.Distance2(closePoint);
00868 if ( bForce || (bestMagnitude2 > closeMagnitude2) )
00869 {
00870 bForce = false;
00871 bestMagnitude2 = closeMagnitude2;
00872 nearestPoint = closePoint;
00873 }
00874 }
00875
00876
00877
00878 if ( bForce )
00879 {
00880 Vector3d<Type> normal;
00881
00882
00883 normal.Cross(lineDelta0, lineDelta1);
00884
00885 Vector3d<double> pointDelta = point - triangle0;
00886 double delta = normal.Dot(pointDelta) / normal.Dot(normal);
00887
00888 nearestPoint = point - delta*normal;
00889 }
00890 }
00891 }
00892
00893
00894
00895
00896 Type x;
00897 Type y;
00898 Type z;
00899 };
00900
00901
00902 template <class Type> class Vector2d
00903 {
00904 public:
00905 Vector2d(void) { };
00906
00907 Vector2d(const Vector2d &a)
00908 {
00909 x = a.x;
00910 y = a.y;
00911 };
00912
00913 Vector2d(const double *t)
00914 {
00915 x = t[0];
00916 y = t[1];
00917 };
00918
00919
00920 Vector2d(Type a,Type b)
00921 {
00922 x = a;
00923 y = b;
00924 };
00925
00926 const Type* Ptr() const { return &x; }
00927 Type* Ptr() { return &x; }
00928
00929 Vector2d & operator+=(const Vector2d &a)
00930 {
00931 x+=a.x;
00932 y+=a.y;
00933 return *this;
00934 };
00935
00936 Vector2d & operator-=(const Vector2d &a)
00937 {
00938 x-=a.x;
00939 y-=a.y;
00940 return *this;
00941 };
00942
00943 Vector2d & operator*=(const Vector2d &a)
00944 {
00945 x*=a.x;
00946 y*=a.y;
00947 return *this;
00948 };
00949
00950 Vector2d & operator/=(const Vector2d &a)
00951 {
00952 x/=a.x;
00953 y/=a.y;
00954 return *this;
00955 };
00956
00957 bool operator==(const Vector2d<Type> &a) const
00958 {
00959 if ( a.x == x && a.y == y ) return true;
00960 return false;
00961 };
00962
00963 bool operator!=(const Vector2d &a) const
00964 {
00965 if ( a.x != x || a.y != y ) return true;
00966 return false;
00967 };
00968
00969 Vector2d operator+(Vector2d a) const
00970 {
00971 a.x+=x;
00972 a.y+=y;
00973 return a;
00974 };
00975
00976 Vector2d operator-(Vector2d a) const
00977 {
00978 a.x = x-a.x;
00979 a.y = y-a.y;
00980 return a;
00981 };
00982
00983 Vector2d operator - (void) const
00984 {
00985 return negative();
00986 };
00987
00988 Vector2d operator*(Vector2d a) const
00989 {
00990 a.x*=x;
00991 a.y*=y;
00992 return a;
00993 };
00994
00995 Vector2d operator*(Type c) const
00996 {
00997 Vector2d<Type> a;
00998
00999 a.x = x * c;
01000 a.y = y * c;
01001
01002 return a;
01003 };
01004
01005 Vector2d operator/(Vector2d a) const
01006 {
01007 a.x = x/a.x;
01008 a.y = y/a.y;
01009 return a;
01010 };
01011
01012
01013 Type Dot(const Vector2d<Type> &a) const
01014 {
01015 return (x * a.x + y * a.y );
01016 };
01017
01018 Type GetX(void) const { return x; };
01019 Type GetY(void) const { return y; };
01020
01021 void SetX(Type t) { x = t; };
01022 void SetY(Type t) { y = t; };
01023
01024 void Set(Type a,Type b)
01025 {
01026 x = a;
01027 y = b;
01028 };
01029
01030 void Zero(void)
01031 {
01032 x = 0;
01033 y = 0;
01034 };
01035
01036 Vector2d negative(void) const
01037 {
01038 Vector2d result;
01039 result.x = -x;
01040 result.y = -y;
01041 return result;
01042 }
01043
01044 Type magnitude(void) const
01045 {
01046 return (Type) sqrtf(x * x + y * y );
01047 }
01048
01049 Type fastmagnitude(void) const
01050 {
01051 return (Type) sqrt(x * x + y * y );
01052 }
01053
01054 Type fastermagnitude(void) const
01055 {
01056 return (Type) sqrt( x * x + y * y );
01057 }
01058
01059 void Reflection(Vector2d &a,Vector2d &b);
01060
01061 Type Length(void) const
01062 {
01063 return Type(sqrtf( x*x + y*y ));
01064 };
01065
01066 Type FastLength(void) const
01067 {
01068 return Type(sqrt( x*x + y*y ));
01069 };
01070
01071 Type FasterLength(void) const
01072 {
01073 return Type(sqrt( x*x + y*y ));
01074 };
01075
01076 Type Length2(void)
01077 {
01078 return x*x+y*y;
01079 }
01080
01081 Type Distance(const Vector2d &a) const
01082 {
01083 Type dx = a.x - x;
01084 Type dy = a.y - y;
01085 Type d = dx*dx+dy*dy;
01086 return sqrtf(d);
01087 };
01088
01089 Type FastDistance(const Vector2d &a) const
01090 {
01091 Type dx = a.x - x;
01092 Type dy = a.y - y;
01093 Type d = dx*dx+dy*dy;
01094 return sqrt(d);
01095 };
01096
01097 Type FasterDistance(const Vector2d &a) const
01098 {
01099 Type dx = a.x - x;
01100 Type dy = a.y - y;
01101 Type d = dx*dx+dy*dy;
01102 return sqrt(d);
01103 };
01104
01105 Type Distance2(Vector2d &a)
01106 {
01107 Type dx = a.x - x;
01108 Type dy = a.y - y;
01109 return dx*dx + dy *dy;
01110 };
01111
01112 void Lerp(const Vector2d<Type>& from,const Vector2d<Type>& to,double slerp)
01113 {
01114 x = ((to.x - from.x)*slerp) + from.x;
01115 y = ((to.y - from.y)*slerp) + from.y;
01116 };
01117
01118
01119 void Cross(const Vector2d<Type> &a,const Vector2d<Type> &b)
01120 {
01121 x = a.y*b.x - a.x*b.y;
01122 y = a.x*b.x - a.x*b.x;
01123 };
01124
01125 Type Normalize(void)
01126 {
01127 Type l = Length();
01128 if ( l != 0 )
01129 {
01130 l = Type( 1 ) / l;
01131 x*=l;
01132 y*=l;
01133 }
01134 else
01135 {
01136 x = y = 0;
01137 }
01138 return l;
01139 };
01140
01141 Type FastNormalize(void)
01142 {
01143 Type l = FastLength();
01144 if ( l != 0 )
01145 {
01146 l = Type( 1 ) / l;
01147 x*=l;
01148 y*=l;
01149 }
01150 else
01151 {
01152 x = y = 0;
01153 }
01154 return l;
01155 };
01156
01157 Type FasterNormalize(void)
01158 {
01159 Type l = FasterLength();
01160 if ( l != 0 )
01161 {
01162 l = Type( 1 ) / l;
01163 x*=l;
01164 y*=l;
01165 }
01166 else
01167 {
01168 x = y = 0;
01169 }
01170 return l;
01171 };
01172
01173
01174 Type x;
01175 Type y;
01176 };
01177
01178 class Line
01179 {
01180 public:
01181 Line(const Vector3d<double> &from,const Vector3d<double> &to)
01182 {
01183 mP1 = from;
01184 mP2 = to;
01185 };
01186
01187
01188 bool Intersect(const Line& src,Vector3d<double> §);
01189 private:
01190 Vector3d<double> mP1;
01191 Vector3d<double> mP2;
01192
01193 };
01194
01195
01196 typedef std::vector< Vector3d<double> > Vector3dVector;
01197 typedef std::vector< Vector2d<double> > Vector2dVector;
01198
01199 template <class Type> Vector3d<Type> operator * (Type s, const Vector3d<Type> &v )
01200 { Vector3d <Type> Scaled(v.x*s, v.y*s, v.z*s);
01201 return(Scaled); };
01202
01203 template <class Type> Vector2d<Type> operator * (Type s, const Vector2d<Type> &v )
01204 { Vector2d <Type> Scaled(v.x*s, v.y*s);
01205 return(Scaled); };
01206
01207 };
01208
01209 #endif