cd_vector.h
Go to the documentation of this file.
00001 #ifndef CD_VECTOR_H
00002 
00003 #define CD_VECTOR_H
00004 
00061 // http://codesuppository.blogspot.com
00062 //
00063 // mailto: jratcliff@infiniplex.net
00064 //
00065 // http://www.amillionpixels.us
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) { };  // null constructor, does not inialize point.
00086 
00087         Vector3d(const Vector3d &a) // constructor copies existing vector.
00088         {
00089                 x = a.x;
00090                 y = a.y;
00091                 z = a.z;
00092         };
00093 
00094         Vector3d(Type a,Type b,Type c) // construct with initial point.
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 // Operators
00126                 Vector3d& operator = (const Vector3d& A)          // ASSIGNMENT (=)
00127                         { x=A.x; y=A.y; z=A.z;
00128                                 return(*this);  };
00129 
00130                 Vector3d operator + (const Vector3d& A) const     // ADDITION (+)
00131                         { Vector3d Sum(x+A.x, y+A.y, z+A.z);
00132                                 return(Sum); };
00133 
00134                 Vector3d operator - (const Vector3d& A) const     // SUBTRACTION (-)
00135                         { Vector3d Diff(x-A.x, y-A.y, z-A.z);
00136                                 return(Diff); };
00137 
00138                 Vector3d operator * (const double s) const       // MULTIPLY BY SCALAR (*)
00139                         { Vector3d Scaled(x*s, y*s, z*s);
00140                                 return(Scaled); };
00141 
00142 
00143                 Vector3d operator + (const double s) const       // ADD CONSTANT TO ALL 3 COMPONENTS (*)
00144                         { Vector3d Scaled(x+s, y+s, z+s);
00145                                 return(Scaled); };
00146 
00147 
00148 
00149 
00150                 Vector3d operator / (const double s) const       // DIVIDE BY SCALAR (/)
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)             // ACCUMULATED VECTOR ADDITION (/=)
00158                         { x/=A; y/=A; z/=A; };
00159 
00160                 void operator += (const Vector3d A)             // ACCUMULATED VECTOR ADDITION (+=)
00161                         { x+=A.x; y+=A.y; z+=A.z; };
00162                 void operator -= (const Vector3d A)             // ACCUMULATED VECTOR SUBTRACTION (+=)
00163                         { x-=A.x; y-=A.y; z-=A.z; };
00164                 void operator *= (const double s)        // ACCUMULATED SCALAR MULTIPLICATION (*=) (bpc 4/24/2000)
00165                         {x*=s; y*=s; z*=s;}
00166 
00167                 void operator += (const double A)             // ACCUMULATED VECTOR ADDITION (+=)
00168                         { x+=A; y+=A; z+=A; };
00169 
00170 
00171                 Vector3d operator - (void) const                // NEGATION (-)
00172                         { Vector3d Negated(-x, -y, -z);
00173                                 return(Negated); };
00174 
00175                 Type operator [] (const int i) const         // ALLOWS VECTOR ACCESS AS AN ARRAY.
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         // accessor methods.
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 // return -(*this).
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         // Highly specialized interpolate routine.  Will compute the interpolated position
00328         // shifted forward or backwards along the ray defined between (from) and (to).
00329         // Reason for existance is so that when a bullet collides with a wall, for
00330         // example, you can generate a graphic effect slightly *before* it hit the
00331         // wall so that the effect doesn't sort into the wall itself.
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; // normalize vector
00342                 d+=offset; // shift along ray
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)// compute reflection vector.
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          // length of vector.
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          // length of vector.
00438         {
00439                 return Type(sqrt( x*x + y*y + z*z ));
00440         };
00441         
00442 
00443         Type FasterLength(void) const          // length of vector.
00444         {
00445                 return Type(sqrt( x*x + y*y + z*z ));
00446         };
00447 
00448         Type Length2(void) const         // squared distance, prior to square root.
00449         {
00450                 Type l2 = x*x+y*y+z*z;
00451                 return l2;
00452         };
00453 
00454         Type Distance(const Vector3d<Type> &a) const   // distance between two points.
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   // distance between two points.
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   // distance between two points.
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  // squared distance.
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)       // normalize to a unit vector, returns distance.
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)       // normalize to a unit vector, returns distance.
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)       // normalize to a unit vector, returns distance.
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        // computes dot product.
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)  // cross two vectors result in this one.
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         // Check if next edge (b to c) turns inward
00578         //
00579         //    Edge from a to b is already in face
00580         //    Edge from b to c is being considered for addition to face
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                 // Check magnitude of cross product, which is a sine function
00605                 // i.e., mag (a x b) = mag (a) * mag (b) * sin (theta);
00606                 // If sin (theta) small, then angle between edges is very close to
00607                 // 180, which we may want to call a concavity.  Setting the
00608                 // CONCAVITY_TOLERANCE value greater than about 0.01 MAY cause
00609                 // face consolidation to get stuck on particular face.  Most meshes
00610                 // convert properly with a value of 0.0
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                 // Dot product of tri normal with cross product result will
00621                 // yield positive number if edges are convex (+1.0 if two tris
00622                 // are coplanar), negative number if edges are concave (-1.0 if
00623                 // two tris are coplanar.)
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         // test to see if this point is inside the triangle specified by
00641         // these three points on the X/Y plane.
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         // test to see if this point is inside the triangle specified by
00667         // these three points on the X/Y plane.
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         // test to see if this point is inside the triangle specified by
00694         // these three points on the X/Y plane.
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         // Given a point and a line (defined by two points), compute the closest point
00720         // in the line.  (The line is treated as infinitely long.)
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                 // Handle degenerate lines
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         // Given a point and a line segment (defined by two points), compute the closest point
00741         // in the line.  Cap the point at the endpoints of the line segment.
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                 // Handle degenerate lines
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                         // Clamp the point to conform to the segment's endpoints
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         // Given a point and a plane (defined by three points), compute the closest point
00769         // in the plane.  (The plane is unbounded.)
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                 // Get the normal of the polygon (doesn't have to be a unit vector)
00782                 normal.Cross(lineDelta0, lineDelta1);
00783 
00784                 double delta = normal.Dot(pointDelta) / normal.Dot(normal);
00785                 nearestPoint = point - delta*normal;
00786         }
00787 
00788         // Given a point and a plane (defined by a coplanar point and a normal), compute the closest point
00789         // in the plane.  (The plane is unbounded.)
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         // Given a point and a triangle (defined by three points), compute the closest point
00802         // in the triangle.  Clamp the point so it's confined to the area of the triangle.
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                 // Handle degenerate triangles
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                         // If bForce is true at this point, it means the nearest point lies
00877                         // inside the triangle; use the nearest-point-on-a-plane equation
00878                         if ( bForce )
00879                         {
00880                                 Vector3d<Type> normal;
00881 
00882                                 // Get the normal of the polygon (doesn't have to be a unit vector)
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 //private:
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) { };  // null constructor, does not inialize point.
00906 
00907         Vector2d(const Vector2d &a) // constructor copies existing vector.
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) // construct with initial point.
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) // += operator.
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        // computes dot product.
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); // compute reflection vector.
01060 
01061         Type Length(void) const          // length of vector.
01062         {
01063                 return Type(sqrtf( x*x + y*y ));
01064         };
01065 
01066         Type FastLength(void) const          // length of vector.
01067         {
01068                 return Type(sqrt( x*x + y*y ));
01069         };
01070 
01071         Type FasterLength(void) const          // length of vector.
01072         {
01073                 return Type(sqrt( x*x + y*y ));
01074         };
01075 
01076         Type Length2(void)        // squared distance, prior to square root.
01077         {
01078                 return x*x+y*y;
01079         }
01080 
01081         Type Distance(const Vector2d &a) const   // distance between two points.
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   // distance between two points.
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   // distance between two points.
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) // squared distance.
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)  // cross two vectors result in this one.
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)       // normalize to a unit vector, returns distance.
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)       // normalize to a unit vector, returns distance.
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)       // normalize to a unit vector, returns distance.
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         // JWR  Test for the intersection of two lines.
01187 
01188         bool Intersect(const Line& src,Vector3d<double> &sect);
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


convex_decomposition
Author(s): John W. Ratcliff
autogenerated on Sat Jun 8 2019 20:01:17