cd_hull.cpp
Go to the documentation of this file.
00001 
00056 #include <stdio.h>
00057 #include <stdlib.h>
00058 #include <string.h>
00059 #include <assert.h>
00060 #include <math.h>
00061 #include <float.h>
00062 
00063 
00064 #include <stdarg.h>
00065 #include <setjmp.h>
00066 
00067 #include "cd_hull.h"
00068 
00069 #define STANDALONE 1  // This #define is used when tranferring this source code to other projects
00070 
00071 #if STANDALONE
00072 
00073 #undef NX_ALLOC
00074 #undef NX_FREE
00075 
00076 #define NX_ALLOC(x,y) malloc(x)
00077 #define NX_FREE(x) free(x)
00078 
00079 #else
00080 #include "Allocateable.h"
00081 #endif
00082 
00083 namespace ConvexDecomposition
00084 {
00085 
00086 //*****************************************************
00087 //*** DARRAY.H
00088 //*****************************************************
00089 
00090 template <class Type> class ArrayRet;
00091 template <class Type> class Array
00092 {
00093         public:
00094                                 Array(int s=0);
00095                                 Array(Array<Type> &array);
00096                                 Array(ArrayRet<Type> &array);
00097                                 ~Array();
00098         void            allocate(int s);
00099         void            SetSize(int s);
00100         void            Pack();
00101         Type&           Add(Type);
00102         void            AddUnique(Type);
00103         int             Contains(Type);
00104         void            Insert(Type,int);
00105         int                     IndexOf(Type);
00106         void            Remove(Type);
00107         void            DelIndex(int i);
00108         Type *          element;
00109         int                     count;
00110         int                     array_size;
00111         const Type      &operator[](int i) const { assert(i>=0 && i<count);  return element[i]; }
00112         Type            &operator[](int i)  { assert(i>=0 && i<count);  return element[i]; }
00113         Type            &Pop() { assert(count); count--;  return element[count]; }
00114         Array<Type> &operator=(Array<Type> &array);
00115         Array<Type> &operator=(ArrayRet<Type> &array);
00116         // operator ArrayRet<Type> &() { return *(ArrayRet<Type> *)this;} // this worked but i suspect could be dangerous
00117 };
00118 
00119 template <class Type> class ArrayRet:public Array<Type>
00120 {
00121 };
00122 
00123 template <class Type> Array<Type>::Array(int s)
00124 {
00125         count=0;
00126         array_size = 0;
00127         element = NULL;
00128         if(s)
00129         {
00130                 allocate(s);
00131         }
00132 }
00133 
00134 
00135 template <class Type> Array<Type>::Array(Array<Type> &array)
00136 {
00137         count=0;
00138         array_size = 0;
00139         element = NULL;
00140         for(int i=0;i<array.count;i++)
00141         {
00142                 Add(array[i]);
00143         }
00144 }
00145 
00146 
00147 template <class Type> Array<Type>::Array(ArrayRet<Type> &array)
00148 {
00149         *this = array;
00150 }
00151 template <class Type> Array<Type> &Array<Type>::operator=(ArrayRet<Type> &array)
00152 {
00153         count=array.count;
00154         array_size = array.array_size;
00155         element = array.element;
00156         array.element=NULL;
00157         array.count=0;
00158         array.array_size=0;
00159         return *this;
00160 }
00161 
00162 
00163 template <class Type> Array<Type> &Array<Type>::operator=(Array<Type> &array)
00164 {
00165         count=0;
00166         for(int i=0;i<array.count;i++)
00167         {
00168                 Add(array[i]);
00169         }
00170         return *this;
00171 }
00172 
00173 template <class Type> Array<Type>::~Array()
00174 {
00175         if (element != NULL)
00176         {
00177           NX_FREE(element);
00178         }
00179         count=0;array_size=0;element=NULL;
00180 }
00181 
00182 template <class Type> void Array<Type>::allocate(int s)
00183 {
00184         assert(s>0);
00185         assert(s>=count);
00186         Type *old = element;
00187         array_size =s;
00188         element = (Type *) NX_ALLOC( sizeof(Type)*array_size, CONVEX_TEMP );
00189         assert(element);
00190         for(int i=0;i<count;i++)
00191         {
00192                 element[i]=old[i];
00193         }
00194         if(old)
00195         {
00196                 NX_FREE(old);
00197         }
00198 }
00199 
00200 template <class Type> void Array<Type>::SetSize(int s)
00201 {
00202         if(s==0)
00203         {
00204                 if(element)
00205                 {
00206                         NX_FREE(element);
00207                         element = NULL;
00208                 }
00209           array_size = s;
00210         }
00211         else
00212         {
00213                 allocate(s);
00214         }
00215         count=s;
00216 }
00217 
00218 template <class Type> void Array<Type>::Pack()
00219 {
00220         allocate(count);
00221 }
00222 
00223 template <class Type> Type& Array<Type>::Add(Type t)
00224 {
00225         assert(count<=array_size);
00226         if(count==array_size)
00227         {
00228                 allocate((array_size)?array_size *2:16);
00229         }
00230         element[count++] = t;
00231         return element[count-1];
00232 }
00233 
00234 template <class Type> int Array<Type>::Contains(Type t)
00235 {
00236         int i;
00237         int found=0;
00238         for(i=0;i<count;i++)
00239         {
00240                 if(element[i] == t) found++;
00241         }
00242         return found;
00243 }
00244 
00245 template <class Type> void Array<Type>::AddUnique(Type t)
00246 {
00247         if(!Contains(t)) Add(t);
00248 }
00249 
00250 
00251 template <class Type> void Array<Type>::DelIndex(int i)
00252 {
00253         assert(i<count);
00254         count--;
00255         while(i<count)
00256         {
00257                 element[i] = element[i+1];
00258                 i++;
00259         }
00260 }
00261 
00262 template <class Type> void Array<Type>::Remove(Type t)
00263 {
00264         int i;
00265         for(i=0;i<count;i++)
00266         {
00267                 if(element[i] == t)
00268                 {
00269                         break;
00270                 }
00271         }
00272         assert(i<count); // assert object t is in the array.
00273         DelIndex(i);
00274         for(i=0;i<count;i++)
00275         {
00276                 assert(element[i] != t);
00277         }
00278 }
00279 
00280 template <class Type> void Array<Type>::Insert(Type t,int k)
00281 {
00282         int i=count;
00283         Add(t); // to allocate space
00284         while(i>k)
00285         {
00286                 element[i]=element[i-1];
00287                 i--;
00288         }
00289         assert(i==k);
00290         element[k]=t;
00291 }
00292 
00293 
00294 template <class Type> int Array<Type>::IndexOf(Type t)
00295 {
00296         int i;
00297         for(i=0;i<count;i++)
00298         {
00299                 if(element[i] == t)
00300                 {
00301                         return i;
00302                 }
00303         }
00304         assert(0);
00305         return -1;
00306 }
00307 
00308 //****************************************************
00309 //** VECMATH.H
00310 //****************************************************
00311 #ifndef PLUGIN_3DSMAX
00312 #define PI (3.1415926535897932384626433832795f)
00313 #endif
00314 
00315 #define DEG2RAD (PI / 180.0f)
00316 #define RAD2DEG (180.0f / PI)
00317 #define SQRT_OF_2 (1.4142135f)
00318 #define OFFSET(Class,Member)  (((char*) (&(((Class*)NULL)-> Member )))- ((char*)NULL))
00319 
00320 
00321 
00322 int    argmin(double a[],int n);
00323 double  sqr(double a); 
00324 double  clampf(double a) ;
00325 double  Round(double a,double precision);
00326 double  Interpolate(const double &f0,const double &f1,double alpha) ;
00327 
00328 template <class T>
00329 void Swap(T &a,T &b) 
00330 {
00331         T tmp = a;
00332         a=b;
00333         b=tmp;
00334 }
00335 
00336 
00337 
00338 template <class T>
00339 T Max(const T &a,const T &b) 
00340 {
00341         return (a>b)?a:b;
00342 }
00343 
00344 template <class T>
00345 T Min(const T &a,const T &b) 
00346 {
00347         return (a<b)?a:b;
00348 }
00349 
00350 //----------------------------------
00351 
00352 class int3  
00353 {
00354 public:
00355         int x,y,z;
00356         int3(){};
00357         int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
00358         const int& operator[](int i) const {return (&x)[i];}
00359         int& operator[](int i) {return (&x)[i];}
00360 };
00361 
00362 
00363 //-------- 2D --------
00364 
00365 class double2
00366 {
00367 public:
00368         double x,y;
00369         double2(){x=0;y=0;};
00370         double2(double _x,double _y){x=_x;y=_y;}
00371         double& operator[](int i) {assert(i>=0&&i<2);return ((double*)this)[i];}
00372         const double& operator[](int i) const {assert(i>=0&&i<2);return ((double*)this)[i];}
00373 };
00374 inline double2 operator-( const double2& a, const double2& b ){return double2(a.x-b.x,a.y-b.y);}
00375 inline double2 operator+( const double2& a, const double2& b ){return double2(a.x+b.x,a.y+b.y);}
00376 
00377 //--------- 3D ---------
00378 
00379 class double3 // 3D
00380 {
00381         public:
00382         double x,y,z;
00383         double3(){x=0;y=0;z=0;};
00384         double3(double _x,double _y,double _z){x=_x;y=_y;z=_z;};
00385         //operator double *() { return &x;};
00386         double& operator[](int i) {assert(i>=0&&i<3);return ((double*)this)[i];}
00387         const double& operator[](int i) const {assert(i>=0&&i<3);return ((double*)this)[i];}
00388 #       ifdef PLUGIN_3DSMAX
00389         double3(const Point3 &p):x(p.x),y(p.y),z(p.z){}
00390         operator Point3(){return *((Point3*)this);}
00391 #       endif
00392 };
00393 
00394 
00395 double3& operator+=( double3 &a, const double3& b );
00396 double3& operator-=( double3 &a ,const double3& b );
00397 double3& operator*=( double3 &v ,const double s );
00398 double3& operator/=( double3 &v, const double s );
00399 
00400 double  magnitude( const double3& v );
00401 double3 normalize( const double3& v );
00402 double3 safenormalize(const double3 &v);
00403 double3 vabs(const double3 &v);
00404 double3 operator+( const double3& a, const double3& b );
00405 double3 operator-( const double3& a, const double3& b );
00406 double3 operator-( const double3& v );
00407 double3 operator*( const double3& v, const double s );
00408 double3 operator*( const double s, const double3& v );
00409 double3 operator/( const double3& v, const double s );
00410 inline int operator==( const double3 &a, const double3 &b ) { return (a.x==b.x && a.y==b.y && a.z==b.z); }
00411 inline int operator!=( const double3 &a, const double3 &b ) { return (a.x!=b.x || a.y!=b.y || a.z!=b.z); }
00412 // due to ambiguity and inconsistent standards ther are no overloaded operators for mult such as va*vb.
00413 double  dot( const double3& a, const double3& b );
00414 double3 cmul( const double3 &a, const double3 &b);
00415 double3 cross( const double3& a, const double3& b );
00416 double3 Interpolate(const double3 &v0,const double3 &v1,double alpha);
00417 double3 Round(const double3& a,double precision);
00418 double3 VectorMax(const double3 &a, const double3 &b);
00419 double3 VectorMin(const double3 &a, const double3 &b);
00420 
00421 
00422 
00423 class double3x3
00424 {
00425         public:
00426         double3 x,y,z;  // the 3 rows of the Matrix
00427         double3x3(){}
00428         double3x3(double xx,double xy,double xz,double yx,double yy,double yz,double zx,double zy,double zz):x(xx,xy,xz),y(yx,yy,yz),z(zx,zy,zz){}
00429         double3x3(double3 _x,double3 _y,double3 _z):x(_x),y(_y),z(_z){}
00430         double3&       operator[](int i)       {assert(i>=0&&i<3);return (&x)[i];}
00431         const double3& operator[](int i) const {assert(i>=0&&i<3);return (&x)[i];}
00432         double&        operator()(int r, int c)       {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];}
00433         const double&  operator()(int r, int c) const {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];}
00434 }; 
00435 double3x3 Transpose( const double3x3& m );
00436 double3   operator*( const double3& v  , const double3x3& m  );
00437 double3   operator*( const double3x3& m , const double3& v   );
00438 double3x3 operator*( const double3x3& m , const double& s   );
00439 double3x3 operator*( const double3x3& ma, const double3x3& mb );
00440 double3x3 operator/( const double3x3& a, const double& s ) ;
00441 double3x3 operator+( const double3x3& a, const double3x3& b );
00442 double3x3 operator-( const double3x3& a, const double3x3& b );
00443 double3x3 &operator+=( double3x3& a, const double3x3& b );
00444 double3x3 &operator-=( double3x3& a, const double3x3& b );
00445 double3x3 &operator*=( double3x3& a, const double& s );
00446 double    Determinant(const double3x3& m );
00447 double3x3 Inverse(const double3x3& a);  // its just 3x3 so we simply do that cofactor method
00448 
00449 
00450 //-------- 4D Math --------
00451 
00452 class double4
00453 {
00454 public:
00455         double x,y,z,w;
00456         double4(){x=0;y=0;z=0;w=0;};
00457         double4(double _x,double _y,double _z,double _w){x=_x;y=_y;z=_z;w=_w;}
00458         double4(const double3 &v,double _w){x=v.x;y=v.y;z=v.z;w=_w;}
00459         //operator double *() { return &x;};
00460         double& operator[](int i) {assert(i>=0&&i<4);return ((double*)this)[i];}
00461         const double& operator[](int i) const {assert(i>=0&&i<4);return ((double*)this)[i];}
00462         const double3& xyz() const { return *((double3*)this);}
00463         double3&       xyz()       { return *((double3*)this);}
00464 };
00465 
00466 
00467 struct D3DXMATRIX; 
00468 
00469 class double4x4
00470 {
00471         public:
00472         double4 x,y,z,w;  // the 4 rows
00473         double4x4(){}
00474         double4x4(const double4 &_x, const double4 &_y, const double4 &_z, const double4 &_w):x(_x),y(_y),z(_z),w(_w){}
00475         double4x4(double m00, double m01, double m02, double m03, 
00476                                                 double m10, double m11, double m12, double m13, 
00477                                 double m20, double m21, double m22, double m23, 
00478                                 double m30, double m31, double m32, double m33 )
00479                         :x(m00,m01,m02,m03),y(m10,m11,m12,m13),z(m20,m21,m22,m23),w(m30,m31,m32,m33){}
00480         double&       operator()(int r, int c)       {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];}
00481         const double& operator()(int r, int c) const {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];}
00482                 operator       double* ()       {return &x.x;}
00483                 operator const double* () const {return &x.x;}
00484         operator       struct D3DXMATRIX* ()       { return (struct D3DXMATRIX*) this;}
00485         operator const struct D3DXMATRIX* () const { return (struct D3DXMATRIX*) this;}
00486 };
00487 
00488 
00489 int     operator==( const double4 &a, const double4 &b );
00490 double4 Homogenize(const double3 &v3,const double &w=1.0f); // Turns a 3D double3 4D vector4 by appending w
00491 double4 cmul( const double4 &a, const double4 &b);
00492 double4 operator*( const double4 &v, double s);
00493 double4 operator*( double s, const double4 &v);
00494 double4 operator+( const double4 &a, const double4 &b);
00495 double4 operator-( const double4 &a, const double4 &b);
00496 double4x4 operator*( const double4x4& a, const double4x4& b );
00497 double4 operator*( const double4& v, const double4x4& m );
00498 double4x4 Inverse(const double4x4 &m);
00499 double4x4 MatrixRigidInverse(const double4x4 &m);
00500 double4x4 MatrixTranspose(const double4x4 &m);
00501 double4x4 MatrixPerspectiveFov(double fovy, double Aspect, double zn, double zf );
00502 double4x4 MatrixTranslation(const double3 &t);
00503 double4x4 MatrixRotationZ(const double angle_radians);
00504 double4x4 MatrixLookAt(const double3& eye, const double3& at, const double3& up);
00505 int     operator==( const double4x4 &a, const double4x4 &b );
00506 
00507 
00508 //-------- Quaternion ------------
00509 
00510 class Quaternion :public double4
00511 {
00512  public:
00513         Quaternion() { x = y = z = 0.0f; w = 1.0f; }
00514         Quaternion( double3 v, double t ) { v = normalize(v); w = cos(t/2.0f); v = v*sin(t/2.0f); x = v.x; y = v.y; z = v.z; }
00515         Quaternion(double _x, double _y, double _z, double _w){x=_x;y=_y;z=_z;w=_w;}
00516         double angle() const { return acos(w)*2.0f; }
00517         double3 axis() const { double3 a(x,y,z); if(fabs(angle())<0.0000001f) return double3(1,0,0); return a*(1/sin(angle()/2.0f)); }
00518         double3 xdir() const { return double3( 1-2*(y*y+z*z),  2*(x*y+w*z),  2*(x*z-w*y) ); }
00519         double3 ydir() const { return double3(   2*(x*y-w*z),1-2*(x*x+z*z),  2*(y*z+w*x) ); }
00520         double3 zdir() const { return double3(   2*(x*z+w*y),  2*(y*z-w*x),1-2*(x*x+y*y) ); }
00521         double3x3 getmatrix() const { return double3x3( xdir(), ydir(), zdir() ); }
00522         operator double3x3() { return getmatrix(); }
00523         void Normalize();
00524 };
00525 
00526 Quaternion& operator*=(Quaternion& a, double s );
00527 Quaternion      operator*( const Quaternion& a, double s );
00528 Quaternion      operator*( const Quaternion& a, const Quaternion& b);
00529 Quaternion      operator+( const Quaternion& a, const Quaternion& b );
00530 Quaternion      normalize( Quaternion a );
00531 double          dot( const Quaternion &a, const Quaternion &b );
00532 double3         operator*( const Quaternion& q, const double3& v );
00533 double3         operator*( const double3& v, const Quaternion& q );
00534 Quaternion      slerp( Quaternion a, const Quaternion& b, double interp );
00535 Quaternion  Interpolate(const Quaternion &q0,const Quaternion &q1,double alpha); 
00536 Quaternion  RotationArc(double3 v0, double3 v1 );  // returns quat q where q*v0=v1
00537 Quaternion  Inverse(const Quaternion &q);
00538 double4x4     MatrixFromQuatVec(const Quaternion &q, const double3 &v);
00539 
00540 
00541 //------ Euler Angle -----
00542 
00543 Quaternion YawPitchRoll( double yaw, double pitch, double roll );
00544 double Yaw( const Quaternion& q );
00545 double Pitch( const Quaternion& q );
00546 double Roll( Quaternion q );
00547 double Yaw( const double3& v );
00548 double Pitch( const double3& v );
00549 
00550 
00551 //------- Plane ----------
00552 
00553 class Plane 
00554 {
00555         public:
00556         double3 normal;
00557         double  dist;   // distance below origin - the D from plane equasion Ax+By+Cz+D=0
00558                         Plane(const double3 &n,double d):normal(n),dist(d){}
00559                         Plane():normal(),dist(0){}
00560         void    Transform(const double3 &position, const Quaternion &orientation);
00561 };
00562 
00563 inline Plane PlaneFlip(const Plane &plane){return Plane(-plane.normal,-plane.dist);}
00564 inline int operator==( const Plane &a, const Plane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
00565 inline int coplanar( const Plane &a, const Plane &b ) { return (a==b || a==PlaneFlip(b)); }
00566 
00567 
00568 //--------- Utility Functions ------
00569 
00570 double3  PlaneLineIntersection(const Plane &plane, const double3 &p0, const double3 &p1);
00571 double3  PlaneProject(const Plane &plane, const double3 &point);
00572 double3  LineProject(const double3 &p0, const double3 &p1, const double3 &a);  // projects a onto infinite line p0p1
00573 double   LineProjectTime(const double3 &p0, const double3 &p1, const double3 &a);
00574 double3  ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2);
00575 int     PolyHit(const double3 *vert,const int n,const double3 &v0, const double3 &v1, double3 *impact=NULL, double3 *normal=NULL);
00576 int     BoxInside(const double3 &p,const double3 &bmin, const double3 &bmax) ;
00577 int     BoxIntersect(const double3 &v0, const double3 &v1, const double3 &bmin, const double3 &bmax, double3 *impact);
00578 double   DistanceBetweenLines(const double3 &ustart, const double3 &udir, const double3 &vstart, const double3 &vdir, double3 *upoint=NULL, double3 *vpoint=NULL);
00579 double3  TriNormal(const double3 &v0, const double3 &v1, const double3 &v2);
00580 double3  NormalOf(const double3 *vert, const int n);
00581 Quaternion VirtualTrackBall(const double3 &cop, const double3 &cor, const double3 &dir0, const double3 &dir1);
00582 
00583 
00584 
00585 
00586 //*****************************************************
00587 // ** VECMATH.CPP
00588 //*****************************************************
00589 
00590 
00591 double   sqr(double a) {return a*a;}
00592 double   clampf(double a) {return Min(1.0,Max(0.0,a));}
00593 
00594 
00595 double Round(double a,double precision)
00596 {
00597         return floor(0.5f+a/precision)*precision;
00598 }
00599 
00600 
00601 double Interpolate(const double &f0,const double &f1,double alpha) 
00602 {
00603         return f0*(1-alpha) + f1*alpha;
00604 }
00605 
00606 
00607 int     argmin(double a[],int n)
00608 {
00609         int r=0;
00610         for(int i=1;i<n;i++) 
00611                 {
00612                 if(a[i]<a[r]) 
00613                                 {
00614                         r = i;                  
00615                 }
00616         }
00617         return r;
00618 }
00619 
00620 
00621 
00622 //------------ double3 (3D) --------------
00623 
00624 
00625 
00626 double3 operator+( const double3& a, const double3& b ) 
00627 {
00628         return double3(a.x+b.x, a.y+b.y, a.z+b.z); 
00629 }
00630 
00631 
00632 double3 operator-( const double3& a, const double3& b )
00633 {
00634         return double3( a.x-b.x, a.y-b.y, a.z-b.z ); 
00635 }
00636 
00637 
00638 double3 operator-( const double3& v )                     
00639 {
00640         return double3( -v.x, -v.y, -v.z ); 
00641 }
00642 
00643 
00644 double3 operator*( const double3& v, double s )      
00645 {
00646         return double3( v.x*s, v.y*s, v.z*s ); 
00647 }
00648 
00649 
00650 double3 operator*( double s, const double3& v )      
00651 {
00652         return double3( v.x*s, v.y*s, v.z*s ); 
00653 }
00654 
00655 
00656 double3 operator/( const double3& v, double s )
00657 { 
00658         return v*(1.0f/s); 
00659 }
00660 
00661 double  dot( const double3& a, const double3& b )    
00662 {
00663         return a.x*b.x + a.y*b.y + a.z*b.z; 
00664 }
00665 
00666 double3 cmul( const double3 &v1, const double3 &v2) 
00667 { 
00668         return double3(v1.x*v2.x, v1.y*v2.y, v1.z*v2.z); 
00669 }
00670 
00671 
00672 double3 cross( const double3& a, const double3& b )
00673 {
00674                 return double3( a.y*b.z - a.z*b.y,
00675                                                                          a.z*b.x - a.x*b.z,
00676                                                                          a.x*b.y - a.y*b.x );
00677 }
00678 
00679 
00680 
00681 
00682 double3& operator+=( double3& a , const double3& b )
00683 {
00684                 a.x += b.x;
00685                 a.y += b.y;
00686                 a.z += b.z;
00687                 return a;
00688 }
00689 
00690 
00691 double3& operator-=( double3& a , const double3& b )
00692 {
00693                 a.x -= b.x;
00694                 a.y -= b.y;
00695                 a.z -= b.z;
00696                 return a;
00697 }
00698 
00699 
00700 double3& operator*=(double3& v , double s )
00701 {
00702                 v.x *= s;
00703                 v.y *= s;
00704                 v.z *= s;
00705                 return v;
00706 }
00707 
00708 
00709 double3& operator/=(double3& v , double s )
00710 {
00711                 double sinv = 1.0f / s;
00712                 v.x *= sinv;
00713                 v.y *= sinv;
00714                 v.z *= sinv;
00715                 return v;
00716 }
00717 
00718 double3 vabs(const double3 &v)
00719 {
00720         return double3(fabs(v.x),fabs(v.y),fabs(v.z));
00721 }
00722 
00723 
00724 double magnitude( const double3& v )
00725 {
00726                 return sqrt(sqr(v.x) + sqr( v.y)+ sqr(v.z));
00727 }
00728 
00729 
00730 
00731 double3 normalize( const double3 &v )
00732 {
00733         // this routine, normalize, is ok, provided magnitude works!!
00734                 double d=magnitude(v);
00735                 if (d==0) 
00736                 {
00737                 printf("Cant normalize ZERO vector\n");
00738                 assert(0);// yes this could go here
00739                 d=0.1f;
00740         }
00741         d = 1/d;
00742         return double3(v.x*d,v.y*d,v.z*d);
00743 }
00744 
00745 double3 safenormalize(const double3 &v)
00746 {
00747         if(magnitude(v)<=0.0f)
00748         {
00749                 return double3(1,0,0);
00750         }
00751         return normalize(v);
00752 }
00753 
00754 double3 Round(const double3 &a,double precision)
00755 {
00756         return double3(Round(a.x,precision),Round(a.y,precision),Round(a.z,precision));
00757 }
00758 
00759 
00760 double3 Interpolate(const double3 &v0,const double3 &v1,double alpha) 
00761 {
00762         return v0*(1-alpha) + v1*alpha;
00763 }
00764 
00765 double3 VectorMin(const double3 &a,const double3 &b)
00766 {
00767         return double3(Min(a.x,b.x),Min(a.y,b.y),Min(a.z,b.z));
00768 }
00769 double3 VectorMax(const double3 &a,const double3 &b)
00770 {
00771         return double3(Max(a.x,b.x),Max(a.y,b.y),Max(a.z,b.z));
00772 }
00773 
00774 // the statement v1*v2 is ambiguous since there are 3 types
00775 // of vector multiplication
00776 //  - componantwise (for example combining colors)
00777 //  - dot product
00778 //  - cross product
00779 // Therefore we never declare/implement this function.
00780 // So we will never see:  double3 operator*(double3 a,double3 b) 
00781 
00782 
00783 
00784 
00785 //------------ double3x3 ---------------
00786 double Determinant(const double3x3 &m)
00787 {
00788         return  m.x.x*m.y.y*m.z.z + m.y.x*m.z.y*m.x.z + m.z.x*m.x.y*m.y.z 
00789                          -m.x.x*m.z.y*m.y.z - m.y.x*m.x.y*m.z.z - m.z.x*m.y.y*m.x.z ;
00790 }
00791 
00792 double3x3 Inverse(const double3x3 &a)
00793 {
00794         double3x3 b;
00795         double d=Determinant(a);
00796         assert(d!=0);
00797         for(int i=0;i<3;i++) 
00798                 {
00799                 for(int j=0;j<3;j++) 
00800                                 {
00801                         int i1=(i+1)%3;
00802                         int i2=(i+2)%3;
00803                         int j1=(j+1)%3;
00804                         int j2=(j+2)%3;
00805                         // reverse indexs i&j to take transpose
00806                         b[j][i] = (a[i1][j1]*a[i2][j2]-a[i1][j2]*a[i2][j1])/d;
00807                 }
00808         }
00809         // Matrix check=a*b; // Matrix 'check' should be the identity (or close to it)
00810         return b;
00811 }
00812 
00813 
00814 double3x3 Transpose( const double3x3& m )
00815 {
00816         return double3x3( double3(m.x.x,m.y.x,m.z.x),
00817                                         double3(m.x.y,m.y.y,m.z.y),
00818                                         double3(m.x.z,m.y.z,m.z.z));
00819 }
00820 
00821 
00822 double3 operator*(const double3& v , const double3x3 &m ) { 
00823         return double3((m.x.x*v.x + m.y.x*v.y + m.z.x*v.z), 
00824                                         (m.x.y*v.x + m.y.y*v.y + m.z.y*v.z), 
00825                                         (m.x.z*v.x + m.y.z*v.y + m.z.z*v.z));
00826 }
00827 double3 operator*(const double3x3 &m,const double3& v  ) { 
00828         return double3(dot(m.x,v),dot(m.y,v),dot(m.z,v));
00829 }
00830 
00831 
00832 double3x3 operator*( const double3x3& a, const double3x3& b )  
00833 { 
00834         return double3x3(a.x*b,a.y*b,a.z*b);
00835 }
00836 
00837 double3x3 operator*( const double3x3& a, const double& s )  
00838 { 
00839         return double3x3(a.x*s, a.y*s ,a.z*s); 
00840 }
00841 double3x3 operator/( const double3x3& a, const double& s )  
00842 { 
00843         double t=1/s;
00844         return double3x3(a.x*t, a.y*t ,a.z*t); 
00845 }
00846 double3x3 operator+( const double3x3& a, const double3x3& b )
00847 {
00848         return double3x3(a.x+b.x, a.y+b.y, a.z+b.z);
00849 }
00850 double3x3 operator-( const double3x3& a, const double3x3& b )
00851 {
00852         return double3x3(a.x-b.x, a.y-b.y, a.z-b.z);
00853 }
00854 double3x3 &operator+=( double3x3& a, const double3x3& b )
00855 {
00856         a.x+=b.x;
00857         a.y+=b.y;
00858         a.z+=b.z;
00859         return a;
00860 }
00861 double3x3 &operator-=( double3x3& a, const double3x3& b )
00862 {
00863         a.x-=b.x;
00864         a.y-=b.y;
00865         a.z-=b.z;
00866         return a;
00867 }
00868 double3x3 &operator*=( double3x3& a, const double& s )
00869 {
00870         a.x*=s;
00871         a.y*=s;
00872         a.z*=s;
00873         return a;
00874 }
00875 
00876 
00877 
00878 double3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2){
00879         double3x3 mp =Transpose(double3x3(p0.normal,p1.normal,p2.normal));
00880         double3x3 mi = Inverse(mp);
00881         double3 b(p0.dist,p1.dist,p2.dist);
00882         return -b * mi;
00883 }
00884 
00885 
00886 //--------------- 4D ----------------
00887 
00888 double4   operator*( const double4&   v, const double4x4& m )
00889 {
00890         return v.x*m.x + v.y*m.y + v.z*m.z + v.w*m.w; // yes this actually works
00891 }
00892 
00893 int operator==( const double4 &a, const double4 &b ) 
00894 {
00895         return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w); 
00896 }
00897 
00898 
00899 //  Dont implement m*v for now, since that might confuse us
00900 //  All our transforms are based on multiplying the "row" vector on the left
00901 //double4   operator*(const double4x4& m , const double4&   v )
00902 //{
00903 //      return double4(dot(v,m.x),dot(v,m.y),dot(v,m.z),dot(v,m.w));
00904 //}
00905 
00906 
00907 
00908 double4 cmul( const double4 &a, const double4 &b) 
00909 {
00910         return double4(a.x*b.x,a.y*b.y,a.z*b.z,a.w*b.w);
00911 }
00912 
00913 
00914 double4 operator*( const double4 &v, double s) 
00915 {
00916         return double4(v.x*s,v.y*s,v.z*s,v.w*s);
00917 }
00918 
00919 
00920 double4 operator*( double s, const double4 &v) 
00921 {
00922         return double4(v.x*s,v.y*s,v.z*s,v.w*s);
00923 }
00924 
00925 
00926 double4 operator+( const double4 &a, const double4 &b) 
00927 {
00928         return double4(a.x+b.x,a.y+b.y,a.z+b.z,a.w+b.w);
00929 }
00930 
00931 
00932 
00933 double4 operator-( const double4 &a, const double4 &b) 
00934 {
00935         return double4(a.x-b.x,a.y-b.y,a.z-b.z,a.w-b.w);
00936 }
00937 
00938 
00939 double4 Homogenize(const double3 &v3,const double &w)
00940 {
00941         return double4(v3.x,v3.y,v3.z,w);
00942 }
00943 
00944 
00945 
00946 double4x4 operator*( const double4x4& a, const double4x4& b )
00947 {
00948         return double4x4(a.x*b,a.y*b,a.z*b,a.w*b);
00949 }
00950 
00951 double4x4 MatrixTranspose(const double4x4 &m)
00952 {
00953         return double4x4(
00954                 m.x.x, m.y.x, m.z.x, m.w.x,
00955                 m.x.y, m.y.y, m.z.y, m.w.y,
00956                 m.x.z, m.y.z, m.z.z, m.w.z,
00957                 m.x.w, m.y.w, m.z.w, m.w.w );
00958 }
00959 
00960 double4x4 MatrixRigidInverse(const double4x4 &m)
00961 {
00962         double4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
00963         double4x4 rot   = m;
00964         rot.w = double4(0,0,0,1);
00965         return trans_inverse * MatrixTranspose(rot);
00966 }
00967 
00968 
00969 double4x4 MatrixPerspectiveFov(double fovy, double aspect, double zn, double zf )
00970 {
00971         double h = 1.0f/tan(fovy/2.0f); // view space height
00972         double w = h / aspect ;  // view space width
00973         return double4x4(
00974                 w, 0, 0             ,   0,
00975                 0, h, 0             ,   0,
00976                 0, 0, zf/(zn-zf)    ,  -1,
00977                 0, 0, zn*zf/(zn-zf) ,   0 );
00978 }
00979 
00980 
00981 
00982 double4x4 MatrixLookAt(const double3& eye, const double3& at, const double3& up)
00983 {
00984         double4x4 m;
00985         m.w.w = 1.0f;
00986         m.w.xyz() = eye;
00987         m.z.xyz() = normalize(eye-at);
00988         m.x.xyz() = normalize(cross(up,m.z.xyz()));
00989         m.y.xyz() = cross(m.z.xyz(),m.x.xyz());
00990         return MatrixRigidInverse(m);
00991 }
00992 
00993 
00994 double4x4 MatrixTranslation(const double3 &t)
00995 {
00996         return double4x4(
00997                 1,  0,  0,  0,
00998                 0,  1,  0,  0,
00999                 0,  0,  1,  0,
01000                 t.x,t.y,t.z,1 );
01001 }
01002 
01003 
01004 double4x4 MatrixRotationZ(const double angle_radians)
01005 {
01006         double s =  sin(angle_radians);
01007         double c =  cos(angle_radians);
01008         return double4x4(
01009                 c,  s,  0,  0,
01010                 -s, c,  0,  0,
01011                 0,  0,  1,  0,
01012                 0,  0,  0,  1 );
01013 }
01014 
01015 
01016 
01017 int operator==( const double4x4 &a, const double4x4 &b )
01018 {
01019         return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);
01020 }
01021 
01022 
01023 double4x4 Inverse(const double4x4 &m)
01024 {
01025         double4x4 d;
01026         double *dst = &d.x.x;
01027         double tmp[12]; /* temp array for pairs */
01028         double src[16]; /* array of transpose source matrix */
01029         double det; /* determinant */
01030         /* transpose matrix */
01031         for ( int i = 0; i < 4; i++) {
01032                 src[i] = m(i,0) ;
01033                 src[i + 4] = m(i,1);
01034                 src[i + 8] = m(i,2);
01035                 src[i + 12] = m(i,3); 
01036         }
01037         /* calculate pairs for first 8 elements (cofactors) */
01038         tmp[0]  = src[10] * src[15];
01039         tmp[1]  = src[11] * src[14];
01040         tmp[2]  = src[9] * src[15];
01041         tmp[3]  = src[11] * src[13];
01042         tmp[4]  = src[9] * src[14];
01043         tmp[5]  = src[10] * src[13];
01044         tmp[6]  = src[8] * src[15];
01045         tmp[7]  = src[11] * src[12];
01046         tmp[8]  = src[8] * src[14];
01047         tmp[9]  = src[10] * src[12];
01048         tmp[10] = src[8] * src[13];
01049         tmp[11] = src[9] * src[12];
01050         /* calculate first 8 elements (cofactors) */
01051         dst[0]  = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
01052         dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
01053         dst[1]  = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
01054         dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
01055         dst[2]  = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
01056         dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
01057         dst[3]  = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
01058         dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
01059         dst[4]  = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
01060         dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
01061         dst[5]  = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
01062         dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
01063         dst[6]  = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
01064         dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
01065         dst[7]  = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
01066         dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
01067         /* calculate pairs for second 8 elements (cofactors) */
01068         tmp[0]  = src[2]*src[7];
01069         tmp[1]  = src[3]*src[6];
01070         tmp[2]  = src[1]*src[7];
01071         tmp[3]  = src[3]*src[5];
01072         tmp[4]  = src[1]*src[6];
01073         tmp[5]  = src[2]*src[5];
01074         tmp[6]  = src[0]*src[7];
01075         tmp[7]  = src[3]*src[4];
01076         tmp[8]  = src[0]*src[6];
01077         tmp[9]  = src[2]*src[4];
01078         tmp[10] = src[0]*src[5];
01079         tmp[11] = src[1]*src[4];
01080         /* calculate second 8 elements (cofactors) */
01081         dst[8]  = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
01082         dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
01083         dst[9]  = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
01084         dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
01085         dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
01086         dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
01087         dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
01088         dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
01089         dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
01090         dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
01091         dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
01092         dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
01093         dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
01094         dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
01095         dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
01096         dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
01097         /* calculate determinant */
01098         det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
01099         /* calculate matrix inverse */
01100         det = 1/det;
01101         for ( int j = 0; j < 16; j++)
01102         dst[j] *= det;
01103         return d;
01104 }
01105 
01106 
01107 //--------- Quaternion --------------
01108         
01109 Quaternion operator*( const Quaternion& a, const Quaternion& b )
01110 {
01111         Quaternion c;
01112         c.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; 
01113         c.x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y; 
01114         c.y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x; 
01115         c.z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w; 
01116         return c;
01117 }
01118 
01119 
01120 Quaternion operator*( const Quaternion& a, double b )
01121 {
01122         return Quaternion(a.x*b, a.y*b, a.z*b ,a.w*b);
01123 }
01124 
01125 Quaternion  Inverse(const Quaternion &q)
01126 {
01127         return Quaternion(-q.x,-q.y,-q.z,q.w);
01128 }
01129 
01130 Quaternion& operator*=( Quaternion& q, const double s )
01131 {
01132                 q.x *= s;
01133                 q.y *= s;
01134                 q.z *= s;
01135                 q.w *= s;
01136                 return q;
01137 }
01138 void Quaternion::Normalize()
01139 {
01140         double m = sqrt(sqr(w)+sqr(x)+sqr(y)+sqr(z));
01141         if(m<0.000000001f) {
01142                 w=1.0f;
01143                 x=y=z=0.0f;
01144                 return;
01145         }
01146         (*this) *= (1.0f/m);
01147 }
01148 
01149 double3 operator*( const Quaternion& q, const double3& v )
01150 {
01151         // The following is equivalent to:   
01152         //return (q.getmatrix() * v);  
01153         double qx2 = q.x*q.x;
01154         double qy2 = q.y*q.y;
01155         double qz2 = q.z*q.z;
01156 
01157         double qxqy = q.x*q.y;
01158         double qxqz = q.x*q.z;
01159         double qxqw = q.x*q.w;
01160         double qyqz = q.y*q.z;
01161         double qyqw = q.y*q.w;
01162         double qzqw = q.z*q.w;
01163         return double3(
01164                 (1-2*(qy2+qz2))*v.x + (2*(qxqy-qzqw))*v.y + (2*(qxqz+qyqw))*v.z ,
01165                 (2*(qxqy+qzqw))*v.x + (1-2*(qx2+qz2))*v.y + (2*(qyqz-qxqw))*v.z ,
01166                 (2*(qxqz-qyqw))*v.x + (2*(qyqz+qxqw))*v.y + (1-2*(qx2+qy2))*v.z  );
01167 }
01168 
01169 double3 operator*( const double3& v, const Quaternion& q )
01170 {
01171         assert(0);  // must multiply with the quat on the left
01172         return double3(0.0f,0.0f,0.0f);
01173 }
01174 
01175 Quaternion operator+( const Quaternion& a, const Quaternion& b )
01176 {
01177         return Quaternion(a.x+b.x, a.y+b.y, a.z+b.z, a.w+b.w);
01178 }
01179 
01180 double dot( const Quaternion &a,const Quaternion &b )
01181 {
01182         return  (a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z); 
01183 }
01184 
01185 Quaternion normalize( Quaternion a )
01186 {
01187         double m = sqrt(sqr(a.w)+sqr(a.x)+sqr(a.y)+sqr(a.z));
01188         if(m<0.000000001) 
01189                 {    
01190                 a.w=1;
01191                 a.x=a.y=a.z=0;
01192                 return a;
01193         }
01194         return a * (1/m);
01195 }
01196 
01197 Quaternion slerp( Quaternion a, const Quaternion& b, double interp )
01198 {
01199         if(dot(a,b) <0.0) 
01200                 {
01201                 a.w=-a.w;
01202                 a.x=-a.x;
01203                 a.y=-a.y;
01204                 a.z=-a.z;
01205         }
01206         double d = dot(a,b);
01207         if(d>=1.0) {
01208                 return a;
01209         }
01210         double theta = acos(d);
01211         if(theta==0.0f) { return(a);}
01212         return a*(sin(theta-interp*theta)/sin(theta)) + b*(sin(interp*theta)/sin(theta));
01213 }
01214 
01215 
01216 Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,double alpha) {
01217         return slerp(q0,q1,alpha);
01218 }
01219 
01220 
01221 Quaternion YawPitchRoll( double yaw, double pitch, double roll ) 
01222 {
01223         roll  *= DEG2RAD;
01224         yaw   *= DEG2RAD;
01225         pitch *= DEG2RAD;
01226         return Quaternion(double3(0.0f,0.0f,1.0f),yaw)*Quaternion(double3(1.0f,0.0f,0.0f),pitch)*Quaternion(double3(0.0f,1.0f,0.0f),roll);
01227 }
01228 
01229 double Yaw( const Quaternion& q )
01230 {
01231         static double3 v;
01232         v=q.ydir();
01233         return (v.y==0.0&&v.x==0.0) ? 0.0: atan2(-v.x,v.y)*RAD2DEG;
01234 }
01235 
01236 double Pitch( const Quaternion& q )
01237 {
01238         static double3 v;
01239         v=q.ydir();
01240         return atan2(v.z,sqrt(sqr(v.x)+sqr(v.y)))*RAD2DEG;
01241 }
01242 
01243 double Roll( Quaternion q )
01244 {
01245         q = Quaternion(double3(0.0f,0.0f,1.0f),-Yaw(q)*DEG2RAD)  *q;
01246         q = Quaternion(double3(1.0f,0.0f,0.0f),-Pitch(q)*DEG2RAD)  *q;
01247         return atan2(-q.xdir().z,q.xdir().x)*RAD2DEG;
01248 }
01249 
01250 double Yaw( const double3& v )
01251 {
01252         return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2(-v.x,v.y)*RAD2DEG;
01253 }
01254 
01255 double Pitch( const double3& v )
01256 {
01257         return atan2(v.z,sqrt(sqr(v.x)+sqr(v.y)))*RAD2DEG;
01258 }
01259 
01260 
01261 //------------- Plane --------------
01262 
01263 
01264 void Plane::Transform(const double3 &position, const Quaternion &orientation) {
01265         //   Transforms the plane to the space defined by the 
01266         //   given position/orientation.
01267         static double3 newnormal;
01268         static double3 origin;
01269 
01270         newnormal = Inverse(orientation)*normal;
01271         origin = Inverse(orientation)*(-normal*dist - position);
01272 
01273         normal = newnormal;
01274         dist = -dot(newnormal, origin);
01275 }
01276 
01277 
01278 
01279 
01280 //--------- utility functions -------------
01281 
01282 //        RotationArc()
01283 // Given two vectors v0 and v1 this function
01284 // returns quaternion q where q*v0==v1.
01285 // Routine taken from game programming gems.
01286 Quaternion RotationArc(double3 v0,double3 v1){
01287         static Quaternion q;
01288         v0 = normalize(v0);  // Comment these two lines out if you know its not needed.
01289         v1 = normalize(v1);  // If vector is already unit length then why do it again?
01290         double3  c = cross(v0,v1);
01291         double   d = dot(v0,v1);
01292         if(d<=-1.0f) { return Quaternion(1,0,0,0);} // 180 about x axis
01293         double   s = sqrt((1+d)*2);
01294         q.x = c.x / s;
01295         q.y = c.y / s;
01296         q.z = c.z / s;
01297         q.w = s /2.0f;
01298         return q;
01299 }
01300 
01301 
01302 double4x4 MatrixFromQuatVec(const Quaternion &q, const double3 &v) 
01303 {
01304         // builds a 4x4 transformation matrix based on orientation q and translation v 
01305         double qx2 = q.x*q.x;
01306         double qy2 = q.y*q.y;
01307         double qz2 = q.z*q.z;
01308 
01309         double qxqy = q.x*q.y;
01310         double qxqz = q.x*q.z;
01311         double qxqw = q.x*q.w;
01312         double qyqz = q.y*q.z;
01313         double qyqw = q.y*q.w;
01314         double qzqw = q.z*q.w;
01315 
01316         return double4x4(
01317                 1-2*(qy2+qz2),  
01318                 2*(qxqy+qzqw),  
01319                 2*(qxqz-qyqw),  
01320                 0            ,  
01321                 2*(qxqy-qzqw),  
01322                 1-2*(qx2+qz2),  
01323                 2*(qyqz+qxqw),  
01324                 0            ,  
01325                 2*(qxqz+qyqw),  
01326                 2*(qyqz-qxqw),  
01327                 1-2*(qx2+qy2),  
01328                 0    , 
01329                  v.x ,
01330                  v.y ,
01331                  v.z ,
01332                  1.0f );
01333 }
01334 
01335 
01336 double3 PlaneLineIntersection(const Plane &plane, const double3 &p0, const double3 &p1)
01337 {
01338         // returns the point where the line p0-p1 intersects the plane n&d
01339                                 static double3 dif;
01340                 dif = p1-p0;
01341                                 double dn= dot(plane.normal,dif);
01342                                 double t = -(plane.dist+dot(plane.normal,p0) )/dn;
01343                                 return p0 + (dif*t);
01344 }
01345 
01346 double3 PlaneProject(const Plane &plane, const double3 &point)
01347 {
01348         return point - plane.normal * (dot(point,plane.normal)+plane.dist);
01349 }
01350 
01351 double3 LineProject(const double3 &p0, const double3 &p1, const double3 &a)
01352 {
01353         double3 w;
01354         w = p1-p0;
01355         double t= dot(w,(a-p0)) / (sqr(w.x)+sqr(w.y)+sqr(w.z));
01356         return p0+ w*t;
01357 }
01358 
01359 
01360 double LineProjectTime(const double3 &p0, const double3 &p1, const double3 &a)
01361 {
01362         double3 w;
01363         w = p1-p0;
01364         double t= dot(w,(a-p0)) / (sqr(w.x)+sqr(w.y)+sqr(w.z));
01365         return t;
01366 }
01367 
01368 
01369 
01370 double3 TriNormal(const double3 &v0, const double3 &v1, const double3 &v2)
01371 {
01372         // return the normal of the triangle
01373         // inscribed by v0, v1, and v2
01374         double3 cp=cross(v1-v0,v2-v1);
01375         double m=magnitude(cp);
01376         if(m==0) return double3(1,0,0);
01377         return cp*(1.0f/m);
01378 }
01379 
01380 
01381 
01382 int BoxInside(const double3 &p, const double3 &bmin, const double3 &bmax) 
01383 {
01384         return (p.x >= bmin.x && p.x <=bmax.x && 
01385                         p.y >= bmin.y && p.y <=bmax.y && 
01386                         p.z >= bmin.z && p.z <=bmax.z );
01387 }
01388 
01389 
01390 int BoxIntersect(const double3 &v0, const double3 &v1, const double3 &bmin, const double3 &bmax,double3 *impact)
01391 {
01392         if(BoxInside(v0,bmin,bmax))
01393                 {
01394                                 *impact=v0;
01395                                 return 1;
01396                 }
01397         if(v0.x<=bmin.x && v1.x>=bmin.x) 
01398                 {
01399                 double a = (bmin.x-v0.x)/(v1.x-v0.x);
01400                 //v.x = bmin.x;
01401                 double vy =  (1-a) *v0.y + a*v1.y;
01402                 double vz =  (1-a) *v0.z + a*v1.z;
01403                 if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z) 
01404                                 {
01405                         impact->x = bmin.x;
01406                         impact->y = vy;
01407                         impact->z = vz;
01408                         return 1;
01409                 }
01410         }
01411         else if(v0.x >= bmax.x  &&  v1.x <= bmax.x) 
01412                 {
01413                 double a = (bmax.x-v0.x)/(v1.x-v0.x);
01414                 //v.x = bmax.x;
01415                 double vy =  (1-a) *v0.y + a*v1.y;
01416                 double vz =  (1-a) *v0.z + a*v1.z;
01417                 if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z) 
01418                                 {
01419                         impact->x = bmax.x;
01420                         impact->y = vy;
01421                         impact->z = vz;
01422                         return 1;
01423                 }
01424         }
01425         if(v0.y<=bmin.y && v1.y>=bmin.y) 
01426                 {
01427                 double a = (bmin.y-v0.y)/(v1.y-v0.y);
01428                 double vx =  (1-a) *v0.x + a*v1.x;
01429                 //v.y = bmin.y;
01430                 double vz =  (1-a) *v0.z + a*v1.z;
01431                 if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z) 
01432                                 {
01433                         impact->x = vx;
01434                         impact->y = bmin.y;
01435                         impact->z = vz;
01436                         return 1;
01437                 }
01438         }
01439         else if(v0.y >= bmax.y  &&  v1.y <= bmax.y) 
01440                 {
01441                 double a = (bmax.y-v0.y)/(v1.y-v0.y);
01442                 double vx =  (1-a) *v0.x + a*v1.x;
01443                 // vy = bmax.y;
01444                 double vz =  (1-a) *v0.z + a*v1.z;
01445                 if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z) 
01446                                 {
01447                         impact->x = vx;
01448                         impact->y = bmax.y;
01449                         impact->z = vz;
01450                         return 1;
01451                 }
01452         }
01453         if(v0.z<=bmin.z && v1.z>=bmin.z) 
01454                 {
01455                 double a = (bmin.z-v0.z)/(v1.z-v0.z);
01456                 double vx =  (1-a) *v0.x + a*v1.x;
01457                 double vy =  (1-a) *v0.y + a*v1.y;
01458                 // v.z = bmin.z;
01459                 if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x) 
01460                                 {
01461                         impact->x = vx;
01462                         impact->y = vy;
01463                         impact->z = bmin.z;
01464                         return 1;
01465                 }
01466         }
01467         else if(v0.z >= bmax.z  &&  v1.z <= bmax.z) 
01468                 {
01469                 double a = (bmax.z-v0.z)/(v1.z-v0.z);
01470                 double vx =  (1-a) *v0.x + a*v1.x;
01471                 double vy =  (1-a) *v0.y + a*v1.y;
01472                 // v.z = bmax.z;
01473                 if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x) 
01474                                 {
01475                         impact->x = vx;
01476                         impact->y = vy;
01477                         impact->z = bmax.z;
01478                         return 1;
01479                 }
01480         }
01481         return 0;
01482 }
01483 
01484 
01485 double DistanceBetweenLines(const double3 &ustart, const double3 &udir, const double3 &vstart, const double3 &vdir, double3 *upoint, double3 *vpoint)
01486 {
01487         static double3 cp;
01488         cp = normalize(cross(udir,vdir));
01489 
01490         double distu = -dot(cp,ustart);
01491         double distv = -dot(cp,vstart);
01492         double dist = (double)fabs(distu-distv);
01493         if(upoint) 
01494                 {
01495                 Plane plane;
01496                 plane.normal = normalize(cross(vdir,cp));
01497                 plane.dist = -dot(plane.normal,vstart);
01498                 *upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
01499         }
01500         if(vpoint) 
01501                 {
01502                 Plane plane;
01503                 plane.normal = normalize(cross(udir,cp));
01504                 plane.dist = -dot(plane.normal,ustart);
01505                 *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
01506         }
01507         return dist;
01508 }
01509 
01510 
01511 Quaternion VirtualTrackBall(const double3 &cop, const double3 &cor, const double3 &dir1, const double3 &dir2) 
01512 {
01513         // routine taken from game programming gems.
01514         // Implement track ball functionality to spin stuf on the screen
01515         //  cop   center of projection
01516         //  cor   center of rotation
01517         //  dir1  old mouse direction 
01518         //  dir2  new mouse direction
01519         // pretend there is a sphere around cor.  Then find the points
01520         // where dir1 and dir2 intersect that sphere.  Find the
01521         // rotation that takes the first point to the second.
01522         double m;
01523         // compute plane 
01524         double3 nrml = cor - cop;
01525         double fudgefactor = 1.0f/(magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop
01526         nrml = normalize(nrml);
01527         double dist = -dot(nrml,cor);
01528         double3 u= PlaneLineIntersection(Plane(nrml,dist),cop,cop+dir1);
01529         u=u-cor;
01530         u=u*fudgefactor;
01531         m= magnitude(u);
01532         if(m>1) 
01533                 {
01534                                 u/=m;
01535                 }
01536         else 
01537                 {
01538                 u=u - (nrml * sqrt(1-m*m));
01539         }
01540         double3 v= PlaneLineIntersection(Plane(nrml,dist),cop,cop+dir2);
01541         v=v-cor;
01542         v=v*fudgefactor;
01543         m= magnitude(v);
01544         if(m>1) 
01545                 {
01546                                 v/=m;
01547                 }
01548         else 
01549                 {
01550                 v=v - (nrml * sqrt(1-m*m));
01551         }
01552         return RotationArc(u,v);
01553 }
01554 
01555 
01556 int countpolyhit=0;
01557 int PolyHit(const double3 *vert, const int n, const double3 &v0, const double3 &v1, double3 *impact, double3 *normal)
01558 {
01559         countpolyhit++;
01560         int i;
01561         double3 nrml(0,0,0);
01562         for(i=0;i<n;i++) 
01563                 {
01564                 int i1=(i+1)%n;
01565                 int i2=(i+2)%n;
01566                 nrml = nrml + cross(vert[i1]-vert[i],vert[i2]-vert[i1]);
01567         }
01568 
01569         double m = magnitude(nrml);
01570         if(m==0.0)
01571                 {
01572                                 return 0;
01573                 }
01574         nrml = nrml * (1.0f/m);
01575         double dist = -dot(nrml,vert[0]);
01576         double d0,d1;
01577         if((d0=dot(v0,nrml)+dist) <0  ||  (d1=dot(v1,nrml)+dist) >0) 
01578                 {        
01579                                 return 0;
01580                 }
01581 
01582         static double3 the_point; 
01583         // By using the cached plane distances d0 and d1
01584         // we can optimize the following:
01585         //     the_point = planelineintersection(nrml,dist,v0,v1);
01586         double a = d0/(d0-d1);
01587         the_point = v0*(1-a) + v1*a;
01588 
01589 
01590         int inside=1;
01591         for(int j=0;inside && j<n;j++) 
01592                 {
01593                         // let inside = 0 if outside
01594                         double3 pp1,pp2,side;
01595                         pp1 = vert[j] ;
01596                         pp2 = vert[(j+1)%n];
01597                         side = cross((pp2-pp1),(the_point-pp1));
01598                         inside = (dot(nrml,side) >= 0.0);
01599         }
01600         if(inside) 
01601                 {
01602                 if(normal){*normal=nrml;}
01603                 if(impact){*impact=the_point;}
01604         }
01605         return inside;
01606 }
01607 
01608 //****************************************************
01609 // HULL.H source code goes here
01610 //****************************************************
01611 class PHullResult
01612 {
01613 public:
01614 
01615         PHullResult(void)
01616         {
01617                 mVcount = 0;
01618                 mIndexCount = 0;
01619                 mFaceCount = 0;
01620                 mVertices = 0;
01621                 mIndices  = 0;
01622         }
01623 
01624         unsigned int mVcount;
01625         unsigned int mIndexCount;
01626         unsigned int mFaceCount;
01627         double       *mVertices;
01628         unsigned int *mIndices;
01629 };
01630 
01631 bool ComputeHull(unsigned int vcount,const double *vertices,PHullResult &result,unsigned int maxverts,double inflate);
01632 void ReleaseHull(PHullResult &result);
01633 
01634 //*****************************************************
01635 // HULL.cpp source code goes here
01636 //*****************************************************
01637 
01638 
01639 #define REAL3 double3
01640 #define REAL  double
01641 
01642 #define COPLANAR   (0)
01643 #define UNDER      (1)
01644 #define OVER       (2)
01645 #define SPLIT      (OVER|UNDER)
01646 #define PAPERWIDTH (0.001f)
01647 #define VOLUME_EPSILON (1e-20f)
01648 
01649 double planetestepsilon = PAPERWIDTH;
01650 
01651 #if STANDALONE
01652 class ConvexH 
01653 #else
01654 class ConvexH : public NxFoundation::NxAllocateable
01655 #endif
01656 {
01657   public:
01658         class HalfEdge
01659         {
01660           public:
01661                 short ea;         // the other half of the edge (index into edges list)
01662                 unsigned char v;  // the vertex at the start of this edge (index into vertices list)
01663                 unsigned char p;  // the facet on which this edge lies (index into facets list)
01664                 HalfEdge(){}
01665                 HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){}
01666         };
01667         Array<REAL3> vertices;
01668         Array<HalfEdge> edges;
01669         Array<Plane>  facets;
01670         ConvexH(int vertices_size,int edges_size,int facets_size);
01671 };
01672 
01673 typedef ConvexH::HalfEdge HalfEdge;
01674 
01675 ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
01676         :vertices(vertices_size)
01677         ,edges(edges_size)
01678         ,facets(facets_size)
01679 {
01680         vertices.count=vertices_size;
01681         edges.count   = edges_size;
01682         facets.count  = facets_size;
01683 }
01684 
01685 ConvexH *ConvexHDup(ConvexH *src)
01686 {
01687 #if STANDALONE
01688         ConvexH *dst = new ConvexH(src->vertices.count,src->edges.count,src->facets.count);
01689 #else
01690         ConvexH *dst = NX_NEW_MEM(ConvexH(src->vertices.count,src->edges.count,src->facets.count), CONVEX_TEMP);
01691 #endif
01692 
01693         memcpy(dst->vertices.element,src->vertices.element,sizeof(double3)*src->vertices.count);
01694         memcpy(dst->edges.element,src->edges.element,sizeof(HalfEdge)*src->edges.count);
01695         memcpy(dst->facets.element,src->facets.element,sizeof(Plane)*src->facets.count);
01696         return dst;
01697 }
01698 
01699 
01700 int PlaneTest(const Plane &p, const REAL3 &v) {
01701         REAL a  = dot(v,p.normal)+p.dist;
01702         int   flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
01703         return flag;
01704 }
01705 
01706 int SplitTest(ConvexH &convex,const Plane &plane) {
01707         int flag=0;
01708         for(int i=0;i<convex.vertices.count;i++) {
01709                 flag |= PlaneTest(plane,convex.vertices[i]);
01710         }
01711         return flag;
01712 }
01713 
01714 class VertFlag 
01715 {
01716 public:
01717         unsigned char planetest;
01718         unsigned char junk;
01719         unsigned char undermap;
01720         unsigned char overmap;
01721 };
01722 class EdgeFlag 
01723 {
01724 public:
01725         unsigned char planetest;
01726         unsigned char fixes;
01727         short undermap;
01728         short overmap;
01729 };
01730 class PlaneFlag 
01731 {
01732 public:
01733         unsigned char undermap;
01734         unsigned char overmap;
01735 };
01736 class Coplanar{
01737 public:
01738         unsigned short ea;
01739         unsigned char v0;
01740         unsigned char v1;
01741 };
01742 
01743 int AssertIntact(ConvexH &convex) {
01744         int i;
01745         int estart=0;
01746         for(i=0;i<convex.edges.count;i++) {
01747                 if(convex.edges[estart].p!= convex.edges[i].p) {
01748                         estart=i;
01749                 }
01750                 int inext = i+1;
01751                 if(inext>= convex.edges.count || convex.edges[inext].p != convex.edges[i].p) {
01752                         inext = estart;
01753                 }
01754                 assert(convex.edges[inext].p == convex.edges[i].p);
01755                 HalfEdge &edge = convex.edges[i];
01756                 int nb = convex.edges[i].ea;
01757                 assert(nb!=255);
01758                 if(nb==255 || nb==-1) return 0;
01759                 assert(nb!=-1);
01760                 assert(i== convex.edges[nb].ea);
01761         }
01762         for(i=0;i<convex.edges.count;i++) {
01763                 assert(COPLANAR==PlaneTest(convex.facets[convex.edges[i].p],convex.vertices[convex.edges[i].v]));
01764                 if(COPLANAR!=PlaneTest(convex.facets[convex.edges[i].p],convex.vertices[convex.edges[i].v])) return 0;
01765                 if(convex.edges[estart].p!= convex.edges[i].p) {
01766                         estart=i;
01767                 }
01768                 int i1 = i+1;
01769                 if(i1>= convex.edges.count || convex.edges[i1].p != convex.edges[i].p) {
01770                         i1 = estart;
01771                 }
01772                 int i2 = i1+1;
01773                 if(i2>= convex.edges.count || convex.edges[i2].p != convex.edges[i].p) {
01774                         i2 = estart;
01775                 }
01776                 if(i==i2) continue; // i sliced tangent to an edge and created 2 meaningless edges
01777                 REAL3 localnormal = TriNormal(convex.vertices[convex.edges[i ].v],
01778                                                    convex.vertices[convex.edges[i1].v],
01779                                                    convex.vertices[convex.edges[i2].v]);
01780                 //assert(dot(localnormal,convex.facets[convex.edges[i].p].normal)>0);//Commented out on Stan Melax' advice
01781                 if(dot(localnormal,convex.facets[convex.edges[i].p].normal)<=0)return 0;
01782         }
01783         return 1;
01784 }
01785 
01786 // back to back quads
01787 ConvexH *test_btbq()
01788 {
01789 
01790 #if STANDALONE
01791         ConvexH *convex = new ConvexH(4,8,2);
01792 #else
01793         ConvexH *convex = NX_NEW_MEM(ConvexH(4,8,2), CONVEX_TEMP);
01794 #endif
01795 
01796         convex->vertices[0] = REAL3(0,0,0);
01797         convex->vertices[1] = REAL3(1,0,0);
01798         convex->vertices[2] = REAL3(1,1,0);
01799         convex->vertices[3] = REAL3(0,1,0);
01800         convex->facets[0] = Plane(REAL3(0,0,1),0);
01801         convex->facets[1] = Plane(REAL3(0,0,-1),0);
01802         convex->edges[0]  = HalfEdge(7,0,0);
01803         convex->edges[1]  = HalfEdge(6,1,0);
01804         convex->edges[2]  = HalfEdge(5,2,0);
01805         convex->edges[3]  = HalfEdge(4,3,0);
01806 
01807         convex->edges[4]  = HalfEdge(3,0,1);
01808         convex->edges[5]  = HalfEdge(2,3,1);
01809         convex->edges[6]  = HalfEdge(1,2,1);
01810         convex->edges[7]  = HalfEdge(0,1,1);
01811         AssertIntact(*convex);
01812         return convex;
01813 }
01814 
01815 ConvexH *test_cube()
01816 {
01817 #if STANDALONE
01818         ConvexH *convex = new ConvexH(8,24,6);
01819 #else
01820         ConvexH *convex = NX_NEW_MEM(ConvexH(8,24,6), CONVEX_TEMP);
01821 #endif
01822         convex->vertices[0] = REAL3(0,0,0);
01823         convex->vertices[1] = REAL3(0,0,1);
01824         convex->vertices[2] = REAL3(0,1,0);
01825         convex->vertices[3] = REAL3(0,1,1);
01826         convex->vertices[4] = REAL3(1,0,0);
01827         convex->vertices[5] = REAL3(1,0,1);
01828         convex->vertices[6] = REAL3(1,1,0);
01829         convex->vertices[7] = REAL3(1,1,1);
01830 
01831         convex->facets[0] = Plane(REAL3(-1,0,0),0);
01832         convex->facets[1] = Plane(REAL3(1,0,0),-1);
01833         convex->facets[2] = Plane(REAL3(0,-1,0),0);
01834         convex->facets[3] = Plane(REAL3(0,1,0),-1);
01835         convex->facets[4] = Plane(REAL3(0,0,-1),0);
01836         convex->facets[5] = Plane(REAL3(0,0,1),-1);
01837 
01838         convex->edges[0 ] = HalfEdge(11,0,0);
01839         convex->edges[1 ] = HalfEdge(23,1,0);
01840         convex->edges[2 ] = HalfEdge(15,3,0);
01841         convex->edges[3 ] = HalfEdge(16,2,0);
01842 
01843         convex->edges[4 ] = HalfEdge(13,6,1);
01844         convex->edges[5 ] = HalfEdge(21,7,1);
01845         convex->edges[6 ] = HalfEdge( 9,5,1);
01846         convex->edges[7 ] = HalfEdge(18,4,1);
01847 
01848         convex->edges[8 ] = HalfEdge(19,0,2);
01849         convex->edges[9 ] = HalfEdge( 6,4,2);
01850         convex->edges[10] = HalfEdge(20,5,2);
01851         convex->edges[11] = HalfEdge( 0,1,2);
01852 
01853         convex->edges[12] = HalfEdge(22,3,3);
01854         convex->edges[13] = HalfEdge( 4,7,3);
01855         convex->edges[14] = HalfEdge(17,6,3);
01856         convex->edges[15] = HalfEdge( 2,2,3);
01857 
01858         convex->edges[16] = HalfEdge( 3,0,4);
01859         convex->edges[17] = HalfEdge(14,2,4);
01860         convex->edges[18] = HalfEdge( 7,6,4);
01861         convex->edges[19] = HalfEdge( 8,4,4);
01862         
01863         convex->edges[20] = HalfEdge(10,1,5);
01864         convex->edges[21] = HalfEdge( 5,5,5);
01865         convex->edges[22] = HalfEdge(12,7,5);
01866         convex->edges[23] = HalfEdge( 1,3,5);
01867 
01868         
01869         return convex;
01870 }
01871 ConvexH *ConvexHMakeCube(const REAL3 &bmin, const REAL3 &bmax) {
01872         ConvexH *convex = test_cube();
01873         convex->vertices[0] = REAL3(bmin.x,bmin.y,bmin.z);
01874         convex->vertices[1] = REAL3(bmin.x,bmin.y,bmax.z);
01875         convex->vertices[2] = REAL3(bmin.x,bmax.y,bmin.z);
01876         convex->vertices[3] = REAL3(bmin.x,bmax.y,bmax.z);
01877         convex->vertices[4] = REAL3(bmax.x,bmin.y,bmin.z);
01878         convex->vertices[5] = REAL3(bmax.x,bmin.y,bmax.z);
01879         convex->vertices[6] = REAL3(bmax.x,bmax.y,bmin.z);
01880         convex->vertices[7] = REAL3(bmax.x,bmax.y,bmax.z);
01881 
01882         convex->facets[0] = Plane(REAL3(-1,0,0), bmin.x);
01883         convex->facets[1] = Plane(REAL3(1,0,0), -bmax.x);
01884         convex->facets[2] = Plane(REAL3(0,-1,0), bmin.y);
01885         convex->facets[3] = Plane(REAL3(0,1,0), -bmax.y);
01886         convex->facets[4] = Plane(REAL3(0,0,-1), bmin.z);
01887         convex->facets[5] = Plane(REAL3(0,0,1), -bmax.z);
01888         return convex;
01889 }
01890 ConvexH *ConvexHCrop(ConvexH &convex,const Plane &slice)
01891 {
01892         int i;
01893         int vertcountunder=0;
01894         int vertcountover =0;
01895         int edgecountunder=0;
01896         int edgecountover =0;
01897         int planecountunder=0;
01898         int planecountover =0;
01899         static Array<int> vertscoplanar;  // existing vertex members of convex that are coplanar
01900         vertscoplanar.count=0;
01901         static Array<int> edgesplit;  // existing edges that members of convex that cross the splitplane
01902         edgesplit.count=0;
01903 
01904         assert(convex.edges.count<480);
01905 
01906         EdgeFlag  edgeflag[512];
01907         VertFlag  vertflag[256];
01908         PlaneFlag planeflag[128];
01909         HalfEdge  tmpunderedges[512];
01910         Plane     tmpunderplanes[128];
01911         Coplanar coplanaredges[512];
01912         int coplanaredges_num=0;
01913 
01914         Array<REAL3> createdverts;
01915         // do the side-of-plane tests
01916         for(i=0;i<convex.vertices.count;i++) {
01917                 vertflag[i].planetest = PlaneTest(slice,convex.vertices[i]);
01918                 if(vertflag[i].planetest == COPLANAR) {
01919                         // ? vertscoplanar.Add(i);
01920                         vertflag[i].undermap = vertcountunder++;
01921                         vertflag[i].overmap  = vertcountover++;
01922                 }
01923                 else if(vertflag[i].planetest == UNDER) {
01924                         vertflag[i].undermap = vertcountunder++;
01925                 }
01926                 else {
01927                         assert(vertflag[i].planetest == OVER);
01928                         vertflag[i].overmap  = vertcountover++;
01929                         vertflag[i].undermap = (unsigned char)-1; // for debugging purposes
01930                 }
01931         }
01932         int vertcountunderold = vertcountunder; // for debugging only
01933 
01934         int under_edge_count =0;
01935         int underplanescount=0;
01936         int e0=0;
01937 
01938         for(int currentplane=0; currentplane<convex.facets.count; currentplane++) {
01939                 int estart =e0;
01940                 int enextface;
01941                 int planeside = 0;
01942                 int e1 = e0+1;
01943                 int eus=-1;
01944                 int ecop=-1;
01945                 int vout=-1;
01946                 int vin =-1;
01947                 int coplanaredge = -1;
01948                 do{
01949 
01950                         if(e1 >= convex.edges.count || convex.edges[e1].p!=currentplane) {
01951                                 enextface = e1;
01952                                 e1=estart;
01953                         }
01954                         HalfEdge &edge0 = convex.edges[e0];
01955                         HalfEdge &edge1 = convex.edges[e1];
01956                         HalfEdge &edgea = convex.edges[edge0.ea];
01957 
01958 
01959                         planeside |= vertflag[edge0.v].planetest;
01960                         //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest)  == COPLANAR) {
01961                         //      assert(ecop==-1);
01962                         //      ecop=e;
01963                         //}
01964 
01965 
01966                         if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == OVER){
01967                                 // both endpoints over plane
01968                                 edgeflag[e0].undermap  = -1;
01969                         }
01970                         else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest)  == UNDER) {
01971                                 // at least one endpoint under, the other coplanar or under
01972                                 
01973                                 edgeflag[e0].undermap = under_edge_count;
01974                                 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
01975                                 tmpunderedges[under_edge_count].p = underplanescount;
01976                                 if(edge0.ea < e0) {
01977                                         // connect the neighbors
01978                                         assert(edgeflag[edge0.ea].undermap !=-1);
01979                                         tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
01980                                         tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
01981                                 }
01982                                 under_edge_count++;
01983                         }
01984                         else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest)  == COPLANAR) {
01985                                 // both endpoints coplanar 
01986                                 // must check a 3rd point to see if UNDER
01987                                 int e2 = e1+1; 
01988                                 if(e2>=convex.edges.count || convex.edges[e2].p!=currentplane) {
01989                                         e2 = estart;
01990                                 }
01991                                 assert(convex.edges[e2].p==currentplane);
01992                                 HalfEdge &edge2 = convex.edges[e2];
01993                                 if(vertflag[edge2.v].planetest==UNDER) {
01994                                         
01995                                         edgeflag[e0].undermap = under_edge_count;
01996                                         tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
01997                                         tmpunderedges[under_edge_count].p = underplanescount;
01998                                         tmpunderedges[under_edge_count].ea = -1;
01999                                         // make sure this edge is added to the "coplanar" list
02000                                         coplanaredge = under_edge_count;
02001                                         vout = vertflag[edge0.v].undermap;
02002                                         vin  = vertflag[edge1.v].undermap;
02003                                         under_edge_count++;
02004                                 }
02005                                 else {
02006                                         edgeflag[e0].undermap = -1;
02007                                 }
02008                         }
02009                         else if(vertflag[edge0.v].planetest == UNDER && vertflag[edge1.v].planetest == OVER) {
02010                                 // first is under 2nd is over 
02011                                 
02012                                 edgeflag[e0].undermap = under_edge_count;
02013                                 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
02014                                 tmpunderedges[under_edge_count].p = underplanescount;
02015                                 if(edge0.ea < e0) {
02016                                         assert(edgeflag[edge0.ea].undermap !=-1);
02017                                         // connect the neighbors
02018                                         tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
02019                                         tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
02020                                         vout = tmpunderedges[edgeflag[edge0.ea].undermap].v;
02021                                 }
02022                                 else {
02023                                         Plane &p0 = convex.facets[edge0.p];
02024                                         Plane &pa = convex.facets[edgea.p];
02025                                         createdverts.Add(ThreePlaneIntersection(p0,pa,slice));
02026                                         //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])));
02027                                         //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]));
02028                                         vout = vertcountunder++;
02029                                 }
02030                                 under_edge_count++;
02032                                 // wheter or not we know v-in yet.  ok i;ll try this now:
02033                                 tmpunderedges[under_edge_count].v = vout;
02034                                 tmpunderedges[under_edge_count].p = underplanescount;
02035                                 tmpunderedges[under_edge_count].ea = -1;
02036                                 coplanaredge = under_edge_count;
02037                                 under_edge_count++;
02038 
02039                                 if(vin!=-1) {
02040                                         // we previously processed an edge  where we came under
02041                                         // now we know about vout as well
02042 
02043                                         // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
02044                                 }
02045 
02046                         }
02047                         else if(vertflag[edge0.v].planetest == COPLANAR && vertflag[edge1.v].planetest == OVER) {
02048                                 // first is coplanar 2nd is over 
02049                                 
02050                                 edgeflag[e0].undermap = -1;
02051                                 vout = vertflag[edge0.v].undermap;
02052                                 // I hate this but i have to make sure part of this face is UNDER before ouputting this vert
02053                                 int k=estart;
02054                                 assert(edge0.p == currentplane);
02055                                 while(!(planeside&UNDER) && k<convex.edges.count && convex.edges[k].p==edge0.p) {
02056                                         planeside |= vertflag[convex.edges[k].v].planetest;
02057                                         k++;
02058                                 }
02059                                 if(planeside&UNDER){
02060                                         tmpunderedges[under_edge_count].v = vout;
02061                                         tmpunderedges[under_edge_count].p = underplanescount;
02062                                         tmpunderedges[under_edge_count].ea = -1;
02063                                         coplanaredge = under_edge_count; // hmmm should make a note of the edge # for later on
02064                                         under_edge_count++;
02065                                         
02066                                 }
02067                         }
02068                         else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == UNDER) {
02069                                 // first is over next is under 
02070                                 // new vertex!!!
02071                                 if (vin!=-1) return NULL;
02072                                 if(e0<edge0.ea) {
02073                                         Plane &p0 = convex.facets[edge0.p];
02074                                         Plane &pa = convex.facets[edgea.p];
02075                                         createdverts.Add(ThreePlaneIntersection(p0,pa,slice));
02076                                         //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]));
02077                                         //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])));
02078                                         vin = vertcountunder++;
02079                                 }
02080                                 else {
02081                                         // find the new vertex that was created by edge[edge0.ea]
02082                                         int nea = edgeflag[edge0.ea].undermap;
02083                                         assert(tmpunderedges[nea].p==tmpunderedges[nea+1].p);
02084                                         vin = tmpunderedges[nea+1].v;
02085                                         assert(vin < vertcountunder);
02086                                         assert(vin >= vertcountunderold);   // for debugging only
02087                                 }
02088                                 if(vout!=-1) {
02089                                         // we previously processed an edge  where we went over
02090                                         // now we know vin too
02091                                         // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
02092                                 }
02093                                 // output edge
02094                                 tmpunderedges[under_edge_count].v = vin;
02095                                 tmpunderedges[under_edge_count].p = underplanescount;
02096                                 edgeflag[e0].undermap = under_edge_count;
02097                                 if(e0>edge0.ea) {
02098                                         assert(edgeflag[edge0.ea].undermap !=-1);
02099                                         // connect the neighbors
02100                                         tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
02101                                         tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
02102                                 }
02103                                 assert(edgeflag[e0].undermap == under_edge_count);
02104                                 under_edge_count++;
02105                         }
02106                         else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == COPLANAR) {
02107                                 // first is over next is coplanar 
02108                                 
02109                                 edgeflag[e0].undermap = -1;
02110                                 vin = vertflag[edge1.v].undermap;
02111                                 if (vin==-1) return NULL;
02112                                 if(vout!=-1) {
02113                                         // we previously processed an edge  where we came under
02114                                         // now we know both endpoints
02115                                         // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
02116                                 }
02117 
02118                         }
02119                         else {
02120                                 assert(0);
02121                         }
02122                         
02123 
02124                         e0=e1;
02125                         e1++; // do the modulo at the beginning of the loop
02126 
02127                 } while(e0!=estart) ;
02128                 e0 = enextface;
02129                 if(planeside&UNDER) {
02130                         planeflag[currentplane].undermap = underplanescount;
02131                         tmpunderplanes[underplanescount] = convex.facets[currentplane];
02132                         underplanescount++;
02133                 }
02134                 else {
02135                         planeflag[currentplane].undermap = 0;
02136                 }
02137                 if(vout>=0 && (planeside&UNDER)) {
02138                         assert(vin>=0);
02139                         assert(coplanaredge>=0);
02140                         assert(coplanaredge!=511);
02141                         coplanaredges[coplanaredges_num].ea = coplanaredge;
02142                         coplanaredges[coplanaredges_num].v0 = vin;
02143                         coplanaredges[coplanaredges_num].v1 = vout;
02144                         coplanaredges_num++;
02145                 }
02146         }
02147 
02148         // add the new plane to the mix:
02149         if(coplanaredges_num>0) {
02150                 tmpunderplanes[underplanescount++]=slice;
02151         }
02152         for(i=0;i<coplanaredges_num-1;i++) {
02153                 if(coplanaredges[i].v1 != coplanaredges[i+1].v0) {
02154                         int j = 0;
02155                         for(j=i+2;j<coplanaredges_num;j++) {
02156                                 if(coplanaredges[i].v1 == coplanaredges[j].v0) {
02157                                         Coplanar tmp = coplanaredges[i+1];
02158                                         coplanaredges[i+1] = coplanaredges[j];
02159                                         coplanaredges[j] = tmp;
02160                                         break;
02161                                 }
02162                         }
02163                         if(j>=coplanaredges_num)
02164                         {
02165                                 // assert(j<coplanaredges_num);
02166                                 return NULL;
02167                         }
02168                 }
02169         }
02170 #if STANDALONE
02171         ConvexH *punder = new ConvexH(vertcountunder,under_edge_count+coplanaredges_num,underplanescount);
02172 #else
02173         ConvexH *punder = NX_NEW_MEM(ConvexH(vertcountunder,under_edge_count+coplanaredges_num,underplanescount), CONVEX_TEMP);
02174 #endif
02175 
02176         ConvexH &under = *punder;
02177         int k=0;
02178         for(i=0;i<convex.vertices.count;i++) {
02179                 if(vertflag[i].planetest != OVER){
02180                         under.vertices[k++] = convex.vertices[i];
02181                 }
02182         }
02183         i=0;
02184         while(k<vertcountunder) {
02185                 under.vertices[k++] = createdverts[i++];
02186         }
02187         assert(i==createdverts.count);
02188 
02189         for(i=0;i<coplanaredges_num;i++) {
02190                 under.edges[under_edge_count+i].p  = underplanescount-1;
02191                 under.edges[under_edge_count+i].ea = coplanaredges[i].ea;
02192                 tmpunderedges[coplanaredges[i].ea].ea = under_edge_count+i;
02193                 under.edges[under_edge_count+i].v  = coplanaredges[i].v0;
02194         }
02195 
02196         memcpy(under.edges.element,tmpunderedges,sizeof(HalfEdge)*under_edge_count);
02197         memcpy(under.facets.element,tmpunderplanes,sizeof(Plane)*underplanescount);
02198         return punder;
02199 }
02200 
02201 
02202 double minadjangle = 3.0f;  // in degrees  - result wont have two adjacent facets within this angle of each other.
02203 static int candidateplane(Plane *planes,int planes_count,ConvexH *convex,double epsilon)
02204 {
02205         int p =-1;
02206         REAL md=0;
02207         int i,j;
02208         double maxdot_minang = cos(DEG2RAD*minadjangle);
02209         for(i=0;i<planes_count;i++)
02210         {
02211                 double d=0;
02212                 double dmax=0;
02213                 double dmin=0;
02214                 for(j=0;j<convex->vertices.count;j++)
02215                 {
02216                         dmax = Max(dmax,dot(convex->vertices[j],planes[i].normal)+planes[i].dist);
02217                         dmin = Min(dmin,dot(convex->vertices[j],planes[i].normal)+planes[i].dist);
02218                 }
02219                 double dr = dmax-dmin;
02220                 if(dr<planetestepsilon) dr=1.0f; // shouldn't happen.
02221                 d = dmax /dr;
02222                 if(d<=md) continue;
02223                 for(j=0;j<convex->facets.count;j++)
02224                 {
02225                         if(planes[i]==convex->facets[j]) 
02226                         {
02227                                 d=0;continue;
02228                         }
02229                         if(dot(planes[i].normal,convex->facets[j].normal)>maxdot_minang)
02230                         {
02231                                 for(int k=0;k<convex->edges.count;k++)
02232                                 {
02233                                         if(convex->edges[k].p!=j) continue;
02234                                         if(dot(convex->vertices[convex->edges[k].v],planes[i].normal)+planes[i].dist<0)
02235                                         {
02236                                                 d=0; // so this plane wont get selected.
02237                                                 break;
02238                                         }
02239                                 }
02240                         }
02241                 }
02242                 if(d>md)
02243                 {
02244                         p=i;
02245                         md=d;
02246                 }
02247         }
02248         return (md>epsilon)?p:-1;
02249 }
02250 
02251 
02252 
02253 template<class T>
02254 inline int maxdir(const T *p,int count,const T &dir)
02255 {
02256         assert(count);
02257         int m=0;
02258         for(int i=1;i<count;i++)
02259         {
02260                 if(dot(p[i],dir)>dot(p[m],dir)) m=i;
02261         }
02262         return m;
02263 }
02264 
02265 
02266 template<class T>
02267 int maxdirfiltered(const T *p,int count,const T &dir,Array<int> &allow)
02268 {
02269         assert(count);
02270         int m=-1;
02271         for(int i=0;i<count;i++) if(allow[i])
02272         {
02273                 if(m==-1 || dot(p[i],dir)>dot(p[m],dir)) m=i;
02274         }
02275         assert(m!=-1);
02276         return m;
02277 } 
02278 
02279 double3 orth(const double3 &v)
02280 {
02281         double3 a=cross(v,double3(0,0,1));
02282         double3 b=cross(v,double3(0,1,0));
02283         return normalize((magnitude(a)>magnitude(b))?a:b);
02284 }
02285 
02286 
02287 template<class T>
02288 int maxdirsterid(const T *p,int count,const T &dir,Array<int> &allow)
02289 {
02290         int m=-1;
02291         while(m==-1)
02292         {
02293                 m = maxdirfiltered(p,count,dir,allow);
02294                 if(allow[m]==3) return m;
02295                 T u = orth(dir);
02296                 T v = cross(u,dir);
02297                 int ma=-1;
02298                 for(double x = 0.0f ; x<= 360.0f ; x+= 45.0f)
02299                 {
02300                         double s = sin(DEG2RAD*(x));
02301                         double c = cos(DEG2RAD*(x));
02302                         int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow);
02303                         if(ma==m && mb==m)
02304                         {
02305                                 allow[m]=3;
02306                                 return m;
02307                         }
02308                         if(ma!=-1 && ma!=mb)  // Yuck - this is really ugly
02309                         {
02310                                 int mc = ma;
02311                                 for(double xx = x-40.0f ; xx <= x ; xx+= 5.0f)
02312                                 {
02313                                         double s = sin(DEG2RAD*(xx));
02314                                         double c = cos(DEG2RAD*(xx));
02315                                         int md = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow);
02316                                         if(mc==m && md==m)
02317                                         {
02318                                                 allow[m]=3;
02319                                                 return m;
02320                                         }
02321                                         mc=md;
02322                                 }
02323                         }
02324                         ma=mb;
02325                 }
02326                 allow[m]=0;
02327                 m=-1;
02328         }
02329         assert(0);
02330         return m;
02331 } 
02332 
02333 
02334 
02335 
02336 int operator ==(const int3 &a,const int3 &b) 
02337 {
02338         for(int i=0;i<3;i++) 
02339         {
02340                 if(a[i]!=b[i]) return 0;
02341         }
02342         return 1;
02343 }
02344 
02345 int3 roll3(int3 a) 
02346 {
02347         int tmp=a[0];
02348         a[0]=a[1];
02349         a[1]=a[2];
02350         a[2]=tmp;
02351         return a;
02352 }
02353 int isa(const int3 &a,const int3 &b) 
02354 {
02355         return ( a==b || roll3(a)==b || a==roll3(b) );
02356 }
02357 int b2b(const int3 &a,const int3 &b) 
02358 {
02359         return isa(a,int3(b[2],b[1],b[0]));
02360 }
02361 int above(double3* vertices,const int3& t, const double3 &p, double epsilon) 
02362 {
02363         double3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
02364         return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
02365 }
02366 int hasedge(const int3 &t, int a,int b)
02367 {
02368         for(int i=0;i<3;i++)
02369         {
02370                 int i1= (i+1)%3;
02371                 if(t[i]==a && t[i1]==b) return 1;
02372         }
02373         return 0;
02374 }
02375 int hasvert(const int3 &t, int v)
02376 {
02377         return (t[0]==v || t[1]==v || t[2]==v) ;
02378 }
02379 int shareedge(const int3 &a,const int3 &b)
02380 {
02381         int i;
02382         for(i=0;i<3;i++)
02383         {
02384                 int i1= (i+1)%3;
02385                 if(hasedge(a,b[i1],b[i])) return 1;
02386         }
02387         return 0;
02388 }
02389 
02390 class Tri;
02391 
02392 static Array<Tri*> tris; // djs: For heaven's sake!!!!
02393 
02394 #if STANDALONE
02395 class Tri : public int3
02396 #else
02397 class Tri : public int3, public NxFoundation::NxAllocateable
02398 #endif
02399 {
02400 public:
02401         int3 n;
02402         int id;
02403         int vmax;
02404         double rise;
02405         Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
02406         {
02407                 id = tris.count;
02408                 tris.Add(this);
02409                 vmax=-1;
02410                 rise = 0.0f;
02411         }
02412         ~Tri()
02413         {
02414                 assert(tris[id]==this);
02415                 tris[id]=NULL;
02416         }
02417         int &neib(int a,int b);
02418 };
02419 
02420 
02421 int &Tri::neib(int a,int b)
02422 {
02423         static int er=-1;
02424         int i;
02425         for(i=0;i<3;i++) 
02426         {
02427                 int i1=(i+1)%3;
02428                 int i2=(i+2)%3;
02429                 if((*this)[i]==a && (*this)[i1]==b) return n[i2];
02430                 if((*this)[i]==b && (*this)[i1]==a) return n[i2];
02431         }
02432         assert(0);
02433         return er;
02434 }
02435 void b2bfix(Tri* s,Tri*t)
02436 {
02437         int i;
02438         for(i=0;i<3;i++) 
02439         {
02440                 int i1=(i+1)%3;
02441                 int i2=(i+2)%3;
02442                 int a = (*s)[i1];
02443                 int b = (*s)[i2];
02444                 assert(tris[s->neib(a,b)]->neib(b,a) == s->id);
02445                 assert(tris[t->neib(a,b)]->neib(b,a) == t->id);
02446                 tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
02447                 tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
02448         }
02449 }
02450 
02451 void removeb2b(Tri* s,Tri*t)
02452 {
02453         b2bfix(s,t);
02454         delete s;
02455         delete t;
02456 }
02457 
02458 void checkit(Tri *t)
02459 {
02460         int i;
02461         assert(tris[t->id]==t);
02462         for(i=0;i<3;i++)
02463         {
02464                 int i1=(i+1)%3;
02465                 int i2=(i+2)%3;
02466                 int a = (*t)[i1];
02467                 int b = (*t)[i2];
02468                 assert(a!=b);
02469                 assert( tris[t->n[i]]->neib(b,a) == t->id);
02470         }
02471 }
02472 void extrude(Tri *t0,int v)
02473 {
02474         int3 t= *t0;
02475         int n = tris.count;
02476 #if STANDALONE
02477         Tri* ta = new Tri(v,t[1],t[2]);
02478 #else
02479         Tri* ta = NX_NEW_MEM(Tri(v,t[1],t[2]), CONVEX_TEMP);
02480 #endif
02481         ta->n = int3(t0->n[0],n+1,n+2);
02482         tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
02483 #if STANDALONE
02484         Tri* tb = new Tri(v,t[2],t[0]);
02485 #else
02486         Tri* tb = NX_NEW_MEM(Tri(v,t[2],t[0]), CONVEX_TEMP);
02487 #endif
02488         tb->n = int3(t0->n[1],n+2,n+0);
02489         tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
02490 #if STANDALONE
02491         Tri* tc = new Tri(v,t[0],t[1]);
02492 #else
02493         Tri* tc = NX_NEW_MEM(Tri(v,t[0],t[1]), CONVEX_TEMP);
02494 #endif
02495         tc->n = int3(t0->n[2],n+0,n+1);
02496         tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
02497         checkit(ta);
02498         checkit(tb);
02499         checkit(tc);
02500         if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]]);
02501         if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]]);
02502         if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]]);
02503         delete t0;
02504 
02505 }
02506 
02507 Tri *extrudable(double epsilon)
02508 {
02509         int i;
02510         Tri *t=NULL;
02511         for(i=0;i<tris.count;i++)
02512         {
02513                 if(!t || (tris[i] && t->rise<tris[i]->rise))
02514                 {
02515                         t = tris[i];
02516                 }
02517         }
02518         return (t->rise >epsilon)?t:NULL ;
02519 }
02520 
02521 class int4
02522 {
02523 public:
02524         int x,y,z,w;
02525         int4(){};
02526         int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;}
02527         const int& operator[](int i) const {return (&x)[i];}
02528         int& operator[](int i) {return (&x)[i];}
02529 };
02530 
02531 
02532 
02533 bool hasVolume(double3 *verts, int p0, int p1, int p2, int p3)
02534 {
02535         double3 result3 = cross(verts[p1]-verts[p0], verts[p2]-verts[p0]);
02536         if (magnitude(result3) < VOLUME_EPSILON && magnitude(result3) > -VOLUME_EPSILON) // Almost collinear or otherwise very close to each other
02537                 return false;
02538         double result = dot(normalize(result3), verts[p3]-verts[p0]);
02539         return (result > VOLUME_EPSILON || result < -VOLUME_EPSILON); // Returns true iff volume is significantly non-zero
02540 }
02541 
02542 int4 FindSimplex(double3 *verts,int verts_count,Array<int> &allow)
02543 {
02544         double3 basis[3];
02545         basis[0] = double3( 0.01f, 0.02f, 1.0f );      
02546         int p0 = maxdirsterid(verts,verts_count, basis[0],allow);
02547         int     p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
02548         basis[0] = verts[p0]-verts[p1];
02549         if(p0==p1 || basis[0]==double3(0,0,0)) 
02550                 return int4(-1,-1,-1,-1);
02551         basis[1] = cross(double3(     1, 0.02f, 0),basis[0]);
02552         basis[2] = cross(double3(-0.02f,     1, 0),basis[0]);
02553         basis[1] = normalize( (magnitude(basis[1])>magnitude(basis[2])) ? basis[1]:basis[2]);
02554         int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
02555         if(p2 == p0 || p2 == p1)
02556         {
02557                 p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
02558         }
02559         if(p2 == p0 || p2 == p1) 
02560                 return int4(-1,-1,-1,-1);
02561         basis[1] = verts[p2] - verts[p0];
02562         basis[2] = normalize(cross(basis[1],basis[0]));
02563         int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
02564         if(p3==p0||p3==p1||p3==p2||!hasVolume(verts, p0, p1, p2, p3)) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
02565         if(p3==p0||p3==p1||p3==p2) 
02566                 return int4(-1,-1,-1,-1);
02567         assert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
02568         if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
02569         return int4(p0,p1,p2,p3);
02570 }
02571 
02572 int calchullgen(double3 *verts,int verts_count, int vlimit) 
02573 {
02574         if(verts_count <4) return 0;
02575         if(vlimit==0) vlimit=1000000000;
02576         int j;
02577         double3 bmin(*verts),bmax(*verts);
02578         Array<int> isextreme(verts_count);
02579         Array<int> allow(verts_count);
02580         for(j=0;j<verts_count;j++) 
02581         {
02582                 allow.Add(1);
02583                 isextreme.Add(0);
02584                 bmin = VectorMin(bmin,verts[j]);
02585                 bmax = VectorMax(bmax,verts[j]);
02586         }
02587         double epsilon = magnitude(bmax-bmin) * 0.001f;
02588 
02589 
02590         int4 p = FindSimplex(verts,verts_count,allow);
02591         if(p.x==-1) return 0; // simplex failed
02592 
02593 
02594 
02595         double3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) /4.0f;  // a valid interior point
02596 #if STANDALONE
02597         Tri *t0 = new Tri(p[2],p[3],p[1]); t0->n=int3(2,3,1);
02598         Tri *t1 = new Tri(p[3],p[2],p[0]); t1->n=int3(3,2,0);
02599         Tri *t2 = new Tri(p[0],p[1],p[3]); t2->n=int3(0,1,3);
02600         Tri *t3 = new Tri(p[1],p[0],p[2]); t3->n=int3(1,0,2);
02601 #else
02602         Tri *t0 = NX_NEW_MEM(Tri(p[2],p[3],p[1]); t0->n=int3(2,3,1), CONVEX_TEMP);
02603         Tri *t1 = NX_NEW_MEM(Tri(p[3],p[2],p[0]); t1->n=int3(3,2,0), CONVEX_TEMP);
02604         Tri *t2 = NX_NEW_MEM(Tri(p[0],p[1],p[3]); t2->n=int3(0,1,3), CONVEX_TEMP);
02605         Tri *t3 = NX_NEW_MEM(Tri(p[1],p[0],p[2]); t3->n=int3(1,0,2), CONVEX_TEMP);
02606 #endif
02607         isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
02608         checkit(t0);checkit(t1);checkit(t2);checkit(t3);
02609 
02610         for(j=0;j<tris.count;j++)
02611         {
02612                 Tri *t=tris[j];
02613                 assert(t);
02614                 assert(t->vmax<0);
02615                 double3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
02616                 t->vmax = maxdirsterid(verts,verts_count,n,allow);
02617                 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
02618         }
02619         Tri *te;
02620         vlimit-=4;
02621         while(vlimit >0 && (te=extrudable(epsilon)))
02622         {
02623                 int3 ti=*te;
02624                 int v=te->vmax;
02625                 assert(!isextreme[v]);  // wtf we've already done this vertex
02626                 isextreme[v]=1;
02627                 //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
02628                 j=tris.count;
02629                 int newstart=j;
02630                 while(j--) {
02631                         if(!tris[j]) continue;
02632                         int3 t=*tris[j];
02633                         if(above(verts,t,verts[v],0.01f*epsilon))
02634                         {
02635                                 extrude(tris[j],v);
02636                         }
02637                 }
02638                 // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
02639                 j=tris.count;
02640                 while(j--)
02641                 {
02642                         if(!tris[j]) continue;
02643                         if(!hasvert(*tris[j],v)) break;
02644                         int3 nt=*tris[j];
02645                         if(above(verts,nt,center,0.01f*epsilon)  || magnitude(cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]))< epsilon*epsilon*0.1f )
02646                         {
02647                                 Tri *nb = tris[tris[j]->n[0]];
02648                                 assert(nb);assert(!hasvert(*nb,v));assert(nb->id<j);
02649                                 extrude(nb,v);
02650                                 j=tris.count;
02651                         }
02652                 }
02653                 j=tris.count;
02654                 while(j--)
02655                 {
02656                         Tri *t=tris[j];
02657                         if(!t) continue;
02658                         if(t->vmax>=0) break;
02659                         double3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
02660                         t->vmax = maxdirsterid(verts,verts_count,n,allow);
02661                         if(isextreme[t->vmax]) 
02662                         {
02663                                 t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate.
02664                         }
02665                         else
02666                         {
02667                                 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
02668                         }
02669                 }
02670                 vlimit --;
02671         }
02672         return 1;
02673 }
02674 
02675 int calchull(double3 *verts,int verts_count, int *&tris_out, int &tris_count,int vlimit) 
02676 {
02677         int rc=calchullgen(verts,verts_count,  vlimit) ;
02678         if(!rc) return 0;
02679         Array<int> ts;
02680         for(int i=0;i<tris.count;i++)if(tris[i])
02681         {
02682                 for(int j=0;j<3;j++)ts.Add((*tris[i])[j]);
02683                 delete tris[i];
02684         }
02685         tris_count = ts.count/3;
02686         tris_out   = ts.element;
02687         ts.element=NULL; ts.count=ts.array_size=0;
02688         // please reset here, otherwise, we get a nice virtual function call (R6025) error with NxCooking library
02689         tris.SetSize( 0 );
02690         return 1;
02691 }
02692 
02693 static double area2(const double3 &v0,const double3 &v1,const double3 &v2)
02694 {
02695         double3 cp = cross(v0-v1,v2-v0);
02696         return dot(cp,cp);
02697 }
02698 int calchullpbev(double3 *verts,int verts_count,int vlimit, Array<Plane> &planes,double bevangle) 
02699 {
02700         int i,j;
02701         Array<Plane> bplanes;
02702         planes.count=0;
02703         int rc = calchullgen(verts,verts_count,vlimit);
02704         if(!rc) return 0;
02705         extern double minadjangle; // default is 3.0f;  // in degrees  - result wont have two adjacent facets within this angle of each other.
02706         double maxdot_minang = cos(DEG2RAD*minadjangle);
02707         for(i=0;i<tris.count;i++)if(tris[i])
02708         {
02709                 Plane p;
02710                 Tri *t = tris[i];
02711                 p.normal = TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
02712                 p.dist   = -dot(p.normal, verts[(*t)[0]]);
02713                 for(j=0;j<3;j++)
02714                 {
02715                         if(t->n[j]<t->id) continue;
02716                         Tri *s = tris[t->n[j]];
02717                         REAL3 snormal = TriNormal(verts[(*s)[0]],verts[(*s)[1]],verts[(*s)[2]]);
02718                         if(dot(snormal,p.normal)>=cos(bevangle*DEG2RAD)) continue;
02719                         REAL3 e = verts[(*t)[(j+2)%3]] - verts[(*t)[(j+1)%3]];
02720                         REAL3 n = (e!=REAL3(0,0,0))? cross(snormal,e)+cross(e,p.normal) : snormal+p.normal;
02721                         assert(n!=REAL3(0,0,0));
02722                         if(n==REAL3(0,0,0)) return 0;  
02723                         n=normalize(n);
02724                         bplanes.Add(Plane(n,-dot(n,verts[maxdir(verts,verts_count,n)])));
02725                 }
02726         }
02727         for(i=0;i<tris.count;i++)if(tris[i])for(j=i+1;j<tris.count;j++)if(tris[i] && tris[j])
02728         {
02729                 Tri *ti = tris[i];
02730                 Tri *tj = tris[j];
02731                 REAL3 ni = TriNormal(verts[(*ti)[0]],verts[(*ti)[1]],verts[(*ti)[2]]);
02732                 REAL3 nj = TriNormal(verts[(*tj)[0]],verts[(*tj)[1]],verts[(*tj)[2]]);
02733                 if(dot(ni,nj)>maxdot_minang)
02734                 {
02735                         // somebody has to die, keep the biggest triangle
02736                         if( area2(verts[(*ti)[0]],verts[(*ti)[1]],verts[(*ti)[2]]) < area2(verts[(*tj)[0]],verts[(*tj)[1]],verts[(*tj)[2]]))
02737                         {
02738                                 delete tris[i]; tris[i]=NULL;
02739                         }
02740                         else
02741                         {
02742                                 delete tris[j]; tris[j]=NULL;
02743                         }
02744                 }
02745         }
02746         for(i=0;i<tris.count;i++)if(tris[i])
02747         {
02748                 Plane p;
02749                 Tri *t = tris[i];
02750                 p.normal = TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
02751                 p.dist   = -dot(p.normal, verts[(*t)[0]]);
02752                 planes.Add(p);
02753         }
02754         for(i=0;i<bplanes.count;i++)
02755         {
02756                 for(j=0;j<planes.count;j++)
02757                 {
02758                         if(dot(bplanes[i].normal,planes[j].normal)>maxdot_minang) break;
02759                 }
02760                 if(j==planes.count)
02761                 {
02762                         planes.Add(bplanes[i]);
02763                 }
02764         }
02765         for(i=0;i<tris.count;i++)if(tris[i])
02766         {
02767                 delete tris[i];
02768         }
02769         tris.count = 0; //bad place to do the tris.SetSize(0) fix, this line is executed many times, and will result in a whole lot of allocations if the array is totally cleared here
02770         return 1;
02771 }
02772 
02773 static int overhull(Plane *planes,int planes_count,double3 *verts, int verts_count,int maxplanes, 
02774                          double3 *&verts_out, int &verts_count_out,  int *&faces_out, int &faces_count_out ,double inflate)
02775 {
02776         int i,j;
02777         if(verts_count <4) return NULL;
02778         maxplanes = Min(maxplanes,planes_count);
02779         double3 bmin(verts[0]),bmax(verts[0]);
02780         for(i=0;i<verts_count;i++) 
02781         {
02782                 bmin = VectorMin(bmin,verts[i]);
02783                 bmax = VectorMax(bmax,verts[i]);
02784         }
02785         double diameter = magnitude(bmax-bmin);
02786 //      inflate *=diameter;   // RELATIVE INFLATION
02787         bmin -= double3(inflate*2.5f,inflate*2.5f,inflate*2.5f); 
02788         bmax += double3(inflate*2.5f,inflate*2.5f,inflate*2.5f); 
02789         // 2 is from the formula:
02790         // D = d*|n1+n2|/(1-n1 dot n2), where d is "inflate" and
02791         // n1 and n2 are the normals of two planes at bevelAngle to each other
02792         // for 120 degrees, D is 2d
02793 
02794         //bmin -= double3(inflate,inflate,inflate);
02795         //bmax += double3(inflate,inflate,inflate);
02796         for(i=0;i<planes_count;i++)
02797         {
02798                 planes[i].dist -= inflate;
02799         }
02800         double3 emin = bmin; // VectorMin(bmin,double3(0,0,0));
02801         double3 emax = bmax; // VectorMax(bmax,double3(0,0,0));
02802         double epsilon  = 0.01f; // size of object is taken into account within candidate plane function.  Used to multiply here by magnitude(emax-emin) 
02803         planetestepsilon = magnitude(emax-emin) * PAPERWIDTH;
02804         // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think.
02805         // ConvexH *convex = ConvexHMakeCube(bmin - double3(diameter,diameter,diameter),bmax+double3(diameter,diameter,diameter));
02806         double maxdot_minang = cos(DEG2RAD*minadjangle);
02807         for(j=0;j<6;j++)
02808         {
02809                 double3 n(0,0,0);
02810                 n[j/2] = (j%2)? 1.0f : -1.0f;
02811                 for(i=0;i<planes_count;i++)
02812                 {
02813                         if(dot(n,planes[i].normal)> maxdot_minang)
02814                         {
02815                                 (*((j%2)?&bmax:&bmin)) += n * (diameter*0.5f);
02816                                 break;
02817                         }
02818                 }
02819         }
02820         ConvexH *c = ConvexHMakeCube(REAL3(bmin),REAL3(bmax)); 
02821         int k;
02822         while(maxplanes-- && (k=candidateplane(planes,planes_count,c,epsilon))>=0)
02823         {
02824                 ConvexH *tmp = c;
02825                 c = ConvexHCrop(*tmp,planes[k]);
02826                 if(c==NULL) {c=tmp; break;} // might want to debug this case better!!!
02827                 if(!AssertIntact(*c)) {c=tmp; break;} // might want to debug this case better too!!!
02828                 delete tmp;
02829         }
02830 
02831         assert(AssertIntact(*c));
02832         //return c;
02833         faces_out = (int*)NX_ALLOC(sizeof(int)*(1+c->facets.count+c->edges.count), CONVEX_TEMP);     // new int[1+c->facets.count+c->edges.count];
02834         faces_count_out=0;
02835         i=0;
02836         faces_out[faces_count_out++]=-1;
02837         k=0;
02838         while(i<c->edges.count)
02839         {
02840                 j=1;
02841                 while(j+i<c->edges.count && c->edges[i].p==c->edges[i+j].p) { j++; }
02842                 faces_out[faces_count_out++]=j;
02843                 while(j--)
02844                 {
02845                         faces_out[faces_count_out++] = c->edges[i].v;
02846                         i++;
02847                 }
02848                 k++;
02849         }
02850         faces_out[0]=k; // number of faces.
02851         assert(k==c->facets.count);
02852         assert(faces_count_out == 1+c->facets.count+c->edges.count);
02853         verts_out = c->vertices.element; // new double3[c->vertices.count];
02854         verts_count_out = c->vertices.count;
02855         for(i=0;i<c->vertices.count;i++)
02856         {
02857                 verts_out[i] = double3(c->vertices[i]);
02858         }
02859         c->vertices.count=c->vertices.array_size=0;     c->vertices.element=NULL;
02860         delete c;
02861         return 1;
02862 }
02863 
02864 static int overhullv(double3 *verts, int verts_count,int maxplanes,
02865                          double3 *&verts_out, int &verts_count_out,  int *&faces_out, int &faces_count_out ,double inflate,double bevangle,int vlimit)
02866 {
02867         if(!verts_count) return 0;
02868         extern int calchullpbev(double3 *verts,int verts_count,int vlimit, Array<Plane> &planes,double bevangle) ;
02869         Array<Plane> planes;
02870         int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle) ;
02871         if(!rc) return 0;
02872         return overhull(planes.element,planes.count,verts,verts_count,maxplanes,verts_out,verts_count_out,faces_out,faces_count_out,inflate);
02873 }
02874 
02875 
02876 //*****************************************************
02877 //*****************************************************
02878 
02879 
02880 bool ComputeHull(unsigned int vcount,const double *vertices,PHullResult &result,unsigned int vlimit,double inflate)
02881 {
02882 
02883         int index_count;
02884         int *faces;
02885         double3 *verts_out;
02886         int     verts_count_out;
02887 
02888         if(inflate==0.0f)
02889         {
02890                 int  *tris_out;
02891                 int    tris_count;
02892                 int ret = calchull( (double3 *) vertices, (int) vcount, tris_out, tris_count, vlimit );
02893                 if(!ret) return false;
02894                 result.mIndexCount = (unsigned int) (tris_count*3);
02895                 result.mFaceCount  = (unsigned int) tris_count;
02896                 result.mVertices   = (double*) vertices;
02897                 result.mVcount     = (unsigned int) vcount;
02898                 result.mIndices    = (unsigned int *) tris_out;
02899                 return true;
02900         }
02901 
02902         int ret = overhullv((double3*)vertices,vcount,35,verts_out,verts_count_out,faces,index_count,inflate,120.0f,vlimit);
02903         if(!ret) {
02904                 tris.SetSize(0); //have to set the size to 0 in order to protect from a "pure virtual function call" problem
02905                 return false;
02906         }
02907 
02908         Array<int3> tris;
02909         int n=faces[0];
02910         int k=1;
02911         for(int i=0;i<n;i++)
02912         {
02913                 int pn = faces[k++];
02914                 for(int j=2;j<pn;j++) tris.Add(int3(faces[k],faces[k+j-1],faces[k+j]));
02915                 k+=pn;
02916         }
02917         assert(tris.count == index_count-1-(n*3));
02918         NX_FREE(faces); // PT: I added that. Is it ok ?
02919 
02920         result.mIndexCount = (unsigned int) (tris.count*3);
02921         result.mFaceCount  = (unsigned int) tris.count;
02922         result.mVertices   = (double*) verts_out;
02923         result.mVcount     = (unsigned int) verts_count_out;
02924         result.mIndices    = (unsigned int *) tris.element;
02925         tris.element=NULL; tris.count = tris.array_size=0;
02926         ConvexDecomposition::tris.SetSize(0); //have to set the size to 0 in order to protect from a "pure virtual function call" problem
02927 
02928         return true;
02929 }
02930 
02931 
02932 void ReleaseHull(PHullResult &result)
02933 {
02934 NX_FREE(result.mIndices);       // PT: I added that. Is it ok ?
02935 NX_FREE(result.mVertices);      // PT: I added that. Is it ok ?
02936         result.mVcount = 0;
02937         result.mIndexCount = 0;
02938         result.mIndices = 0;
02939         result.mVertices = 0;
02940         result.mIndices  = 0;
02941 }
02942 
02943 
02944 
02945 //****** HULLLIB source code
02946 
02947 
02948 HullError HullLibrary::CreateConvexHull(const HullDesc       &desc,           // describes the input request
02949                                                                                                                                                                 HullResult           &result)         // contains the resulst
02950 {
02951         HullError ret = QE_FAIL;
02952 
02953 
02954         PHullResult hr;
02955 
02956         unsigned int vcount = desc.mVcount;
02957         if ( vcount < 8 ) vcount = 8;
02958 
02959         double *vsource  = (double *) NX_ALLOC( sizeof(double)*vcount*3, CONVEX_TEMP );
02960 
02961 
02962         double scale[3];
02963 
02964         unsigned int ovcount;
02965 
02966         bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, vsource, desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates!
02967 
02968         if ( ok )
02969         {
02970     double bmin[3];
02971     double bmax[3];
02972 
02973 
02974                 if ( 1 ) // scale vertices back to their original size.
02975                 {
02976 
02977                         for (unsigned int i=0; i<ovcount; i++)
02978                         {
02979                                 double *v = &vsource[i*3];
02980                                 v[0]*=scale[0];
02981                                 v[1]*=scale[1];
02982                                 v[2]*=scale[2];
02983 
02984         if ( i == 0 )
02985         {
02986           bmin[0] = bmax[0] = v[0];
02987           bmin[1] = bmax[1] = v[1];
02988           bmin[2] = bmax[2] = v[2];
02989         }
02990         else
02991         {
02992           if ( v[0] < bmin[0] ) bmin[0] = v[0];
02993           if ( v[1] < bmin[1] ) bmin[1] = v[1];
02994           if ( v[2] < bmin[2] ) bmin[2] = v[2];
02995           if ( v[0] > bmax[0] ) bmax[0] = v[0];
02996           if ( v[1] > bmax[1] ) bmax[1] = v[1];
02997           if ( v[2] > bmax[2] ) bmax[2] = v[2];
02998         }
02999 
03000                         }
03001                 }
03002 
03003                 double skinwidth = 0;
03004 
03005                 if ( desc.HasHullFlag(QF_SKIN_WIDTH) )
03006     {
03007                         skinwidth = desc.mSkinWidth;
03008       if ( skinwidth < 0 ) // if it is a negative skinwidth we shrink the hull points relative to the center.
03009       {
03010         double center[3];
03011 
03012         center[0] = (bmax[0] - bmin[0])*0.5f + bmin[0];
03013         center[1] = (bmax[1] - bmin[1])*0.5f + bmin[1];
03014         center[2] = (bmax[2] - bmin[2])*0.5f + bmin[2];
03015 
03016         double dx = (bmax[0]-bmin[0])*0.5f;
03017         double dy = (bmax[1]-bmin[1])*0.5f;
03018         double dz = (bmax[2]-bmin[2])*0.5f;
03019         double dist = sqrt(dx*dx+dy*dy+dz*dz);
03020 
03021         skinwidth*=-1; // make it positive...
03022 
03023         double scale = 1.0f - (skinwidth/dist);
03024         if ( scale < 0.3f ) scale = 0.3f;
03025                         for (unsigned int i=0; i<ovcount; i++)
03026                         {
03027                                 double *v = &vsource[i*3];
03028 
03029           v[0]-=center[0];
03030           v[1]-=center[1];
03031           v[2]-=center[2];
03032 
03033           v[0]*=scale;
03034           v[1]*=scale;
03035           v[2]*=scale;
03036 
03037           v[0]+=center[0];
03038           v[1]+=center[1];
03039           v[2]+=center[2];
03040         }
03041         skinwidth = 0;
03042       }
03043     }
03044 
03045                 ok = ComputeHull(ovcount,vsource,hr,desc.mMaxVertices,skinwidth);
03046 
03047                 if ( ok )
03048                 {
03049 
03050                         // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
03051                         double *vscratch = (double *) NX_ALLOC( sizeof(double)*hr.mVcount*3, CONVEX_TEMP );
03052                         BringOutYourDead(hr.mVertices,hr.mVcount, vscratch, ovcount, hr.mIndices, hr.mIndexCount );
03053 
03054                         ret = QE_OK;
03055 
03056                         if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle!
03057                         {
03058                                 result.mPolygons          = false;
03059                                 result.mNumOutputVertices = ovcount;
03060                                 result.mOutputVertices    = (double *)NX_ALLOC( sizeof(double)*ovcount*3, CONVEX_TEMP );
03061                                 result.mNumFaces          = hr.mFaceCount;
03062                                 result.mNumIndices        = hr.mIndexCount;
03063 
03064                                 result.mIndices           = (unsigned int *) NX_ALLOC( sizeof(unsigned int)*hr.mIndexCount, CONVEX_TEMP );
03065 
03066                                 memcpy(result.mOutputVertices, vscratch, sizeof(double)*3*ovcount );
03067 
03068                         if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
03069                                 {
03070 
03071                                         const unsigned int *source = hr.mIndices;
03072                                                                 unsigned int *dest   = result.mIndices;
03073 
03074                                         for (unsigned int i=0; i<hr.mFaceCount; i++)
03075                                         {
03076                                                 dest[0] = source[2];
03077                                                 dest[1] = source[1];
03078                                                 dest[2] = source[0];
03079                                                 dest+=3;
03080                                                 source+=3;
03081                                         }
03082 
03083                                 }
03084                                 else
03085                                 {
03086                                         memcpy(result.mIndices, hr.mIndices, sizeof(unsigned int)*hr.mIndexCount);
03087                                 }
03088                         }
03089                         else
03090                         {
03091                                 result.mPolygons          = true;
03092                                 result.mNumOutputVertices = ovcount;
03093                                 result.mOutputVertices    = (double *)NX_ALLOC( sizeof(double)*ovcount*3, CONVEX_TEMP );
03094                                 result.mNumFaces          = hr.mFaceCount;
03095                                 result.mNumIndices        = hr.mIndexCount+hr.mFaceCount;
03096                                 result.mIndices           = (unsigned int *) NX_ALLOC( sizeof(unsigned int)*result.mNumIndices, CONVEX_TEMP );
03097                                 memcpy(result.mOutputVertices, vscratch, sizeof(double)*3*ovcount );
03098 
03099                                 if ( 1 )
03100                                 {
03101                                         const unsigned int *source = hr.mIndices;
03102                                                                 unsigned int *dest   = result.mIndices;
03103                                         for (unsigned int i=0; i<hr.mFaceCount; i++)
03104                                         {
03105                                                 dest[0] = 3;
03106                                                 if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
03107                                                 {
03108                                                         dest[1] = source[2];
03109                                                         dest[2] = source[1];
03110                                                         dest[3] = source[0];
03111                                                 }
03112                                                 else
03113                                                 {
03114                                                         dest[1] = source[0];
03115                                                         dest[2] = source[1];
03116                                                         dest[3] = source[2];
03117                                                 }
03118 
03119                                                 dest+=4;
03120                                                 source+=3;
03121                                         }
03122                                 }
03123                         }
03124                         // ReleaseHull frees memory for hr.mVertices, which can be the
03125                         // same pointer as vsource, so be sure to set it to NULL if necessary
03126                         if ( hr.mVertices == vsource) vsource = NULL;
03127 
03128                         ReleaseHull(hr);
03129 
03130                         if ( vscratch )
03131                         {
03132                                 NX_FREE(vscratch);
03133                         }
03134                 }
03135         }
03136 
03137         // this pointer is usually freed in ReleaseHull()
03138         if ( vsource )
03139         {
03140                 NX_FREE(vsource);
03141         }
03142 
03143 
03144         return ret;
03145 }
03146 
03147 
03148 
03149 HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it.
03150 {
03151         if ( result.mOutputVertices )
03152         {
03153                 NX_FREE(result.mOutputVertices);
03154                 result.mOutputVertices = 0;
03155         }
03156         if ( result.mIndices )
03157         {
03158                 NX_FREE(result.mIndices);
03159                 result.mIndices = 0;
03160         }
03161         return QE_OK;
03162 }
03163 
03164 
03165 static void AddPoint(unsigned int &vcount,double *p,double x,double y,double z)
03166 {
03167         double *dest = &p[vcount*3];
03168         dest[0] = x;
03169         dest[1] = y;
03170         dest[2] = z;
03171         vcount++;
03172 }
03173 
03174 
03175 double GetDist(double px,double py,double pz,const double *p2)
03176 {
03177 
03178         double dx = px - p2[0];
03179         double dy = py - p2[1];
03180         double dz = pz - p2[2];
03181 
03182         return dx*dx+dy*dy+dz*dz;
03183 }
03184 
03185 
03186 
03187 bool  HullLibrary::CleanupVertices(unsigned int svcount,
03188                                                                                                                                 const double *svertices,
03189                                                                                                                                 unsigned int stride,
03190                                                                                                                                 unsigned int &vcount,       // output number of vertices
03191                                                                                                                                 double *vertices,                 // location to store the results.
03192                                                                                                                                 double  normalepsilon,
03193                                                                                                                                 double *scale)
03194 {
03195         if ( svcount == 0 ) return false;
03196 
03197 
03198         #define EPSILON 0.000001f // close enough to consider two doubleing point numbers to be 'the same'.
03199 
03200         bool ret = false;
03201 
03202         vcount = 0;
03203 
03204         double recip[3];
03205 
03206         if ( scale )
03207         {
03208                 scale[0] = 1;
03209                 scale[1] = 1;
03210                 scale[2] = 1;
03211         }
03212 
03213         double bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
03214         double bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
03215 
03216         const char *vtx = (const char *) svertices;
03217 
03218         if ( 1 )
03219         {
03220                 for (unsigned int i=0; i<svcount; i++)
03221                 {
03222                         const double *p = (const double *) vtx;
03223 
03224                         vtx+=stride;
03225 
03226                         for (int j=0; j<3; j++)
03227                         {
03228                                 if ( p[j] < bmin[j] ) bmin[j] = p[j];
03229                                 if ( p[j] > bmax[j] ) bmax[j] = p[j];
03230                         }
03231                 }
03232         }
03233 
03234         double dx = bmax[0] - bmin[0];
03235         double dy = bmax[1] - bmin[1];
03236         double dz = bmax[2] - bmin[2];
03237 
03238         double center[3];
03239 
03240         center[0] = dx*0.5f + bmin[0];
03241         center[1] = dy*0.5f + bmin[1];
03242         center[2] = dz*0.5f + bmin[2];
03243 
03244         if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
03245         {
03246 
03247                 double len = FLT_MAX;
03248 
03249                 if ( dx > EPSILON && dx < len ) len = dx;
03250                 if ( dy > EPSILON && dy < len ) len = dy;
03251                 if ( dz > EPSILON && dz < len ) len = dz;
03252 
03253                 if ( len == FLT_MAX )
03254                 {
03255                         dx = dy = dz = 0.01f; // one centimeter
03256                 }
03257                 else
03258                 {
03259                         if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge.
03260                         if ( dy < EPSILON ) dy = len * 0.05f;
03261                         if ( dz < EPSILON ) dz = len * 0.05f;
03262                 }
03263 
03264                 double x1 = center[0] - dx;
03265                 double x2 = center[0] + dx;
03266 
03267                 double y1 = center[1] - dy;
03268                 double y2 = center[1] + dy;
03269 
03270                 double z1 = center[2] - dz;
03271                 double z2 = center[2] + dz;
03272 
03273                 AddPoint(vcount,vertices,x1,y1,z1);
03274                 AddPoint(vcount,vertices,x2,y1,z1);
03275                 AddPoint(vcount,vertices,x2,y2,z1);
03276                 AddPoint(vcount,vertices,x1,y2,z1);
03277                 AddPoint(vcount,vertices,x1,y1,z2);
03278                 AddPoint(vcount,vertices,x2,y1,z2);
03279                 AddPoint(vcount,vertices,x2,y2,z2);
03280                 AddPoint(vcount,vertices,x1,y2,z2);
03281 
03282                 return true; // return cube
03283 
03284 
03285         }
03286         else
03287         {
03288                 if ( scale )
03289                 {
03290                         scale[0] = dx;
03291                         scale[1] = dy;
03292                         scale[2] = dz;
03293 
03294                         recip[0] = 1 / dx;
03295                         recip[1] = 1 / dy;
03296                         recip[2] = 1 / dz;
03297 
03298                         center[0]*=recip[0];
03299                         center[1]*=recip[1];
03300                         center[2]*=recip[2];
03301 
03302                 }
03303 
03304         }
03305 
03306 
03307 
03308         vtx = (const char *) svertices;
03309 
03310         for (unsigned int i=0; i<svcount; i++)
03311         {
03312 
03313                 const double *p = (const double *)vtx;
03314                 vtx+=stride;
03315 
03316                 double px = p[0];
03317                 double py = p[1];
03318                 double pz = p[2];
03319 
03320                 if ( scale )
03321                 {
03322                         px = px*recip[0]; // normalize
03323                         py = py*recip[1]; // normalize
03324                         pz = pz*recip[2]; // normalize
03325                 }
03326 
03327                 if ( 1 )
03328                 {
03329                         unsigned int j;
03330 
03331                         for (j=0; j<vcount; j++)
03332                         {
03333                                 double *v = &vertices[j*3];
03334 
03335                                 double x = v[0];
03336                                 double y = v[1];
03337                                 double z = v[2];
03338 
03339                                 double dx = fabs(x - px );
03340                                 double dy = fabs(y - py );
03341                                 double dz = fabs(z - pz );
03342 
03343                                 if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
03344                                 {
03345                                         // ok, it is close enough to the old one
03346                                         // now let us see if it is further from the center of the point cloud than the one we already recorded.
03347                                         // in which case we keep this one instead.
03348 
03349                                         double dist1 = GetDist(px,py,pz,center);
03350                                         double dist2 = GetDist(v[0],v[1],v[2],center);
03351 
03352                                         if ( dist1 > dist2 )
03353                                         {
03354                                                 v[0] = px;
03355                                                 v[1] = py;
03356                                                 v[2] = pz;
03357                                         }
03358 
03359                                         break;
03360                                 }
03361                         }
03362 
03363                         if ( j == vcount )
03364                         {
03365                                 double *dest = &vertices[vcount*3];
03366                                 dest[0] = px;
03367                                 dest[1] = py;
03368                                 dest[2] = pz;
03369                                 vcount++;
03370                         }
03371                 }
03372         }
03373 
03374         // ok..now make sure we didn't prune so many vertices it is now invalid.
03375         if ( 1 )
03376         {
03377                 double bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
03378                 double bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
03379 
03380                 for (unsigned int i=0; i<vcount; i++)
03381                 {
03382                         const double *p = &vertices[i*3];
03383                         for (int j=0; j<3; j++)
03384                         {
03385                                 if ( p[j] < bmin[j] ) bmin[j] = p[j];
03386                                 if ( p[j] > bmax[j] ) bmax[j] = p[j];
03387                         }
03388                 }
03389 
03390                 double dx = bmax[0] - bmin[0];
03391                 double dy = bmax[1] - bmin[1];
03392                 double dz = bmax[2] - bmin[2];
03393 
03394                 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
03395                 {
03396                         double cx = dx*0.5f + bmin[0];
03397                         double cy = dy*0.5f + bmin[1];
03398                         double cz = dz*0.5f + bmin[2];
03399 
03400                         double len = FLT_MAX;
03401 
03402                         if ( dx >= EPSILON && dx < len ) len = dx;
03403                         if ( dy >= EPSILON && dy < len ) len = dy;
03404                         if ( dz >= EPSILON && dz < len ) len = dz;
03405 
03406                         if ( len == FLT_MAX )
03407                         {
03408                                 dx = dy = dz = 0.01f; // one centimeter
03409                         }
03410                         else
03411                         {
03412                                 if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge.
03413                                 if ( dy < EPSILON ) dy = len * 0.05f;
03414                                 if ( dz < EPSILON ) dz = len * 0.05f;
03415                         }
03416 
03417                         double x1 = cx - dx;
03418                         double x2 = cx + dx;
03419 
03420                         double y1 = cy - dy;
03421                         double y2 = cy + dy;
03422 
03423                         double z1 = cz - dz;
03424                         double z2 = cz + dz;
03425 
03426                         vcount = 0; // add box
03427 
03428                         AddPoint(vcount,vertices,x1,y1,z1);
03429                         AddPoint(vcount,vertices,x2,y1,z1);
03430                         AddPoint(vcount,vertices,x2,y2,z1);
03431                         AddPoint(vcount,vertices,x1,y2,z1);
03432                         AddPoint(vcount,vertices,x1,y1,z2);
03433                         AddPoint(vcount,vertices,x2,y1,z2);
03434                         AddPoint(vcount,vertices,x2,y2,z2);
03435                         AddPoint(vcount,vertices,x1,y2,z2);
03436 
03437                         return true;
03438                 }
03439         }
03440 
03441         return true;
03442 }
03443 
03444 void HullLibrary::BringOutYourDead(const double *verts,unsigned int vcount, double *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
03445 {
03446         unsigned int *used = (unsigned int *)NX_ALLOC(sizeof(unsigned int)*vcount, CONVEX_TEMP );
03447         memset(used,0,sizeof(unsigned int)*vcount);
03448 
03449         ocount = 0;
03450 
03451         for (unsigned int i=0; i<indexcount; i++)
03452         {
03453                 unsigned int v = indices[i]; // original array index
03454 
03455                 assert( v >= 0 && v < vcount );
03456 
03457                 if ( used[v] ) // if already remapped
03458                 {
03459                         indices[i] = used[v]-1; // index to new array
03460                 }
03461                 else
03462                 {
03463 
03464                         indices[i] = ocount;      // new index mapping
03465 
03466                         overts[ocount*3+0] = verts[v*3+0]; // copy old vert to new vert array
03467                         overts[ocount*3+1] = verts[v*3+1];
03468                         overts[ocount*3+2] = verts[v*3+2];
03469 
03470                         ocount++; // increment output vert count
03471 
03472                         assert( ocount >=0 && ocount <= vcount );
03473 
03474                         used[v] = ocount; // assign new index remapping
03475                 }
03476         }
03477 
03478         NX_FREE(used);
03479 }
03480 
03481 
03482 //==================================================================================
03483 HullError HullLibrary::CreateTriangleMesh(HullResult &answer,ConvexHullTriangleInterface *iface)
03484 {
03485         HullError ret = QE_FAIL;
03486 
03487 
03488         const double *p            = answer.mOutputVertices;
03489         const unsigned int   *idx = answer.mIndices;
03490         unsigned int fcount       = answer.mNumFaces;
03491 
03492         if ( p && idx && fcount )
03493         {
03494                 ret = QE_OK;
03495 
03496                 for (unsigned int i=0; i<fcount; i++)
03497                 {
03498                         unsigned int pcount = *idx++;
03499 
03500                         unsigned int i1 = *idx++;
03501                         unsigned int i2 = *idx++;
03502                         unsigned int i3 = *idx++;
03503 
03504                         const double *p1 = &p[i1*3];
03505                         const double *p2 = &p[i2*3];
03506                         const double *p3 = &p[i3*3];
03507 
03508                         AddConvexTriangle(iface,p1,p2,p3);
03509 
03510                         pcount-=3;
03511                         while ( pcount )
03512                         {
03513                                 i3 = *idx++;
03514                                 p2 = p3;
03515                                 p3 = &p[i3*3];
03516 
03517                                 AddConvexTriangle(iface,p1,p2,p3);
03518                                 pcount--;
03519                         }
03520 
03521                 }
03522         }
03523 
03524         return ret;
03525 }
03526 
03527 //==================================================================================
03528 void HullLibrary::AddConvexTriangle(ConvexHullTriangleInterface *callback,const double *p1,const double *p2,const double *p3)
03529 {
03530         ConvexHullVertex v1,v2,v3;
03531 
03532         #define TSCALE1 (1.0f/4.0f)
03533 
03534         v1.mPos[0] = p1[0];
03535         v1.mPos[1] = p1[1];
03536         v1.mPos[2] = p1[2];
03537 
03538         v2.mPos[0] = p2[0];
03539         v2.mPos[1] = p2[1];
03540         v2.mPos[2] = p2[2];
03541 
03542         v3.mPos[0] = p3[0];
03543         v3.mPos[1] = p3[1];
03544         v3.mPos[2] = p3[2];
03545 
03546         double n[3];
03547         ComputeNormal(n,p1,p2,p3);
03548 
03549         v1.mNormal[0] = n[0];
03550         v1.mNormal[1] = n[1];
03551         v1.mNormal[2] = n[2];
03552 
03553         v2.mNormal[0] = n[0];
03554         v2.mNormal[1] = n[1];
03555         v2.mNormal[2] = n[2];
03556 
03557         v3.mNormal[0] = n[0];
03558         v3.mNormal[1] = n[1];
03559         v3.mNormal[2] = n[2];
03560 
03561         const double *tp1 = p1;
03562         const double *tp2 = p2;
03563         const double *tp3 = p3;
03564 
03565         int i1 = 0;
03566         int i2 = 0;
03567 
03568         double nx = fabs(n[0]);
03569         double ny = fabs(n[1]);
03570         double nz = fabs(n[2]);
03571 
03572         if ( nx <= ny && nx <= nz ) 
03573                 i1 = 0;
03574         if ( ny <= nx && ny <= nz ) 
03575                 i1 = 1;
03576         if ( nz <= nx && nz <= ny ) 
03577                 i1 = 2;
03578 
03579         switch ( i1 )
03580         {
03581                 case 0:
03582                         if ( ny < nz )
03583                                 i2 = 1;
03584                         else
03585                                 i2 = 2;
03586                         break;
03587                 case 1:
03588                         if ( nx < nz )
03589                                 i2 = 0;
03590                         else
03591                                 i2 = 2;
03592                         break;
03593                 case 2:
03594                         if ( nx < ny )
03595                                 i2 = 0;
03596                         else
03597                                 i2 = 1;
03598                         break;
03599         }
03600 
03601         v1.mTexel[0] = tp1[i1]*TSCALE1;
03602         v1.mTexel[1] = tp1[i2]*TSCALE1;
03603 
03604         v2.mTexel[0] = tp2[i1]*TSCALE1;
03605         v2.mTexel[1] = tp2[i2]*TSCALE1;
03606 
03607         v3.mTexel[0] = tp3[i1]*TSCALE1;
03608         v3.mTexel[1] = tp3[i2]*TSCALE1;
03609 
03610         callback->ConvexHullTriangle(v3,v2,v1);
03611 }
03612 
03613 //==================================================================================
03614 double HullLibrary::ComputeNormal(double *n,const double *A,const double *B,const double *C)
03615 {
03616         double vx,vy,vz,wx,wy,wz,vw_x,vw_y,vw_z,mag;
03617 
03618         vx = (B[0] - C[0]);
03619         vy = (B[1] - C[1]);
03620         vz = (B[2] - C[2]);
03621 
03622         wx = (A[0] - B[0]);
03623         wy = (A[1] - B[1]);
03624         wz = (A[2] - B[2]);
03625 
03626         vw_x = vy * wz - vz * wy;
03627         vw_y = vz * wx - vx * wz;
03628         vw_z = vx * wy - vy * wx;
03629 
03630         mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
03631 
03632         if ( mag < 0.000001f )
03633         {
03634                 mag = 0;
03635         }
03636         else
03637         {
03638                 mag = 1.0f/mag;
03639         }
03640 
03641         n[0] = vw_x * mag;
03642         n[1] = vw_y * mag;
03643         n[2] = vw_z * mag;
03644 
03645         return mag;
03646 }
03647 
03648 
03649 
03650 };
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


convex_decomposition
Author(s): John Ratcliff
autogenerated on Mon Aug 19 2013 11:02:33