Extras/ConvexDecomposition/cd_vector.h

   1 #ifndef CD_VECTOR_H
   2
   3 #define CD_VECTOR_H
   4
   5 /*----------------------------------------------------------------------
   6                 Copyright (c) 2004 Open Dynamics Framework Group
   7                                         www.physicstools.org
   8                 All rights reserved.
   9
  10                 Redistribution and use in source and binary forms, with or without modification, are permitted provided
  11                 that the following conditions are met:
  12
  13                 Redistributions of source code must retain the above copyright notice, this list of conditions
  14                 and the following disclaimer.
  15
  16                 Redistributions in binary form must reproduce the above copyright notice,
  17                 this list of conditions and the following disclaimer in the documentation
  18                 and/or other materials provided with the distribution.
  19
  20                 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
  21                 be used to endorse or promote products derived from this software without specific prior written permission.
  22
  23                 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
  24                 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  25                 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  26                 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  27                 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
  28                 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  29                 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  30 -----------------------------------------------------------------------*/
  31
  32 // http://codesuppository.blogspot.com
  33 //
  34 // mailto: jratcliff@infiniplex.net
  35 //
  36 // http://www.amillionpixels.us
  37 //
  38
  39
  40 #pragma warning(disable:4786)
  41
  42 #include <math.h>
  43 #include <float.h>
  44 #include <vector>
  45
  46 namespace ConvexDecomposition
  47 {
  48
  49
  50 const float DEG_TO_RAD = ((2.0f * 3.14152654f) / 360.0f);
  51 const float RAD_TO_DEG = (360.0f / (2.0f * 3.141592654f));
  52
  53 class Vector3d
  54 {
  55 public:
  56         Vector3d(void) { };  // null constructor, does not inialize point.
  57
  58         Vector3d(const Vector3d &a) // constructor copies existing vector.
  59         {
  60                 x = a.x;
  61                 y = a.y;
  62                 z = a.z;
  63         };
  64
  65         Vector3d(float a,float b,float c) // construct with initial point.
  66         {
  67                 x = a;
  68                 y = b;
  69                 z = c;
  70         };
  71
  72         Vector3d(const float *t)
  73         {
  74                 x = t[0];
  75                 y = t[1];
  76                 z = t[2];
  77         };
  78
  79         Vector3d(const int *t)
  80         {
  81                 x = t[0];
  82                 y = t[1];
  83                 z = t[2];
  84         };
  85
  86         bool operator==(const Vector3d &a) const
  87         {
  88                 return( a.x == x && a.y == y && a.z == z );
  89         };
  90
  91         bool operator!=(const Vector3d &a) const
  92         {
  93                 return( a.x != x || a.y != y || a.z != z );
  94         };
  95
  96 // Operators
  97                 Vector3d& operator = (const Vector3d& A)          // ASSIGNMENT (=)
  98                         { x=A.x; y=A.y; z=A.z;
  99                                 return(*this);  };
 100
 101                 Vector3d operator + (const Vector3d& A) const     // ADDITION (+)
 102                         { Vector3d Sum(x+A.x, y+A.y, z+A.z);
 103                                 return(Sum); };
 104
 105                 Vector3d operator - (const Vector3d& A) const     // SUBTRACTION (-)
 106                         { Vector3d Diff(x-A.x, y-A.y, z-A.z);
 107                                 return(Diff); };
 108
 109                 Vector3d operator * (const float s) const       // MULTIPLY BY SCALAR (*)
 110                         { Vector3d Scaled(x*s, y*s, z*s);
 111                                 return(Scaled); };
 112
 113
 114                 Vector3d operator + (const float s) const       // ADD CONSTANT TO ALL 3 COMPONENTS (*)
 115                         { Vector3d Scaled(x+s, y+s, z+s);
 116                                 return(Scaled); };
 117
 118
 119
 120
 121                 Vector3d operator / (const float s) const       // DIVIDE BY SCALAR (/)
 122                 {
 123                         float r = 1.0f / s;
 124                                 Vector3d Scaled(x*r, y*r, z*r);
 125                                 return(Scaled);
 126                 };
 127
 128                 void operator /= (float A)             // ACCUMULATED VECTOR ADDITION (/=)
 129                         { x/=A; y/=A; z/=A; };
 130
 131                 void operator += (const Vector3d A)             // ACCUMULATED VECTOR ADDITION (+=)
 132                         { x+=A.x; y+=A.y; z+=A.z; };
 133                 void operator -= (const Vector3d A)             // ACCUMULATED VECTOR SUBTRACTION (+=)
 134                         { x-=A.x; y-=A.y; z-=A.z; };
 135                 void operator *= (const float s)        // ACCUMULATED SCALAR MULTIPLICATION (*=) (bpc 4/24/2000)
 136                         {x*=s; y*=s; z*=s;}
 137
 138                 void operator += (const float A)             // ACCUMULATED VECTOR ADDITION (+=)
 139                         { x+=A; y+=A; z+=A; };
 140
 141
 142                 Vector3d operator - (void) const                // NEGATION (-)
 143                         { Vector3d Negated(-x, -y, -z);
 144                                 return(Negated); };
 145
 146                 float operator [] (const int i) const         // ALLOWS VECTOR ACCESS AS AN ARRAY.
 147                         { return( (i==0)?x:((i==1)?y:z) ); };
 148                 float & operator [] (const int i)
 149                         { return( (i==0)?x:((i==1)?y:z) ); };
 150 //
 151
 152         // accessor methods.
 153         float GetX(void) const { return x; };
 154         float GetY(void) const { return y; };
 155         float GetZ(void) const { return z; };
 156
 157         float X(void) const { return x; };
 158         float Y(void) const { return y; };
 159         float Z(void) const { return z; };
 160
 161         void SetX(float t)   { x   = t; };
 162         void SetY(float t)   { y   = t; };
 163         void SetZ(float t)   { z   = t; };
 164
 165         bool IsSame(const Vector3d &v,float epsilon) const
 166         {
 167                 float dx = fabsf( x - v.x );
 168                 if ( dx > epsilon ) return false;
 169                 float dy = fabsf( y - v.y );
 170                 if ( dy > epsilon ) return false;
 171                 float dz = fabsf( z - v.z );
 172                 if ( dz > epsilon ) return false;
 173                 return true;
 174         }
 175
 176
 177         float ComputeNormal(const Vector3d &A,
 178                                                                                  const Vector3d &B,
 179                                                                                  const Vector3d &C)
 180         {
 181                 float vx,vy,vz,wx,wy,wz,vw_x,vw_y,vw_z,mag;
 182
 183                 vx = (B.x - C.x);
 184                 vy = (B.y - C.y);
 185                 vz = (B.z - C.z);
 186
 187                 wx = (A.x - B.x);
 188                 wy = (A.y - B.y);
 189                 wz = (A.z - B.z);
 190
 191                 vw_x = vy * wz - vz * wy;
 192                 vw_y = vz * wx - vx * wz;
 193                 vw_z = vx * wy - vy * wx;
 194
 195                 mag = sqrtf((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
 196
 197                 if ( mag < 0.000001f )
 198                 {
 199                         mag = 0;
 200                 }
 201                 else
 202                 {
 203                         mag = 1.0f/mag;
 204                 }
 205
 206                 x = vw_x * mag;
 207                 y = vw_y * mag;
 208                 z = vw_z * mag;
 209
 210                 return mag;
 211         }
 212
 213
 214         void ScaleSumScale(float c0,float c1,const Vector3d &pos)
 215         {
 216                 x = (x*c0) + (pos.x*c1);
 217                 y = (y*c0) + (pos.y*c1);
 218                 z = (z*c0) + (pos.z*c1);
 219         }
 220
 221         void SwapYZ(void)
 222         {
 223                 float t = y;
 224                 y = z;
 225                 z = t;
 226         };
 227
 228         void Get(float *v) const
 229         {
 230                 v[0] = x;
 231                 v[1] = y;
 232                 v[2] = z;
 233         };
 234
 235         void Set(const int *p)
 236         {
 237                 x = (float) p[0];
 238                 y = (float) p[1];
 239                 z = (float) p[2];
 240         }
 241
 242         void Set(const float *p)
 243         {
 244                 x = (float) p[0];
 245                 y = (float) p[1];
 246                 z = (float) p[2];
 247         }
 248
 249
 250         void Set(float a,float b,float c)
 251         {
 252                 x = a;
 253                 y = b;
 254                 z = c;
 255         };
 256
 257         void Zero(void)
 258         {
 259                 x = y = z = 0;
 260         };
 261
 262         const float* Ptr() const { return &x; }
 263         float* Ptr() { return &x; }
 264
 265
 266 // return -(*this).
 267         Vector3d negative(void) const
 268         {
 269                 Vector3d result;
 270                 result.x = -x;
 271                 result.y = -y;
 272                 result.z = -z;
 273                 return result;
 274         }
 275
 276         float Magnitude(void) const
 277         {
 278                 return float(sqrt(x * x + y * y + z * z));
 279         };
 280
 281         float FastMagnitude(void) const
 282         {
 283                 return float(sqrtf(x * x + y * y + z * z));
 284         };
 285
 286         float FasterMagnitude(void) const
 287         {
 288                 return float(sqrtf(x * x + y * y + z * z));
 289         };
 290
 291         void Lerp(const Vector3d& from,const Vector3d& to,float slerp)
 292         {
 293                 x = ((to.x - from.x) * slerp) + from.x;
 294                 y = ((to.y - from.y) * slerp) + from.y;
 295                 z = ((to.z - from.z) * slerp) + from.z;
 296         };
 297
 298         // Highly specialized interpolate routine.  Will compute the interpolated position
 299         // shifted forward or backwards along the ray defined between (from) and (to).
 300         // Reason for existance is so that when a bullet collides with a wall, for
 301         // example, you can generate a graphic effect slightly *before* it hit the
 302         // wall so that the effect doesn't sort into the wall itself.
 303         void Interpolate(const Vector3d &from,const Vector3d &to,float offset)
 304         {
 305                 x = to.x-from.x;
 306                 y = to.y-from.y;
 307                 z = to.z-from.z;
 308                 float d = sqrtf( x*x + y*y + z*z );
 309                 float recip = 1.0f / d;
 310                 x*=recip;
 311                 y*=recip;
 312                 z*=recip; // normalize vector
 313                 d+=offset; // shift along ray
 314                 x = x*d + from.x;
 315                 y = y*d + from.y;
 316                 z = z*d + from.z;
 317         };
 318
 319         bool BinaryEqual(const Vector3d &p) const
 320         {
 321                 const int *source = (const int *) &x;
 322                 const int *dest   = (const int *) &p.x;
 323
 324                 if ( source[0] == dest[0] &&
 325                                  source[1] == dest[1] &&
 326                                  source[2] == dest[2] ) return true;
 327
 328                 return false;
 329         };
 330
 331         /*bool BinaryEqual(const Vector3d<int> &p) const
 332         {
 333                 if ( x == p.x && y == p.y && z == p.z ) return true;
 334                 return false;
 335         }
 336         */
 337
 338
 339
 340 /** Computes the reflection vector between two vectors.*/
 341         void Reflection(const Vector3d &a,const Vector3d &b)// compute reflection vector.
 342         {
 343                 Vector3d c;
 344                 Vector3d d;
 345
 346                 float dot = a.Dot(b) * 2.0f;
 347
 348                 c = b * dot;
 349
 350                 d = c - a;
 351
 352                 x = -d.x;
 353                 y = -d.y;
 354                 z = -d.z;
 355         };
 356
 357         void AngleAxis(float angle,const Vector3d& axis)
 358         {
 359                 x = axis.x*angle;
 360                 y = axis.y*angle;
 361                 z = axis.z*angle;
 362         };
 363
 364         float Length(void) const          // length of vector.
 365         {
 366                 return float(sqrt( x*x + y*y + z*z ));
 367         };
 368
 369
 370         float ComputePlane(const Vector3d &A,
 371                                                                                  const Vector3d &B,
 372                                                                                  const Vector3d &C)
 373         {
 374                 float vx,vy,vz,wx,wy,wz,vw_x,vw_y,vw_z,mag;
 375
 376                 vx = (B.x - C.x);
 377                 vy = (B.y - C.y);
 378                 vz = (B.z - C.z);
 379
 380                 wx = (A.x - B.x);
 381                 wy = (A.y - B.y);
 382                 wz = (A.z - B.z);
 383
 384                 vw_x = vy * wz - vz * wy;
 385                 vw_y = vz * wx - vx * wz;
 386                 vw_z = vx * wy - vy * wx;
 387
 388                 mag = sqrtf((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
 389
 390                 if ( mag < 0.000001f )
 391                 {
 392                         mag = 0;
 393                 }
 394                 else
 395                 {
 396                         mag = 1.0f/mag;
 397                 }
 398
 399                 x = vw_x * mag;
 400                 y = vw_y * mag;
 401                 z = vw_z * mag;
 402
 403
 404                 float D = 0.0f - ((x*A.x)+(y*A.y)+(z*A.z));
 405
 406                 return D;
 407         }
 408
 409
 410         float FastLength(void) const          // length of vector.
 411         {
 412                 return float(sqrtf( x*x + y*y + z*z ));
 413         };
 414
 415
 416         float FasterLength(void) const          // length of vector.
 417         {
 418                 return float(sqrtf( x*x + y*y + z*z ));
 419         };
 420
 421         float Length2(void) const         // squared distance, prior to square root.
 422         {
 423                 float l2 = x*x+y*y+z*z;
 424                 return l2;
 425         };
 426
 427         float Distance(const Vector3d &a) const   // distance between two points.
 428         {
 429                 Vector3d d(a.x-x,a.y-y,a.z-z);
 430                 return d.Length();
 431         }
 432
 433         float FastDistance(const Vector3d &a) const   // distance between two points.
 434         {
 435                 Vector3d d(a.x-x,a.y-y,a.z-z);
 436                 return d.FastLength();
 437         }
 438
 439         float FasterDistance(const Vector3d &a) const   // distance between two points.
 440         {
 441                 Vector3d d(a.x-x,a.y-y,a.z-z);
 442                 return d.FasterLength();
 443         }
 444
 445
 446         float DistanceXY(const Vector3d &a) const
 447         {
 448         float dx = a.x - x;
 449         float dy = a.y - y;
 450                 float dist = dx*dx + dy*dy;
 451         return dist;
 452         }
 453
 454         float Distance2(const Vector3d &a) const  // squared distance.
 455         {
 456                 float dx = a.x - x;
 457                 float dy = a.y - y;
 458                 float dz = a.z - z;
 459                 return dx*dx + dy*dy + dz*dz;
 460         };
 461
 462         float Partial(const Vector3d &p) const
 463         {
 464                 return (x*p.y) - (p.x*y);
 465         }
 466
 467         float Area(const Vector3d &p1,const Vector3d &p2) const
 468         {
 469                 float A = Partial(p1);
 470                 A+= p1.Partial(p2);
 471                 A+= p2.Partial(*this);
 472                 return A*0.5f;
 473         }
 474
 475         inline float Normalize(void)       // normalize to a unit vector, returns distance.
 476         {
 477                 float d = sqrtf( static_cast< float >( x*x + y*y + z*z ) );
 478                 if ( d > 0 )
 479                 {
 480                         float r = 1.0f / d;
 481                         x *= r;
 482                         y *= r;
 483                         z *= r;
 484                 }
 485                 else
 486                 {
 487                         x = y = z = 1;
 488                 }
 489                 return d;
 490         };
 491
 492         inline float FastNormalize(void)       // normalize to a unit vector, returns distance.
 493         {
 494                 float d = sqrt( static_cast< float >( x*x + y*y + z*z ) );
 495                 if ( d > 0 )
 496                 {
 497                         float r = 1.0f / d;
 498                         x *= r;
 499                         y *= r;
 500                         z *= r;
 501                 }
 502                 else
 503                 {
 504                         x = y = z = 1;
 505                 }
 506                 return d;
 507         };
 508
 509         inline float FasterNormalize(void)       // normalize to a unit vector, returns distance.
 510         {
 511                 float d = sqrtf( static_cast< float >( x*x + y*y + z*z ) );
 512                 if ( d > 0 )
 513                 {
 514                         float r = 1.0f / d;
 515                         x *= r;
 516                         y *= r;
 517                         z *= r;
 518                 }
 519                 else
 520                 {
 521                         x = y = z = 1;
 522                 }
 523                 return d;
 524         };
 525
 526
 527
 528
 529         float Dot(const Vector3d &a) const        // computes dot product.
 530         {
 531                 return (x * a.x + y * a.y + z * a.z );
 532         };
 533
 534
 535         Vector3d Cross( const Vector3d& other ) const
 536         {
 537                 Vector3d result( y*other.z - z*other.y,  z*other.x - x*other.z,  x*other.y - y*other.x );
 538
 539                 return result;
 540         }
 541
 542         void Cross(const Vector3d &a,const Vector3d &b)  // cross two vectors result in this one.
 543         {
 544                 x = a.y*b.z - a.z*b.y;
 545                 y = a.z*b.x - a.x*b.z;
 546                 z = a.x*b.y - a.y*b.x;
 547         };
 548
 549         /******************************************/
 550         // Check if next edge (b to c) turns inward
 551         //
 552         //    Edge from a to b is already in face
 553         //    Edge from b to c is being considered for addition to face
 554         /******************************************/
 555         bool Concave(const Vector3d& a,const Vector3d& b)
 556         {
 557                 float vx,vy,vz,wx,wy,wz,vw_x,vw_y,vw_z,mag,nx,ny,nz,mag_a,mag_b;
 558
 559                 wx = b.x - a.x;
 560                 wy = b.y - a.y;
 561                 wz = b.z - a.z;
 562
 563                 mag_a = (float) sqrtf((wx * wx) + (wy * wy) + (wz * wz));
 564
 565                 vx = x - b.x;
 566                 vy = y - b.y;
 567                 vz = z - b.z;
 568
 569                 mag_b = (float) sqrtf((vx * vx) + (vy * vy) + (vz * vz));
 570
 571                 vw_x = (vy * wz) - (vz * wy);
 572                 vw_y = (vz * wx) - (vx * wz);
 573                 vw_z = (vx * wy) - (vy * wx);
 574
 575                 mag = (float) sqrtf((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
 576
 577                 // Check magnitude of cross product, which is a sine function
 578                 // i.e., mag (a x b) = mag (a) * mag (b) * sin (theta);
 579                 // If sin (theta) small, then angle between edges is very close to
 580                 // 180, which we may want to call a concavity.  Setting the
 581                 // CONCAVITY_TOLERANCE value greater than about 0.01 MAY cause
 582                 // face consolidation to get stuck on particular face.  Most meshes
 583                 // convert properly with a value of 0.0
 584
 585                 if (mag/(mag_a*mag_b) <= 0.0f ) return true;
 586
 587                 mag = 1.0f / mag;
 588
 589                 nx = vw_x * mag;
 590                 ny = vw_y * mag;
 591                 nz = vw_z * mag;
 592
 593                 // Dot product of tri normal with cross product result will
 594                 // yield positive number if edges are convex (+1.0 if two tris
 595                 // are coplanar), negative number if edges are concave (-1.0 if
 596                 // two tris are coplanar.)
 597
 598                 mag = ( x * nx) + ( y * ny) + ( z * nz);
 599
 600                 if (mag > 0.0f ) return false;
 601
 602                 return(true);
 603         };
 604
 605         bool PointTestXY(const Vector3d &i,const Vector3d &j) const
 606         {
 607                 if (((( i.y <= y ) && ( y  < j.y )) ||
 608                                  (( j.y <= y ) && ( y  < i.y ))) &&
 609                                         ( x < (j.x - i.x) * (y - i.y) / (j.y - i.y) + i.x)) return true;
 610                 return false;
 611         }
 612
 613         // test to see if this point is inside the triangle specified by
 614         // these three points on the X/Y plane.
 615         bool PointInTriXY(const Vector3d &p1,
 616                                                                         const Vector3d &p2,
 617                                                                         const Vector3d &p3) const
 618         {
 619                 float ax  = p3.x - p2.x;
 620                 float ay  = p3.y - p2.y;
 621                 float bx  = p1.x - p3.x;
 622                 float by  = p1.y - p3.y;
 623                 float cx  = p2.x - p1.x;
 624                 float cy  = p2.y - p1.y;
 625                 float apx = x - p1.x;
 626                 float apy = y - p1.y;
 627                 float bpx = x - p2.x;
 628                 float bpy = y - p2.y;
 629                 float cpx = x - p3.x;
 630                 float cpy = y - p3.y;
 631
 632                 float aCROSSbp = ax*bpy - ay*bpx;
 633                 float cCROSSap = cx*apy - cy*apx;
 634                 float bCROSScp = bx*cpy - by*cpx;
 635
 636                 return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
 637         };
 638
 639         // test to see if this point is inside the triangle specified by
 640         // these three points on the X/Y plane.
 641         bool PointInTriYZ(const Vector3d &p1,
 642                                                                         const Vector3d &p2,
 643                                                                         const Vector3d &p3) const
 644         {
 645                 float ay  = p3.y - p2.y;
 646                 float az  = p3.z - p2.z;
 647                 float by  = p1.y - p3.y;
 648                 float bz  = p1.z - p3.z;
 649                 float cy  = p2.y - p1.y;
 650                 float cz  = p2.z - p1.z;
 651                 float apy = y - p1.y;
 652                 float apz = z - p1.z;
 653                 float bpy = y - p2.y;
 654                 float bpz = z - p2.z;
 655                 float cpy = y - p3.y;
 656                 float cpz = z - p3.z;
 657
 658                 float aCROSSbp = ay*bpz - az*bpy;
 659                 float cCROSSap = cy*apz - cz*apy;
 660                 float bCROSScp = by*cpz - bz*cpy;
 661
 662                 return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
 663         };
 664
 665
 666         // test to see if this point is inside the triangle specified by
 667         // these three points on the X/Y plane.
 668         bool PointInTriXZ(const Vector3d &p1,
 669                                                                         const Vector3d &p2,
 670                                                                         const Vector3d &p3) const
 671         {
 672                 float az  = p3.z - p2.z;
 673                 float ax  = p3.x - p2.x;
 674                 float bz  = p1.z - p3.z;
 675                 float bx  = p1.x - p3.x;
 676                 float cz  = p2.z - p1.z;
 677                 float cx  = p2.x - p1.x;
 678                 float apz = z - p1.z;
 679                 float apx = x - p1.x;
 680                 float bpz = z - p2.z;
 681                 float bpx = x - p2.x;
 682                 float cpz = z - p3.z;
 683                 float cpx = x - p3.x;
 684
 685                 float aCROSSbp = az*bpx - ax*bpz;
 686                 float cCROSSap = cz*apx - cx*apz;
 687                 float bCROSScp = bz*cpx - bx*cpz;
 688
 689                 return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
 690         };
 691
 692         // Given a point and a line (defined by two points), compute the closest point
 693         // in the line.  (The line is treated as infinitely long.)
 694         void NearestPointInLine(const Vector3d &point,
 695                                                                                                         const Vector3d &line0,
 696                                                                                                         const Vector3d &line1)
 697         {
 698                 Vector3d &nearestPoint = *this;
 699                 Vector3d lineDelta     = line1 - line0;
 700
 701                 // Handle degenerate lines
 702                 if ( lineDelta == Vector3d(0, 0, 0) )
 703                 {
 704                         nearestPoint = line0;
 705                 }
 706                 else
 707                 {
 708                         float delta = (point-line0).Dot(lineDelta) / (lineDelta).Dot(lineDelta);
 709                         nearestPoint = line0 + lineDelta*delta;
 710                 }
 711         }
 712
 713         // Given a point and a line segment (defined by two points), compute the closest point
 714         // in the line.  Cap the point at the endpoints of the line segment.
 715         void NearestPointInLineSegment(const Vector3d &point,
 716                                                                                                                                  const Vector3d &line0,
 717                                                                                                                                  const Vector3d &line1)
 718         {
 719                 Vector3d &nearestPoint = *this;
 720                 Vector3d lineDelta     = line1 - line0;
 721
 722                 // Handle degenerate lines
 723                 if ( lineDelta == Vector3d(0, 0, 0) )
 724                 {
 725                         nearestPoint = line0;
 726                 }
 727                 else
 728                 {
 729                         float delta = (point-line0).Dot(lineDelta) / (lineDelta).Dot(lineDelta);
 730
 731                         // Clamp the point to conform to the segment's endpoints
 732                         if ( delta < 0 )
 733                                 delta = 0;
 734                         else if ( delta > 1 )
 735                                 delta = 1;
 736
 737                         nearestPoint = line0 + lineDelta*delta;
 738                 }
 739         }
 740
 741         // Given a point and a plane (defined by three points), compute the closest point
 742         // in the plane.  (The plane is unbounded.)
 743         void NearestPointInPlane(const Vector3d &point,
 744                                                                                                          const Vector3d &triangle0,
 745                                                                                                          const Vector3d &triangle1,
 746                                                                                                          const Vector3d &triangle2)
 747         {
 748                 Vector3d &nearestPoint = *this;
 749                 Vector3d lineDelta0    = triangle1 - triangle0;
 750                 Vector3d lineDelta1    = triangle2 - triangle0;
 751                 Vector3d pointDelta    = point - triangle0;
 752                 Vector3d normal;
 753
 754                 // Get the normal of the polygon (doesn't have to be a unit vector)
 755                 normal.Cross(lineDelta0, lineDelta1);
 756
 757                 float delta = normal.Dot(pointDelta) / normal.Dot(normal);
 758                 nearestPoint = point - normal*delta;
 759         }
 760
 761         // Given a point and a plane (defined by a coplanar point and a normal), compute the closest point
 762         // in the plane.  (The plane is unbounded.)
 763         void NearestPointInPlane(const Vector3d &point,
 764                                                                                                          const Vector3d &planePoint,
 765                                                                                                          const Vector3d &planeNormal)
 766         {
 767                 Vector3d &nearestPoint = *this;
 768                 Vector3d pointDelta    = point - planePoint;
 769
 770                 float delta = planeNormal.Dot(pointDelta) / planeNormal.Dot(planeNormal);
 771                 nearestPoint = point - planeNormal*delta;
 772         }
 773
 774         // Given a point and a triangle (defined by three points), compute the closest point
 775         // in the triangle.  Clamp the point so it's confined to the area of the triangle.
 776         void NearestPointInTriangle(const Vector3d &point,
 777                                                                                                                         const Vector3d &triangle0,
 778                                                                                                                         const Vector3d &triangle1,
 779                                                                                                                         const Vector3d &triangle2)
 780         {
 781                 static const Vector3d zeroVector(0, 0, 0);
 782
 783                 Vector3d &nearestPoint = *this;
 784
 785                 Vector3d lineDelta0 = triangle1 - triangle0;
 786                 Vector3d lineDelta1 = triangle2 - triangle0;
 787
 788                 // Handle degenerate triangles
 789                 if ( (lineDelta0 == zeroVector) || (lineDelta1 == zeroVector) )
 790                 {
 791                         nearestPoint.NearestPointInLineSegment(point, triangle1, triangle2);
 792                 }
 793                 else if ( lineDelta0 == lineDelta1 )
 794                 {
 795                         nearestPoint.NearestPointInLineSegment(point, triangle0, triangle1);
 796                 }
 797
 798                 else
 799                 {
 800                         Vector3d axis[3];
 801                         axis[0].NearestPointInLine(triangle0, triangle1, triangle2);
 802                         axis[1].NearestPointInLine(triangle1, triangle0, triangle2);
 803                         axis[2].NearestPointInLine(triangle2, triangle0, triangle1);
 804
 805                         float axisDot[3];
 806                         axisDot[0] = (triangle0-axis[0]).Dot(point-axis[0]);
 807                         axisDot[1] = (triangle1-axis[1]).Dot(point-axis[1]);
 808                         axisDot[2] = (triangle2-axis[2]).Dot(point-axis[2]);
 809
 810                         bool            bForce         = true;
 811                         float            bestMagnitude2 = 0;
 812                         float            closeMagnitude2;
 813                         Vector3d closePoint;
 814
 815                         if ( axisDot[0] < 0 )
 816                         {
 817                                 closePoint.NearestPointInLineSegment(point, triangle1, triangle2);
 818                                 closeMagnitude2 = point.Distance2(closePoint);
 819                                 if ( bForce || (bestMagnitude2 > closeMagnitude2) )
 820                                 {
 821                                         bForce         = false;
 822                                         bestMagnitude2 = closeMagnitude2;
 823                                         nearestPoint   = closePoint;
 824                                 }
 825                         }
 826                         if ( axisDot[1] < 0 )
 827                         {
 828                                 closePoint.NearestPointInLineSegment(point, triangle0, triangle2);
 829                                 closeMagnitude2 = point.Distance2(closePoint);
 830                                 if ( bForce || (bestMagnitude2 > closeMagnitude2) )
 831                                 {
 832                                         bForce         = false;
 833                                         bestMagnitude2 = closeMagnitude2;
 834                                         nearestPoint   = closePoint;
 835                                 }
 836                         }
 837                         if ( axisDot[2] < 0 )
 838                         {
 839                                 closePoint.NearestPointInLineSegment(point, triangle0, triangle1);
 840                                 closeMagnitude2 = point.Distance2(closePoint);
 841                                 if ( bForce || (bestMagnitude2 > closeMagnitude2) )
 842                                 {
 843                                         bForce         = false;
 844                                         bestMagnitude2 = closeMagnitude2;
 845                                         nearestPoint   = closePoint;
 846                                 }
 847                         }
 848
 849                         // If bForce is true at this point, it means the nearest point lies
 850                         // inside the triangle; use the nearest-point-on-a-plane equation
 851                         if ( bForce )
 852                         {
 853                                 Vector3d normal;
 854
 855                                 // Get the normal of the polygon (doesn't have to be a unit vector)
 856                                 normal.Cross(lineDelta0, lineDelta1);
 857
 858                                 Vector3d pointDelta = point - triangle0;
 859                                 float delta = normal.Dot(pointDelta) / normal.Dot(normal);
 860
 861                                 nearestPoint = point - normal*delta;
 862                         }
 863                 }
 864         }
 865
 866
 867 //private:
 868
 869         float x;
 870         float y;
 871         float z;
 872 };
 873
 874
 875 class Vector2d
 876 {
 877 public:
 878         Vector2d(void) { };  // null constructor, does not inialize point.
 879
 880         Vector2d(const Vector2d &a) // constructor copies existing vector.
 881         {
 882                 x = a.x;
 883                 y = a.y;
 884         };
 885
 886         Vector2d(const float *t)
 887         {
 888                 x = t[0];
 889                 y = t[1];
 890         };
 891
 892
 893         Vector2d(float a,float b) // construct with initial point.
 894         {
 895                 x = a;
 896                 y = b;
 897         };
 898
 899         const float* Ptr() const { return &x; }
 900         float* Ptr() { return &x; }
 901
 902         Vector2d & operator+=(const Vector2d &a) // += operator.
 903         {
 904                 x+=a.x;
 905                 y+=a.y;
 906                 return *this;
 907         };
 908
 909         Vector2d & operator-=(const Vector2d &a)
 910         {
 911                 x-=a.x;
 912                 y-=a.y;
 913                 return *this;
 914         };
 915
 916         Vector2d & operator*=(const Vector2d &a)
 917         {
 918                 x*=a.x;
 919                 y*=a.y;
 920                 return *this;
 921         };
 922
 923         Vector2d & operator/=(const Vector2d &a)
 924         {
 925                 x/=a.x;
 926                 y/=a.y;
 927                 return *this;
 928         };
 929
 930         bool operator==(const Vector2d &a) const
 931         {
 932                 if ( a.x == x && a.y == y ) return true;
 933                 return false;
 934         };
 935
 936         bool operator!=(const Vector2d &a) const
 937         {
 938                 if ( a.x != x || a.y != y ) return true;
 939                 return false;
 940         };
 941
 942         Vector2d operator+(Vector2d a) const
 943         {
 944                 a.x+=x;
 945                 a.y+=y;
 946                 return a;
 947         };
 948
 949         Vector2d operator-(Vector2d a) const
 950         {
 951                 a.x = x-a.x;
 952                 a.y = y-a.y;
 953                 return a;
 954         };
 955
 956         Vector2d operator - (void) const
 957         {
 958                 return negative();
 959         };
 960
 961         Vector2d operator*(Vector2d a) const
 962         {
 963                 a.x*=x;
 964                 a.y*=y;
 965                 return a;
 966         };
 967
 968         Vector2d operator*(float c) const
 969         {
 970                 Vector2d a;
 971
 972                 a.x = x * c;
 973                 a.y = y * c;
 974
 975                 return a;
 976         };
 977
 978         Vector2d operator/(Vector2d a) const
 979         {
 980                 a.x = x/a.x;
 981                 a.y = y/a.y;
 982                 return a;
 983         };
 984
 985
 986         float Dot(const Vector2d &a) const        // computes dot product.
 987         {
 988                 return (x * a.x + y * a.y );
 989         };
 990
 991         float GetX(void) const { return x; };
 992         float GetY(void) const { return y; };
 993
 994         void SetX(float t) { x   = t; };
 995         void SetY(float t) { y   = t; };
 996
 997         void Set(float a,float b)
 998         {
 999                 x = a;
1000                 y = b;
1001         };
1002
1003         void Zero(void)
1004         {
1005                 x = y = 0;
1006         };
1007
1008         Vector2d negative(void) const
1009         {
1010                 Vector2d result;
1011                 result.x = -x;
1012                 result.y = -y;
1013                 return result;
1014         }
1015
1016         float magnitude(void) const
1017         {
1018                 return (float) sqrtf(x * x + y * y );
1019         }
1020
1021         float fastmagnitude(void) const
1022         {
1023                 return (float) sqrtf(x * x + y * y );
1024         }
1025
1026         float fastermagnitude(void) const
1027         {
1028                 return (float) sqrtf( x * x + y * y );
1029         }
1030
1031         void Reflection(Vector2d &a,Vector2d &b); // compute reflection vector.
1032
1033         float Length(void) const          // length of vector.
1034         {
1035                 return float(sqrtf( x*x + y*y ));
1036         };
1037
1038         float FastLength(void) const          // length of vector.
1039         {
1040                 return float(sqrtf( x*x + y*y ));
1041         };
1042
1043         float FasterLength(void) const          // length of vector.
1044         {
1045                 return float(sqrtf( x*x + y*y ));
1046         };
1047
1048         float Length2(void)        // squared distance, prior to square root.
1049         {
1050                 return x*x+y*y;
1051         }
1052
1053         float Distance(const Vector2d &a) const   // distance between two points.
1054         {
1055                 float dx = a.x - x;
1056                 float dy = a.y - y;
1057                 float d  = dx*dx+dy*dy;
1058                 return sqrtf(d);
1059         };
1060
1061         float FastDistance(const Vector2d &a) const   // distance between two points.
1062         {
1063                 float dx = a.x - x;
1064                 float dy = a.y - y;
1065                 float d  = dx*dx+dy*dy;
1066                 return sqrtf(d);
1067         };
1068
1069         float FasterDistance(const Vector2d &a) const   // distance between two points.
1070         {
1071                 float dx = a.x - x;
1072                 float dy = a.y - y;
1073                 float d  = dx*dx+dy*dy;
1074                 return sqrtf(d);
1075         };
1076
1077         float Distance2(Vector2d &a) // squared distance.
1078         {
1079                 float dx = a.x - x;
1080                 float dy = a.y - y;
1081                 return dx*dx + dy *dy;
1082         };
1083
1084         void Lerp(const Vector2d& from,const Vector2d& to,float slerp)
1085         {
1086                 x = ((to.x - from.x)*slerp) + from.x;
1087                 y = ((to.y - from.y)*slerp) + from.y;
1088         };
1089
1090
1091         void Cross(const Vector2d &a,const Vector2d &b)  // cross two vectors result in this one.
1092         {
1093                 x = a.y*b.x - a.x*b.y;
1094                 y = a.x*b.x - a.x*b.x;
1095         };
1096
1097         float Normalize(void)       // normalize to a unit vector, returns distance.
1098         {
1099                 float l = Length();
1100                 if ( l != 0 )
1101                 {
1102                         l = float( 1 ) / l;
1103                         x*=l;
1104                         y*=l;
1105                 }
1106                 else
1107                 {
1108                         x = y = 0;
1109                 }
1110                 return l;
1111         };
1112
1113         float FastNormalize(void)       // normalize to a unit vector, returns distance.
1114         {
1115                 float l = FastLength();
1116                 if ( l != 0 )
1117                 {
1118                         l = float( 1 ) / l;
1119                         x*=l;
1120                         y*=l;
1121                 }
1122                 else
1123                 {
1124                         x = y = 0;
1125                 }
1126                 return l;
1127         };
1128
1129         float FasterNormalize(void)       // normalize to a unit vector, returns distance.
1130         {
1131                 float l = FasterLength();
1132                 if ( l != 0 )
1133                 {
1134                         l = float( 1 ) / l;
1135                         x*=l;
1136                         y*=l;
1137                 }
1138                 else
1139                 {
1140                         x = y = 0;
1141                 }
1142                 return l;
1143         };
1144
1145
1146         float x;
1147         float y;
1148 };
1149
1150 class Line
1151 {
1152 public:
1153         Line(const Vector3d &from,const Vector3d &to)
1154         {
1155                 mP1 = from;
1156                 mP2 = to;
1157         };
1158         // JWR  Test for the intersection of two lines.
1159
1160         bool Intersect(const Line& src,Vector3d &sect);
1161 private:
1162         Vector3d mP1;
1163         Vector3d mP2;
1164
1165 };
1166
1167
1168 typedef std::vector< Vector3d > Vector3dVector;
1169 typedef std::vector< Vector2d > Vector2dVector;
1170
1171 inline  Vector3d operator * (float s, const Vector3d &v )
1172 {
1173         Vector3d        Scaled(v.x*s, v.y*s, v.z*s);
1174         return(Scaled);
1175 }
1176
1177 inline  Vector2d        operator * (float s, const Vector2d &v )
1178  {
1179          Vector2d       Scaled(v.x*s, v.y*s);
1180         return(Scaled);
1181  }
1182
1183 }
1184
1185 #endif