Extras/ConvexDecomposition/cd_hull.cpp

   1 #include "float_math.h"
   2
   3 #include <stdio.h>
   4 #include <stdlib.h>
   5 #include <string.h>
   6 #include <assert.h>
   7 #include <math.h>
   8 #include <float.h>
   9
  10 /*----------------------------------------------------------------------
  11                 Copyright (c) 2004 Open Dynamics Framework Group
  12                                         www.physicstools.org
  13                 All rights reserved.
  14
  15                 Redistribution and use in source and binary forms, with or without modification, are permitted provided
  16                 that the following conditions are met:
  17
  18                 Redistributions of source code must retain the above copyright notice, this list of conditions
  19                 and the following disclaimer.
  20
  21                 Redistributions in binary form must reproduce the above copyright notice,
  22                 this list of conditions and the following disclaimer in the documentation
  23                 and/or other materials provided with the distribution.
  24
  25                 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
  26                 be used to endorse or promote products derived from this software without specific prior written permission.
  27
  28                 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
  29                 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  30                 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  31                 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  32                 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
  33                 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  34                 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  35 -----------------------------------------------------------------------*/
  36
  37 // http://codesuppository.blogspot.com
  38 //
  39 // mailto: jratcliff@infiniplex.net
  40 //
  41 // http://www.amillionpixels.us
  42 //
  43
  44 #include "cd_hull.h"
  45
  46 using namespace ConvexDecomposition;
  47
  48 /*----------------------------------------------------------------------
  49                 Copyright (c) 2004 Open Dynamics Framework Group
  50                                         www.physicstools.org
  51                 All rights reserved.
  52
  53                 Redistribution and use in source and binary forms, with or without modification, are permitted provided
  54                 that the following conditions are met:
  55
  56                 Redistributions of source code must retain the above copyright notice, this list of conditions
  57                 and the following disclaimer.
  58
  59                 Redistributions in binary form must reproduce the above copyright notice,
  60                 this list of conditions and the following disclaimer in the documentation
  61                 and/or other materials provided with the distribution.
  62
  63                 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
  64                 be used to endorse or promote products derived from this software without specific prior written permission.
  65
  66                 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
  67                 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  68                 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  69                 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  70                 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
  71                 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  72                 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  73 -----------------------------------------------------------------------*/
  74
  75 #define PI 3.14159264f
  76
  77 //*****************************************************
  78 //*****************************************************
  79 //********* Stan Melax's vector math template needed
  80 //********* to use his hull building code.
  81 //*****************************************************
  82 //*****************************************************
  83
  84 #define DEG2RAD (PI / 180.0f)
  85 #define RAD2DEG (180.0f / PI)
  86 #define SQRT_OF_2 (1.4142135f)
  87 #define OFFSET(Class,Member)  (((char*) (&(((Class*)NULL)-> Member )))- ((char*)NULL))
  88
  89 namespace ConvexDecomposition
  90 {
  91
  92
  93 int    argmin(float a[],int n);
  94 float  sqr(float a);
  95 float  clampf(float a) ;
  96 float  Round(float a,float precision);
  97 float  Interpolate(const float &f0,const float &f1,float alpha) ;
  98
  99 template <class T>
 100 void Swap(T &a,T &b)
 101 {
 102         T tmp = a;
 103         a=b;
 104         b=tmp;
 105 }
 106
 107
 108
 109 template <class T>
 110 T Max(const T &a,const T &b)
 111 {
 112         return (a>b)?a:b;
 113 }
 114
 115 template <class T>
 116 T Min(const T &a,const T &b)
 117 {
 118         return (a<b)?a:b;
 119 }
 120
 121 //----------------------------------
 122
 123 class int3
 124 {
 125 public:
 126         int x,y,z;
 127         int3(){};
 128         int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
 129         const int& operator[](int i) const {return (&x)[i];}
 130         int& operator[](int i) {return (&x)[i];}
 131 };
 132
 133
 134 //-------- 2D --------
 135
 136 class float2
 137 {
 138 public:
 139         float x,y;
 140         float2(){x=0;y=0;};
 141         float2(float _x,float _y){x=_x;y=_y;}
 142         float& operator[](int i) {assert(i>=0&&i<2);return ((float*)this)[i];}
 143         const float& operator[](int i) const {assert(i>=0&&i<2);return ((float*)this)[i];}
 144 };
 145 inline float2 operator-( const float2& a, const float2& b ){return float2(a.x-b.x,a.y-b.y);}
 146 inline float2 operator+( const float2& a, const float2& b ){return float2(a.x+b.x,a.y+b.y);}
 147
 148 //--------- 3D ---------
 149
 150 class float3 // 3D
 151 {
 152         public:
 153         float x,y,z;
 154         float3(){x=0;y=0;z=0;};
 155         float3(float _x,float _y,float _z){x=_x;y=_y;z=_z;};
 156         //operator float *() { return &x;};
 157         float& operator[](int i) {assert(i>=0&&i<3);return ((float*)this)[i];}
 158         const float& operator[](int i) const {assert(i>=0&&i<3);return ((float*)this)[i];}
 159 #       ifdef PLUGIN_3DSMAX
 160         float3(const Point3 &p):x(p.x),y(p.y),z(p.z){}
 161         operator Point3(){return *((Point3*)this);}
 162 #       endif
 163 };
 164
 165
 166 float3& operator+=( float3 &a, const float3& b );
 167 float3& operator-=( float3 &a ,const float3& b );
 168 float3& operator*=( float3 &v ,const float s );
 169 float3& operator/=( float3 &v, const float s );
 170
 171 float  magnitude( const float3& v );
 172 float3 normalize( const float3& v );
 173 float3 safenormalize(const float3 &v);
 174 float3 vabs(const float3 &v);
 175 float3 operator+( const float3& a, const float3& b );
 176 float3 operator-( const float3& a, const float3& b );
 177 float3 operator-( const float3& v );
 178 float3 operator*( const float3& v, const float s );
 179 float3 operator*( const float s, const float3& v );
 180 float3 operator/( const float3& v, const float s );
 181 inline int operator==( const float3 &a, const float3 &b ) { return (a.x==b.x && a.y==b.y && a.z==b.z); }
 182 inline int operator!=( const float3 &a, const float3 &b ) { return (a.x!=b.x || a.y!=b.y || a.z!=b.z); }
 183 // due to ambiguity and inconsistent standards ther are no overloaded operators for mult such as va*vb.
 184 float  dot( const float3& a, const float3& b );
 185 float3 cmul( const float3 &a, const float3 &b);
 186 float3 cross( const float3& a, const float3& b );
 187 float3 Interpolate(const float3 &v0,const float3 &v1,float alpha);
 188 float3 Round(const float3& a,float precision);
 189 float3  VectorMax(const float3 &a, const float3 &b);
 190 float3  VectorMin(const float3 &a, const float3 &b);
 191
 192
 193
 194 class float3x3
 195 {
 196         public:
 197         float3 x,y,z;  // the 3 rows of the Matrix
 198         float3x3(){}
 199         float3x3(float xx,float xy,float xz,float yx,float yy,float yz,float zx,float zy,float zz):x(xx,xy,xz),y(yx,yy,yz),z(zx,zy,zz){}
 200         float3x3(float3 _x,float3 _y,float3 _z):x(_x),y(_y),z(_z){}
 201         float3&       operator[](int i)       {assert(i>=0&&i<3);return (&x)[i];}
 202         const float3& operator[](int i) const {assert(i>=0&&i<3);return (&x)[i];}
 203         float&        operator()(int r, int c)       {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];}
 204         const float&  operator()(int r, int c) const {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];}
 205 };
 206 float3x3 Transpose( const float3x3& m );
 207 float3   operator*( const float3& v  , const float3x3& m  );
 208 float3   operator*( const float3x3& m , const float3& v   );
 209 float3x3 operator*( const float3x3& m , const float& s   );
 210 float3x3 operator*( const float3x3& ma, const float3x3& mb );
 211 float3x3 operator/( const float3x3& a, const float& s ) ;
 212 float3x3 operator+( const float3x3& a, const float3x3& b );
 213 float3x3 operator-( const float3x3& a, const float3x3& b );
 214 float3x3 &operator+=( float3x3& a, const float3x3& b );
 215 float3x3 &operator-=( float3x3& a, const float3x3& b );
 216 float3x3 &operator*=( float3x3& a, const float& s );
 217 float    Determinant(const float3x3& m );
 218 float3x3 Inverse(const float3x3& a);  // its just 3x3 so we simply do that cofactor method
 219
 220
 221 //-------- 4D Math --------
 222
 223 class float4
 224 {
 225 public:
 226         float x,y,z,w;
 227         float4(){x=0;y=0;z=0;w=0;};
 228         float4(float _x,float _y,float _z,float _w){x=_x;y=_y;z=_z;w=_w;}
 229         float4(const float3 &v,float _w){x=v.x;y=v.y;z=v.z;w=_w;}
 230         //operator float *() { return &x;};
 231         float& operator[](int i) {assert(i>=0&&i<4);return ((float*)this)[i];}
 232         const float& operator[](int i) const {assert(i>=0&&i<4);return ((float*)this)[i];}
 233         const float3& xyz() const { return *((float3*)this);}
 234         float3&       xyz()       { return *((float3*)this);}
 235 };
 236
 237
 238 struct D3DXMATRIX;
 239
 240 class float4x4
 241 {
 242         public:
 243         float4 x,y,z,w;  // the 4 rows
 244         float4x4(){}
 245         float4x4(const float4 &_x, const float4 &_y, const float4 &_z, const float4 &_w):x(_x),y(_y),z(_z),w(_w){}
 246         float4x4(float m00, float m01, float m02, float m03,
 247                                                 float m10, float m11, float m12, float m13,
 248                                 float m20, float m21, float m22, float m23,
 249                                 float m30, float m31, float m32, float m33 )
 250                         :x(m00,m01,m02,m03),y(m10,m11,m12,m13),z(m20,m21,m22,m23),w(m30,m31,m32,m33){}
 251         float&       operator()(int r, int c)       {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];}
 252         const float& operator()(int r, int c) const {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];}
 253                 operator       float* ()       {return &x.x;}
 254                 operator const float* () const {return &x.x;}
 255         operator       struct D3DXMATRIX* ()       { return (struct D3DXMATRIX*) this;}
 256         operator const struct D3DXMATRIX* () const { return (struct D3DXMATRIX*) this;}
 257 };
 258
 259
 260 int     operator==( const float4 &a, const float4 &b );
 261 float4 Homogenize(const float3 &v3,const float &w=1.0f); // Turns a 3D float3 4D vector4 by appending w
 262 float4 cmul( const float4 &a, const float4 &b);
 263 float4 operator*( const float4 &v, float s);
 264 float4 operator*( float s, const float4 &v);
 265 float4 operator+( const float4 &a, const float4 &b);
 266 float4 operator-( const float4 &a, const float4 &b);
 267 float4x4 operator*( const float4x4& a, const float4x4& b );
 268 float4 operator*( const float4& v, const float4x4& m );
 269 float4x4 Inverse(const float4x4 &m);
 270 float4x4 MatrixRigidInverse(const float4x4 &m);
 271 float4x4 MatrixTranspose(const float4x4 &m);
 272 float4x4 MatrixPerspectiveFov(float fovy, float Aspect, float zn, float zf );
 273 float4x4 MatrixTranslation(const float3 &t);
 274 float4x4 MatrixRotationZ(const float angle_radians);
 275 float4x4 MatrixLookAt(const float3& eye, const float3& at, const float3& up);
 276 int     operator==( const float4x4 &a, const float4x4 &b );
 277
 278
 279 //-------- Quaternion ------------
 280
 281 class Quaternion :public float4
 282 {
 283  public:
 284         Quaternion() { x = y = z = 0.0f; w = 1.0f; }
 285         Quaternion( float3 v, float t ) { v = normalize(v); w = cosf(t/2.0f); v = v*sinf(t/2.0f); x = v.x; y = v.y; z = v.z; }
 286         Quaternion(float _x, float _y, float _z, float _w){x=_x;y=_y;z=_z;w=_w;}
 287         float angle() const { return acosf(w)*2.0f; }
 288         float3 axis() const { float3 a(x,y,z); if(fabsf(angle())<0.0000001f) return float3(1,0,0); return a*(1/sinf(angle()/2.0f)); }
 289         float3 xdir() const { return float3( 1-2*(y*y+z*z),  2*(x*y+w*z),  2*(x*z-w*y) ); }
 290         float3 ydir() const { return float3(   2*(x*y-w*z),1-2*(x*x+z*z),  2*(y*z+w*x) ); }
 291         float3 zdir() const { return float3(   2*(x*z+w*y),  2*(y*z-w*x),1-2*(x*x+y*y) ); }
 292         float3x3 getmatrix() const { return float3x3( xdir(), ydir(), zdir() ); }
 293         operator float3x3() { return getmatrix(); }
 294         void Normalize();
 295 };
 296
 297 Quaternion& operator*=(Quaternion& a, float s );
 298 Quaternion      operator*( const Quaternion& a, float s );
 299 Quaternion      operator*( const Quaternion& a, const Quaternion& b);
 300 Quaternion      operator+( const Quaternion& a, const Quaternion& b );
 301 Quaternion      normalize( Quaternion a );
 302 float           dot( const Quaternion &a, const Quaternion &b );
 303 float3          operator*( const Quaternion& q, const float3& v );
 304 float3          operator*( const float3& v, const Quaternion& q );
 305 Quaternion      slerp( Quaternion a, const Quaternion& b, float interp );
 306 Quaternion  Interpolate(const Quaternion &q0,const Quaternion &q1,float alpha);
 307 Quaternion  RotationArc(float3 v0, float3 v1 );  // returns quat q where q*v0=v1
 308 Quaternion  Inverse(const Quaternion &q);
 309 float4x4     MatrixFromQuatVec(const Quaternion &q, const float3 &v);
 310
 311
 312 //------ Euler Angle -----
 313
 314 Quaternion YawPitchRoll( float yaw, float pitch, float roll );
 315 float Yaw( const Quaternion& q );
 316 float Pitch( const Quaternion& q );
 317 float Roll( Quaternion q );
 318 float Yaw( const float3& v );
 319 float Pitch( const float3& v );
 320
 321
 322 //------- Plane ----------
 323
 324 class Plane
 325 {
 326         public:
 327         float3  normal;
 328         float   dist;   // distance below origin - the D from plane equasion Ax+By+Cz+D=0
 329                         Plane(const float3 &n,float d):normal(n),dist(d){}
 330                         Plane():normal(),dist(0){}
 331         void    Transform(const float3 &position, const Quaternion &orientation);
 332 };
 333
 334 inline Plane PlaneFlip(const Plane &plane){return Plane(-plane.normal,-plane.dist);}
 335 inline int operator==( const Plane &a, const Plane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
 336 inline int coplanar( const Plane &a, const Plane &b ) { return (a==b || a==PlaneFlip(b)); }
 337
 338
 339 //--------- Utility Functions ------
 340
 341 float3  PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1);
 342 float3  PlaneProject(const Plane &plane, const float3 &point);
 343 float3  LineProject(const float3 &p0, const float3 &p1, const float3 &a);  // projects a onto infinite line p0p1
 344 float   LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a);
 345 float3  ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2);
 346 int     PolyHit(const float3 *vert,const int n,const float3 &v0, const float3 &v1, float3 *impact=NULL, float3 *normal=NULL);
 347 int     BoxInside(const float3 &p,const float3 &bmin, const float3 &bmax) ;
 348 int     BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax, float3 *impact);
 349 float   DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint=NULL, float3 *vpoint=NULL);
 350 float3  TriNormal(const float3 &v0, const float3 &v1, const float3 &v2);
 351 float3  NormalOf(const float3 *vert, const int n);
 352 Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir0, const float3 &dir1);
 353
 354
 355 float   sqr(float a) {return a*a;}
 356 float   clampf(float a) {return Min(1.0f,Max(0.0f,a));}
 357
 358
 359 float Round(float a,float precision)
 360 {
 361         return floorf(0.5f+a/precision)*precision;
 362 }
 363
 364
 365 float Interpolate(const float &f0,const float &f1,float alpha)
 366 {
 367         return f0*(1-alpha) + f1*alpha;
 368 }
 369
 370
 371 int     argmin(float a[],int n)
 372 {
 373         int r=0;
 374         for(int i=1;i<n;i++)
 375                 {
 376                 if(a[i]<a[r])
 377                                 {
 378                         r = i;
 379                 }
 380         }
 381         return r;
 382 }
 383
 384
 385
 386 //------------ float3 (3D) --------------
 387
 388
 389
 390 float3 operator+( const float3& a, const float3& b )
 391 {
 392         return float3(a.x+b.x, a.y+b.y, a.z+b.z);
 393 }
 394
 395
 396 float3 operator-( const float3& a, const float3& b )
 397 {
 398         return float3( a.x-b.x, a.y-b.y, a.z-b.z );
 399 }
 400
 401
 402 float3 operator-( const float3& v )
 403 {
 404         return float3( -v.x, -v.y, -v.z );
 405 }
 406
 407
 408 float3 operator*( const float3& v, float s )
 409 {
 410         return float3( v.x*s, v.y*s, v.z*s );
 411 }
 412
 413
 414 float3 operator*( float s, const float3& v )
 415 {
 416         return float3( v.x*s, v.y*s, v.z*s );
 417 }
 418
 419
 420 float3 operator/( const float3& v, float s )
 421 {
 422         return v*(1.0f/s);
 423 }
 424
 425 float  dot( const float3& a, const float3& b )
 426 {
 427         return a.x*b.x + a.y*b.y + a.z*b.z;
 428 }
 429
 430 float3 cmul( const float3 &v1, const float3 &v2)
 431 {
 432         return float3(v1.x*v2.x, v1.y*v2.y, v1.z*v2.z);
 433 }
 434
 435
 436 float3 cross( const float3& a, const float3& b )
 437 {
 438                 return float3( a.y*b.z - a.z*b.y,
 439                                                                          a.z*b.x - a.x*b.z,
 440                                                                          a.x*b.y - a.y*b.x );
 441 }
 442
 443
 444
 445
 446 float3& operator+=( float3& a , const float3& b )
 447 {
 448                 a.x += b.x;
 449                 a.y += b.y;
 450                 a.z += b.z;
 451                 return a;
 452 }
 453
 454
 455 float3& operator-=( float3& a , const float3& b )
 456 {
 457                 a.x -= b.x;
 458                 a.y -= b.y;
 459                 a.z -= b.z;
 460                 return a;
 461 }
 462
 463
 464 float3& operator*=(float3& v , float s )
 465 {
 466                 v.x *= s;
 467                 v.y *= s;
 468                 v.z *= s;
 469                 return v;
 470 }
 471
 472
 473 float3& operator/=(float3& v , float s )
 474 {
 475                 float sinv = 1.0f / s;
 476                 v.x *= sinv;
 477                 v.y *= sinv;
 478                 v.z *= sinv;
 479                 return v;
 480 }
 481
 482 float3 vabs(const float3 &v)
 483 {
 484         return float3(fabsf(v.x),fabsf(v.y),fabsf(v.z));
 485 }
 486
 487
 488 float magnitude( const float3& v )
 489 {
 490                 return sqrtf(sqr(v.x) + sqr( v.y)+ sqr(v.z));
 491 }
 492
 493
 494
 495 float3 normalize( const float3 &v )
 496 {
 497         // this routine, normalize, is ok, provided magnitude works!!
 498                 float d=magnitude(v);
 499                 if (d==0)
 500                 {
 501                 printf("Cant normalize ZERO vector\n");
 502                 assert(0);// yes this could go here
 503                 d=0.1f;
 504         }
 505         d = 1/d;
 506         return float3(v.x*d,v.y*d,v.z*d);
 507 }
 508
 509 float3 safenormalize(const float3 &v)
 510 {
 511         if(magnitude(v)<=0.0f)
 512         {
 513                 return float3(1,0,0);
 514         }
 515         return normalize(v);
 516 }
 517
 518 float3 Round(const float3 &a,float precision)
 519 {
 520         return float3(Round(a.x,precision),Round(a.y,precision),Round(a.z,precision));
 521 }
 522
 523
 524 float3 Interpolate(const float3 &v0,const float3 &v1,float alpha)
 525 {
 526         return v0*(1-alpha) + v1*alpha;
 527 }
 528
 529 float3 VectorMin(const float3 &a,const float3 &b)
 530 {
 531         return float3(Min(a.x,b.x),Min(a.y,b.y),Min(a.z,b.z));
 532 }
 533 float3 VectorMax(const float3 &a,const float3 &b)
 534 {
 535         return float3(Max(a.x,b.x),Max(a.y,b.y),Max(a.z,b.z));
 536 }
 537
 538 // the statement v1*v2 is ambiguous since there are 3 types
 539 // of vector multiplication
 540 //  - componantwise (for example combining colors)
 541 //  - dot product
 542 //  - cross product
 543 // Therefore we never declare/implement this function.
 544 // So we will never see:  float3 operator*(float3 a,float3 b)
 545
 546
 547
 548
 549 //------------ float3x3 ---------------
 550 float Determinant(const float3x3 &m)
 551 {
 552         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
 553                          -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 ;
 554 }
 555
 556 float3x3 Inverse(const float3x3 &a)
 557 {
 558         float3x3 b;
 559         float d=Determinant(a);
 560         assert(d!=0);
 561         for(int i=0;i<3;i++)
 562                 {
 563                 for(int j=0;j<3;j++)
 564                                 {
 565                         int i1=(i+1)%3;
 566                         int i2=(i+2)%3;
 567                         int j1=(j+1)%3;
 568                         int j2=(j+2)%3;
 569                         // reverse indexs i&j to take transpose
 570                         b[j][i] = (a[i1][j1]*a[i2][j2]-a[i1][j2]*a[i2][j1])/d;
 571                 }
 572         }
 573         // Matrix check=a*b; // Matrix 'check' should be the identity (or close to it)
 574         return b;
 575 }
 576
 577
 578 float3x3 Transpose( const float3x3& m )
 579 {
 580         return float3x3( float3(m.x.x,m.y.x,m.z.x),
 581                                         float3(m.x.y,m.y.y,m.z.y),
 582                                         float3(m.x.z,m.y.z,m.z.z));
 583 }
 584
 585
 586 float3 operator*(const float3& v , const float3x3 &m ) {
 587         return float3((m.x.x*v.x + m.y.x*v.y + m.z.x*v.z),
 588                                         (m.x.y*v.x + m.y.y*v.y + m.z.y*v.z),
 589                                         (m.x.z*v.x + m.y.z*v.y + m.z.z*v.z));
 590 }
 591 float3 operator*(const float3x3 &m,const float3& v  ) {
 592         return float3(dot(m.x,v),dot(m.y,v),dot(m.z,v));
 593 }
 594
 595
 596 float3x3 operator*( const float3x3& a, const float3x3& b )
 597 {
 598         return float3x3(a.x*b,a.y*b,a.z*b);
 599 }
 600
 601 float3x3 operator*( const float3x3& a, const float& s )
 602 {
 603         return float3x3(a.x*s, a.y*s ,a.z*s);
 604 }
 605 float3x3 operator/( const float3x3& a, const float& s )
 606 {
 607         float t=1/s;
 608         return float3x3(a.x*t, a.y*t ,a.z*t);
 609 }
 610 float3x3 operator+( const float3x3& a, const float3x3& b )
 611 {
 612         return float3x3(a.x+b.x, a.y+b.y, a.z+b.z);
 613 }
 614 float3x3 operator-( const float3x3& a, const float3x3& b )
 615 {
 616         return float3x3(a.x-b.x, a.y-b.y, a.z-b.z);
 617 }
 618 float3x3 &operator+=( float3x3& a, const float3x3& b )
 619 {
 620         a.x+=b.x;
 621         a.y+=b.y;
 622         a.z+=b.z;
 623         return a;
 624 }
 625 float3x3 &operator-=( float3x3& a, const float3x3& b )
 626 {
 627         a.x-=b.x;
 628         a.y-=b.y;
 629         a.z-=b.z;
 630         return a;
 631 }
 632 float3x3 &operator*=( float3x3& a, const float& s )
 633 {
 634         a.x*=s;
 635         a.y*=s;
 636         a.z*=s;
 637         return a;
 638 }
 639
 640
 641
 642 float3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2){
 643         float3x3 mp =Transpose(float3x3(p0.normal,p1.normal,p2.normal));
 644         float3x3 mi = Inverse(mp);
 645         float3 b(p0.dist,p1.dist,p2.dist);
 646         return -b * mi;
 647 }
 648
 649
 650 //--------------- 4D ----------------
 651
 652 float4   operator*( const float4&   v, const float4x4& m )
 653 {
 654         return v.x*m.x + v.y*m.y + v.z*m.z + v.w*m.w; // yes this actually works
 655 }
 656
 657 int operator==( const float4 &a, const float4 &b )
 658 {
 659         return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);
 660 }
 661
 662
 663 //  Dont implement m*v for now, since that might confuse us
 664 //  All our transforms are based on multiplying the "row" vector on the left
 665 //float4   operator*(const float4x4& m , const float4&   v )
 666 //{
 667 //      return float4(dot(v,m.x),dot(v,m.y),dot(v,m.z),dot(v,m.w));
 668 //}
 669
 670
 671
 672 float4 cmul( const float4 &a, const float4 &b)
 673 {
 674         return float4(a.x*b.x,a.y*b.y,a.z*b.z,a.w*b.w);
 675 }
 676
 677
 678 float4 operator*( const float4 &v, float s)
 679 {
 680         return float4(v.x*s,v.y*s,v.z*s,v.w*s);
 681 }
 682
 683
 684 float4 operator*( float s, const float4 &v)
 685 {
 686         return float4(v.x*s,v.y*s,v.z*s,v.w*s);
 687 }
 688
 689
 690 float4 operator+( const float4 &a, const float4 &b)
 691 {
 692         return float4(a.x+b.x,a.y+b.y,a.z+b.z,a.w+b.w);
 693 }
 694
 695
 696
 697 float4 operator-( const float4 &a, const float4 &b)
 698 {
 699         return float4(a.x-b.x,a.y-b.y,a.z-b.z,a.w-b.w);
 700 }
 701
 702
 703 float4 Homogenize(const float3 &v3,const float &w)
 704 {
 705         return float4(v3.x,v3.y,v3.z,w);
 706 }
 707
 708
 709
 710 float4x4 operator*( const float4x4& a, const float4x4& b )
 711 {
 712         return float4x4(a.x*b,a.y*b,a.z*b,a.w*b);
 713 }
 714
 715 float4x4 MatrixTranspose(const float4x4 &m)
 716 {
 717         return float4x4(
 718                 m.x.x, m.y.x, m.z.x, m.w.x,
 719                 m.x.y, m.y.y, m.z.y, m.w.y,
 720                 m.x.z, m.y.z, m.z.z, m.w.z,
 721                 m.x.w, m.y.w, m.z.w, m.w.w );
 722 }
 723
 724 float4x4 MatrixRigidInverse(const float4x4 &m)
 725 {
 726         float4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
 727         float4x4 rot   = m;
 728         rot.w = float4(0,0,0,1);
 729         return trans_inverse * MatrixTranspose(rot);
 730 }
 731
 732
 733 float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf )
 734 {
 735         float h = 1.0f/tanf(fovy/2.0f); // view space height
 736         float w = h / aspect ;  // view space width
 737         return float4x4(
 738                 w, 0, 0             ,   0,
 739                 0, h, 0             ,   0,
 740                 0, 0, zf/(zn-zf)    ,  -1,
 741                 0, 0, zn*zf/(zn-zf) ,   0 );
 742 }
 743
 744
 745
 746 float4x4 MatrixLookAt(const float3& eye, const float3& at, const float3& up)
 747 {
 748         float4x4 m;
 749         m.w.w = 1.0f;
 750         m.w.xyz() = eye;
 751         m.z.xyz() = normalize(eye-at);
 752         m.x.xyz() = normalize(cross(up,m.z.xyz()));
 753         m.y.xyz() = cross(m.z.xyz(),m.x.xyz());
 754         return MatrixRigidInverse(m);
 755 }
 756
 757
 758 float4x4 MatrixTranslation(const float3 &t)
 759 {
 760         return float4x4(
 761                 1,  0,  0,  0,
 762                 0,  1,  0,  0,
 763                 0,  0,  1,  0,
 764                 t.x,t.y,t.z,1 );
 765 }
 766
 767
 768 float4x4 MatrixRotationZ(const float angle_radians)
 769 {
 770         float s =  sinf(angle_radians);
 771         float c =  cosf(angle_radians);
 772         return float4x4(
 773                 c,  s,  0,  0,
 774                 -s, c,  0,  0,
 775                 0,  0,  1,  0,
 776                 0,  0,  0,  1 );
 777 }
 778
 779
 780
 781 int operator==( const float4x4 &a, const float4x4 &b )
 782 {
 783         return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);
 784 }
 785
 786
 787 float4x4 Inverse(const float4x4 &m)
 788 {
 789         float4x4 d;
 790         float *dst = &d.x.x;
 791         float tmp[12]; /* temp array for pairs */
 792         float src[16]; /* array of transpose source matrix */
 793         float det; /* determinant */
 794         /* transpose matrix */
 795         for ( int i = 0; i < 4; i++) {
 796                 src[i] = m(i,0) ;
 797                 src[i + 4] = m(i,1);
 798                 src[i + 8] = m(i,2);
 799                 src[i + 12] = m(i,3);
 800         }
 801         /* calculate pairs for first 8 elements (cofactors) */
 802         tmp[0]  = src[10] * src[15];
 803         tmp[1]  = src[11] * src[14];
 804         tmp[2]  = src[9] * src[15];
 805         tmp[3]  = src[11] * src[13];
 806         tmp[4]  = src[9] * src[14];
 807         tmp[5]  = src[10] * src[13];
 808         tmp[6]  = src[8] * src[15];
 809         tmp[7]  = src[11] * src[12];
 810         tmp[8]  = src[8] * src[14];
 811         tmp[9]  = src[10] * src[12];
 812         tmp[10] = src[8] * src[13];
 813         tmp[11] = src[9] * src[12];
 814         /* calculate first 8 elements (cofactors) */
 815         dst[0]  = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
 816         dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
 817         dst[1]  = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
 818         dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
 819         dst[2]  = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
 820         dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
 821         dst[3]  = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
 822         dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
 823         dst[4]  = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
 824         dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
 825         dst[5]  = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
 826         dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
 827         dst[6]  = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
 828         dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
 829         dst[7]  = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
 830         dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
 831         /* calculate pairs for second 8 elements (cofactors) */
 832         tmp[0]  = src[2]*src[7];
 833         tmp[1]  = src[3]*src[6];
 834         tmp[2]  = src[1]*src[7];
 835         tmp[3]  = src[3]*src[5];
 836         tmp[4]  = src[1]*src[6];
 837         tmp[5]  = src[2]*src[5];
 838         tmp[6]  = src[0]*src[7];
 839         tmp[7]  = src[3]*src[4];
 840         tmp[8]  = src[0]*src[6];
 841         tmp[9]  = src[2]*src[4];
 842         tmp[10] = src[0]*src[5];
 843         tmp[11] = src[1]*src[4];
 844         /* calculate second 8 elements (cofactors) */
 845         dst[8]  = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
 846         dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
 847         dst[9]  = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
 848         dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
 849         dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
 850         dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
 851         dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
 852         dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
 853         dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
 854         dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
 855         dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
 856         dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
 857         dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
 858         dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
 859         dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
 860         dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
 861         /* calculate determinant */
 862         det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
 863         /* calculate matrix inverse */
 864         det = 1/det;
 865         for ( int j = 0; j < 16; j++)
 866         dst[j] *= det;
 867         return d;
 868 }
 869
 870
 871 //--------- Quaternion --------------
 872
 873 Quaternion operator*( const Quaternion& a, const Quaternion& b )
 874 {
 875         Quaternion c;
 876         c.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
 877         c.x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;
 878         c.y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x;
 879         c.z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w;
 880         return c;
 881 }
 882
 883
 884 Quaternion operator*( const Quaternion& a, float b )
 885 {
 886         return Quaternion(a.x*b, a.y*b, a.z*b ,a.w*b);
 887 }
 888
 889 Quaternion  Inverse(const Quaternion &q)
 890 {
 891         return Quaternion(-q.x,-q.y,-q.z,q.w);
 892 }
 893
 894 Quaternion& operator*=( Quaternion& q, const float s )
 895 {
 896                 q.x *= s;
 897                 q.y *= s;
 898                 q.z *= s;
 899                 q.w *= s;
 900                 return q;
 901 }
 902 void Quaternion::Normalize()
 903 {
 904         float m = sqrtf(sqr(w)+sqr(x)+sqr(y)+sqr(z));
 905         if(m<0.000000001f) {
 906                 w=1.0f;
 907                 x=y=z=0.0f;
 908                 return;
 909         }
 910         (*this) *= (1.0f/m);
 911 }
 912
 913 float3 operator*( const Quaternion& q, const float3& v )
 914 {
 915         // The following is equivalent to:
 916         //return (q.getmatrix() * v);
 917         float qx2 = q.x*q.x;
 918         float qy2 = q.y*q.y;
 919         float qz2 = q.z*q.z;
 920
 921         float qxqy = q.x*q.y;
 922         float qxqz = q.x*q.z;
 923         float qxqw = q.x*q.w;
 924         float qyqz = q.y*q.z;
 925         float qyqw = q.y*q.w;
 926         float qzqw = q.z*q.w;
 927         return float3(
 928                 (1-2*(qy2+qz2))*v.x + (2*(qxqy-qzqw))*v.y + (2*(qxqz+qyqw))*v.z ,
 929                 (2*(qxqy+qzqw))*v.x + (1-2*(qx2+qz2))*v.y + (2*(qyqz-qxqw))*v.z ,
 930                 (2*(qxqz-qyqw))*v.x + (2*(qyqz+qxqw))*v.y + (1-2*(qx2+qy2))*v.z  );
 931 }
 932
 933 float3 operator*( const float3& v, const Quaternion& q )
 934 {
 935         assert(0);  // must multiply with the quat on the left
 936         return float3(0.0f,0.0f,0.0f);
 937 }
 938
 939 Quaternion operator+( const Quaternion& a, const Quaternion& b )
 940 {
 941         return Quaternion(a.x+b.x, a.y+b.y, a.z+b.z, a.w+b.w);
 942 }
 943
 944 float dot( const Quaternion &a,const Quaternion &b )
 945 {
 946         return  (a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z);
 947 }
 948
 949 Quaternion normalize( Quaternion a )
 950 {
 951         float m = sqrtf(sqr(a.w)+sqr(a.x)+sqr(a.y)+sqr(a.z));
 952         if(m<0.000000001)
 953                 {
 954                 a.w=1;
 955                 a.x=a.y=a.z=0;
 956                 return a;
 957         }
 958         return a * (1/m);
 959 }
 960
 961 Quaternion slerp( Quaternion a, const Quaternion& b, float interp )
 962 {
 963         if(dot(a,b) <0.0)
 964                 {
 965                 a.w=-a.w;
 966                 a.x=-a.x;
 967                 a.y=-a.y;
 968                 a.z=-a.z;
 969         }
 970         float d = dot(a,b);
 971         if(d>=1.0) {
 972                 return a;
 973         }
 974         float theta = acosf(d);
 975         if(theta==0.0f) { return(a);}
 976         return a*(sinf(theta-interp*theta)/sinf(theta)) + b*(sinf(interp*theta)/sinf(theta));
 977 }
 978
 979
 980 Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,float alpha) {
 981         return slerp(q0,q1,alpha);
 982 }
 983
 984
 985 Quaternion YawPitchRoll( float yaw, float pitch, float roll )
 986 {
 987         roll  *= DEG2RAD;
 988         yaw   *= DEG2RAD;
 989         pitch *= DEG2RAD;
 990         return Quaternion(float3(0.0f,0.0f,1.0f),yaw)*Quaternion(float3(1.0f,0.0f,0.0f),pitch)*Quaternion(float3(0.0f,1.0f,0.0f),roll);
 991 }
 992
 993 float Yaw( const Quaternion& q )
 994 {
 995         float3 v;
 996         v=q.ydir();
 997         return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG;
 998 }
 999
1000 float Pitch( const Quaternion& q )
1001 {
1002         float3 v;
1003         v=q.ydir();
1004         return atan2f(v.z,sqrtf(sqr(v.x)+sqr(v.y)))*RAD2DEG;
1005 }
1006
1007 float Roll( Quaternion q )
1008 {
1009         q = Quaternion(float3(0.0f,0.0f,1.0f),-Yaw(q)*DEG2RAD)  *q;
1010         q = Quaternion(float3(1.0f,0.0f,0.0f),-Pitch(q)*DEG2RAD)  *q;
1011         return atan2f(-q.xdir().z,q.xdir().x)*RAD2DEG;
1012 }
1013
1014 float Yaw( const float3& v )
1015 {
1016         return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG;
1017 }
1018
1019 float Pitch( const float3& v )
1020 {
1021         return atan2f(v.z,sqrtf(sqr(v.x)+sqr(v.y)))*RAD2DEG;
1022 }
1023
1024
1025 //------------- Plane --------------
1026
1027
1028 void Plane::Transform(const float3 &position, const Quaternion &orientation) {
1029         //   Transforms the plane to the space defined by the
1030         //   given position/orientation.
1031         float3 newnormal;
1032         float3 origin;
1033
1034         newnormal = Inverse(orientation)*normal;
1035         origin = Inverse(orientation)*(-normal*dist - position);
1036
1037         normal = newnormal;
1038         dist = -dot(newnormal, origin);
1039 }
1040
1041
1042
1043
1044 //--------- utility functions -------------
1045
1046 //        RotationArc()
1047 // Given two vectors v0 and v1 this function
1048 // returns quaternion q where q*v0==v1.
1049 // Routine taken from game programming gems.
1050 Quaternion RotationArc(float3 v0,float3 v1){
1051         Quaternion q;
1052         v0 = normalize(v0);  // Comment these two lines out if you know its not needed.
1053         v1 = normalize(v1);  // If vector is already unit length then why do it again?
1054         float3  c = cross(v0,v1);
1055         float   d = dot(v0,v1);
1056         if(d<=-1.0f) { return Quaternion(1,0,0,0);} // 180 about x axis
1057         float   s = sqrtf((1+d)*2);
1058         q.x = c.x / s;
1059         q.y = c.y / s;
1060         q.z = c.z / s;
1061         q.w = s /2.0f;
1062         return q;
1063 }
1064
1065
1066 float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v)
1067 {
1068         // builds a 4x4 transformation matrix based on orientation q and translation v
1069         float qx2 = q.x*q.x;
1070         float qy2 = q.y*q.y;
1071         float qz2 = q.z*q.z;
1072
1073         float qxqy = q.x*q.y;
1074         float qxqz = q.x*q.z;
1075         float qxqw = q.x*q.w;
1076         float qyqz = q.y*q.z;
1077         float qyqw = q.y*q.w;
1078         float qzqw = q.z*q.w;
1079
1080         return float4x4(
1081                 1-2*(qy2+qz2),
1082                 2*(qxqy+qzqw),
1083                 2*(qxqz-qyqw),
1084                 0            ,
1085                 2*(qxqy-qzqw),
1086                 1-2*(qx2+qz2),
1087                 2*(qyqz+qxqw),
1088                 0            ,
1089                 2*(qxqz+qyqw),
1090                 2*(qyqz-qxqw),
1091                 1-2*(qx2+qy2),
1092                 0    ,
1093                  v.x ,
1094                  v.y ,
1095                  v.z ,
1096                  1.0f );
1097 }
1098
1099
1100 float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1)
1101 {
1102         // returns the point where the line p0-p1 intersects the plane n&d
1103                 float3 dif;
1104                 dif = p1-p0;
1105                                 float dn= dot(plane.normal,dif);
1106                                 float t = -(plane.dist+dot(plane.normal,p0) )/dn;
1107                                 return p0 + (dif*t);
1108 }
1109
1110 float3 PlaneProject(const Plane &plane, const float3 &point)
1111 {
1112         return point - plane.normal * (dot(point,plane.normal)+plane.dist);
1113 }
1114
1115 float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a)
1116 {
1117         float3 w;
1118         w = p1-p0;
1119         float t= dot(w,(a-p0)) / (sqr(w.x)+sqr(w.y)+sqr(w.z));
1120         return p0+ w*t;
1121 }
1122
1123
1124 float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a)
1125 {
1126         float3 w;
1127         w = p1-p0;
1128         float t= dot(w,(a-p0)) / (sqr(w.x)+sqr(w.y)+sqr(w.z));
1129         return t;
1130 }
1131
1132
1133
1134 float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2)
1135 {
1136         // return the normal of the triangle
1137         // inscribed by v0, v1, and v2
1138         float3 cp=cross(v1-v0,v2-v1);
1139         float m=magnitude(cp);
1140         if(m==0) return float3(1,0,0);
1141         return cp*(1.0f/m);
1142 }
1143
1144
1145
1146 int BoxInside(const float3 &p, const float3 &bmin, const float3 &bmax)
1147 {
1148         return (p.x >= bmin.x && p.x <=bmax.x &&
1149                         p.y >= bmin.y && p.y <=bmax.y &&
1150                         p.z >= bmin.z && p.z <=bmax.z );
1151 }
1152
1153
1154 int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax,float3 *impact)
1155 {
1156         if(BoxInside(v0,bmin,bmax))
1157                 {
1158                                 *impact=v0;
1159                                 return 1;
1160                 }
1161         if(v0.x<=bmin.x && v1.x>=bmin.x)
1162                 {
1163                 float a = (bmin.x-v0.x)/(v1.x-v0.x);
1164                 //v.x = bmin.x;
1165                 float vy =  (1-a) *v0.y + a*v1.y;
1166                 float vz =  (1-a) *v0.z + a*v1.z;
1167                 if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z)
1168                                 {
1169                         impact->x = bmin.x;
1170                         impact->y = vy;
1171                         impact->z = vz;
1172                         return 1;
1173                 }
1174         }
1175         else if(v0.x >= bmax.x  &&  v1.x <= bmax.x)
1176                 {
1177                 float a = (bmax.x-v0.x)/(v1.x-v0.x);
1178                 //v.x = bmax.x;
1179                 float vy =  (1-a) *v0.y + a*v1.y;
1180                 float vz =  (1-a) *v0.z + a*v1.z;
1181                 if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z)
1182                                 {
1183                         impact->x = bmax.x;
1184                         impact->y = vy;
1185                         impact->z = vz;
1186                         return 1;
1187                 }
1188         }
1189         if(v0.y<=bmin.y && v1.y>=bmin.y)
1190                 {
1191                 float a = (bmin.y-v0.y)/(v1.y-v0.y);
1192                 float vx =  (1-a) *v0.x + a*v1.x;
1193                 //v.y = bmin.y;
1194                 float vz =  (1-a) *v0.z + a*v1.z;
1195                 if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z)
1196                                 {
1197                         impact->x = vx;
1198                         impact->y = bmin.y;
1199                         impact->z = vz;
1200                         return 1;
1201                 }
1202         }
1203         else if(v0.y >= bmax.y  &&  v1.y <= bmax.y)
1204                 {
1205                 float a = (bmax.y-v0.y)/(v1.y-v0.y);
1206                 float vx =  (1-a) *v0.x + a*v1.x;
1207                 // vy = bmax.y;
1208                 float vz =  (1-a) *v0.z + a*v1.z;
1209                 if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z)
1210                                 {
1211                         impact->x = vx;
1212                         impact->y = bmax.y;
1213                         impact->z = vz;
1214                         return 1;
1215                 }
1216         }
1217         if(v0.z<=bmin.z && v1.z>=bmin.z)
1218                 {
1219                 float a = (bmin.z-v0.z)/(v1.z-v0.z);
1220                 float vx =  (1-a) *v0.x + a*v1.x;
1221                 float vy =  (1-a) *v0.y + a*v1.y;
1222                 // v.z = bmin.z;
1223                 if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x)
1224                                 {
1225                         impact->x = vx;
1226                         impact->y = vy;
1227                         impact->z = bmin.z;
1228                         return 1;
1229                 }
1230         }
1231         else if(v0.z >= bmax.z  &&  v1.z <= bmax.z)
1232                 {
1233                 float a = (bmax.z-v0.z)/(v1.z-v0.z);
1234                 float vx =  (1-a) *v0.x + a*v1.x;
1235                 float vy =  (1-a) *v0.y + a*v1.y;
1236                 // v.z = bmax.z;
1237                 if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x)
1238                                 {
1239                         impact->x = vx;
1240                         impact->y = vy;
1241                         impact->z = bmax.z;
1242                         return 1;
1243                 }
1244         }
1245         return 0;
1246 }
1247
1248
1249 float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint, float3 *vpoint)
1250 {
1251         float3 cp;
1252         cp = normalize(cross(udir,vdir));
1253
1254         float distu = -dot(cp,ustart);
1255         float distv = -dot(cp,vstart);
1256         float dist = (float)fabs(distu-distv);
1257         if(upoint)
1258                 {
1259                 Plane plane;
1260                 plane.normal = normalize(cross(vdir,cp));
1261                 plane.dist = -dot(plane.normal,vstart);
1262                 *upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
1263         }
1264         if(vpoint)
1265                 {
1266                 Plane plane;
1267                 plane.normal = normalize(cross(udir,cp));
1268                 plane.dist = -dot(plane.normal,ustart);
1269                 *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
1270         }
1271         return dist;
1272 }
1273
1274
1275 Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir1, const float3 &dir2)
1276 {
1277         // routine taken from game programming gems.
1278         // Implement track ball functionality to spin stuf on the screen
1279         //  cop   center of projection
1280         //  cor   center of rotation
1281         //  dir1  old mouse direction
1282         //  dir2  new mouse direction
1283         // pretend there is a sphere around cor.  Then find the points
1284         // where dir1 and dir2 intersect that sphere.  Find the
1285         // rotation that takes the first point to the second.
1286         float m;
1287         // compute plane
1288         float3 nrml = cor - cop;
1289         float fudgefactor = 1.0f/(magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop
1290         nrml = normalize(nrml);
1291         float dist = -dot(nrml,cor);
1292         float3 u= PlaneLineIntersection(Plane(nrml,dist),cop,cop+dir1);
1293         u=u-cor;
1294         u=u*fudgefactor;
1295         m= magnitude(u);
1296         if(m>1)
1297                 {
1298                                 u/=m;
1299                 }
1300         else
1301                 {
1302                 u=u - (nrml * sqrtf(1-m*m));
1303         }
1304         float3 v= PlaneLineIntersection(Plane(nrml,dist),cop,cop+dir2);
1305         v=v-cor;
1306         v=v*fudgefactor;
1307         m= magnitude(v);
1308         if(m>1)
1309                 {
1310                                 v/=m;
1311                 }
1312         else
1313                 {
1314                 v=v - (nrml * sqrtf(1-m*m));
1315         }
1316         return RotationArc(u,v);
1317 }
1318
1319
1320 int countpolyhit=0;
1321 int PolyHit(const float3 *vert, const int n, const float3 &v0, const float3 &v1, float3 *impact, float3 *normal)
1322 {
1323         countpolyhit++;
1324         int i;
1325         float3 nrml(0,0,0);
1326         for(i=0;i<n;i++)
1327                 {
1328                 int i1=(i+1)%n;
1329                 int i2=(i+2)%n;
1330                 nrml = nrml + cross(vert[i1]-vert[i],vert[i2]-vert[i1]);
1331         }
1332
1333         float m = magnitude(nrml);
1334         if(m==0.0)
1335                 {
1336                                 return 0;
1337                 }
1338         nrml = nrml * (1.0f/m);
1339         float dist = -dot(nrml,vert[0]);
1340         float d0,d1;
1341         if((d0=dot(v0,nrml)+dist) <0  ||  (d1=dot(v1,nrml)+dist) >0)
1342                 {
1343                                 return 0;
1344                 }
1345
1346         float3 the_point;
1347         // By using the cached plane distances d0 and d1
1348         // we can optimize the following:
1349         //     the_point = planelineintersection(nrml,dist,v0,v1);
1350         float a = d0/(d0-d1);
1351         the_point = v0*(1-a) + v1*a;
1352
1353
1354         int inside=1;
1355         for(int j=0;inside && j<n;j++)
1356                 {
1357                         // let inside = 0 if outside
1358                         float3 pp1,pp2,side;
1359                         pp1 = vert[j] ;
1360                         pp2 = vert[(j+1)%n];
1361                         side = cross((pp2-pp1),(the_point-pp1));
1362                         inside = (dot(nrml,side) >= 0.0);
1363         }
1364         if(inside)
1365                 {
1366                 if(normal){*normal=nrml;}
1367                 if(impact){*impact=the_point;}
1368         }
1369         return inside;
1370 }
1371
1372 //**************************************************************************
1373 //**************************************************************************
1374 //*** Stan Melax's array template, needed to compile his hull generation code
1375 //**************************************************************************
1376 //**************************************************************************
1377
1378 template <class Type> class ArrayRet;
1379 template <class Type> class Array {
1380         public:
1381                                 Array(int s=0);
1382                                 Array(Array<Type> &array);
1383                                 Array(ArrayRet<Type> &array);
1384                                 ~Array();
1385         void            allocate(int s);
1386         void            SetSize(int s);
1387         void            Pack();
1388         Type&           Add(Type);
1389         void            AddUnique(Type);
1390         int             Contains(Type);
1391         void            Insert(Type,int);
1392         int                     IndexOf(Type);
1393         void            Remove(Type);
1394         void            DelIndex(int i);
1395         Type *          element;
1396         int                     count;
1397         int                     array_size;
1398         const Type      &operator[](int i) const { assert(i>=0 && i<count);  return element[i]; }
1399         Type            &operator[](int i)  { assert(i>=0 && i<count);  return element[i]; }
1400         Type            &Pop() { assert(count); count--;  return element[count]; }
1401         Array<Type> &operator=(Array<Type> &array);
1402         Array<Type> &operator=(ArrayRet<Type> &array);
1403         // operator ArrayRet<Type> &() { return *(ArrayRet<Type> *)this;} // this worked but i suspect could be dangerous
1404 };
1405
1406 template <class Type> class ArrayRet:public Array<Type>
1407 {
1408 };
1409
1410 template <class Type> Array<Type>::Array(int s)
1411 {
1412         count=0;
1413         array_size = 0;
1414         element = NULL;
1415         if(s)
1416         {
1417                 allocate(s);
1418         }
1419 }
1420
1421
1422 template <class Type> Array<Type>::Array(Array<Type> &array)
1423 {
1424         count=0;
1425         array_size = 0;
1426         element = NULL;
1427         for(int i=0;i<array.count;i++)
1428         {
1429                 Add(array[i]);
1430         }
1431 }
1432
1433
1434 template <class Type> Array<Type>::Array(ArrayRet<Type> &array)
1435 {
1436         *this = array;
1437 }
1438 template <class Type> Array<Type> &Array<Type>::operator=(ArrayRet<Type> &array)
1439 {
1440         count=array.count;
1441         array_size = array.array_size;
1442         element = array.element;
1443         array.element=NULL;
1444         array.count=0;
1445         array.array_size=0;
1446         return *this;
1447 }
1448
1449
1450 template <class Type> Array<Type> &Array<Type>::operator=(Array<Type> &array)
1451 {
1452         count=0;
1453         for(int i=0;i<array.count;i++)
1454         {
1455                 Add(array[i]);
1456         }
1457         return *this;
1458 }
1459
1460 template <class Type> Array<Type>::~Array()
1461 {
1462         if (element != NULL)
1463         {
1464           free(element);
1465         }
1466         count=0;array_size=0;element=NULL;
1467 }
1468
1469 template <class Type> void Array<Type>::allocate(int s)
1470 {
1471         assert(s>0);
1472         assert(s>=count);
1473         Type *old = element;
1474         array_size =s;
1475         element = (Type *) malloc( sizeof(Type)*array_size);
1476         assert(element);
1477         for(int i=0;i<count;i++)
1478         {
1479                 element[i]=old[i];
1480         }
1481         if(old)
1482         {
1483                 free(old);
1484         }
1485 }
1486
1487 template <class Type> void Array<Type>::SetSize(int s)
1488 {
1489         if(s==0)
1490         {
1491                 if(element)
1492                 {
1493                         free(element);
1494                         element = NULL;
1495                 }
1496           array_size = s;
1497         }
1498         else
1499         {
1500                 allocate(s);
1501         }
1502         count=s;
1503 }
1504
1505 template <class Type> void Array<Type>::Pack()
1506 {
1507         allocate(count);
1508 }
1509
1510 template <class Type> Type& Array<Type>::Add(Type t)
1511 {
1512         assert(count<=array_size);
1513         if(count==array_size)
1514         {
1515                 allocate((array_size)?array_size *2:16);
1516         }
1517         element[count++] = t;
1518         return element[count-1];
1519 }
1520
1521 template <class Type> int Array<Type>::Contains(Type t)
1522 {
1523         int i;
1524         int found=0;
1525         for(i=0;i<count;i++)
1526         {
1527                 if(element[i] == t) found++;
1528         }
1529         return found;
1530 }
1531
1532 template <class Type> void Array<Type>::AddUnique(Type t)
1533 {
1534         if(!Contains(t)) Add(t);
1535 }
1536
1537
1538 template <class Type> void Array<Type>::DelIndex(int i)
1539 {
1540         assert(i<count);
1541         count--;
1542         while(i<count)
1543         {
1544                 element[i] = element[i+1];
1545                 i++;
1546         }
1547 }
1548
1549 template <class Type> void Array<Type>::Remove(Type t)
1550 {
1551         int i;
1552         for(i=0;i<count;i++)
1553         {
1554                 if(element[i] == t)
1555                 {
1556                         break;
1557                 }
1558         }
1559         assert(i<count); // assert object t is in the array.
1560         DelIndex(i);
1561         for(i=0;i<count;i++)
1562         {
1563                 assert(element[i] != t);
1564         }
1565 }
1566
1567 template <class Type> void Array<Type>::Insert(Type t,int k)
1568 {
1569         int i=count;
1570         Add(t); // to allocate space
1571         while(i>k)
1572         {
1573                 element[i]=element[i-1];
1574                 i--;
1575         }
1576         assert(i==k);
1577         element[k]=t;
1578 }
1579
1580
1581 template <class Type> int Array<Type>::IndexOf(Type t)
1582 {
1583         int i;
1584         for(i=0;i<count;i++)
1585         {
1586                 if(element[i] == t)
1587                 {
1588                         return i;
1589                 }
1590         }
1591         assert(0);
1592         return -1;
1593 }
1594
1595
1596
1597 //*********************************************************************
1598 //*********************************************************************
1599 //********  Hull header
1600 //*********************************************************************
1601 //*********************************************************************
1602
1603 class PHullResult
1604 {
1605 public:
1606
1607         PHullResult(void)
1608         {
1609                 mVcount = 0;
1610                 mIndexCount = 0;
1611                 mFaceCount = 0;
1612                 mVertices = 0;
1613                 mIndices  = 0;
1614         }
1615
1616         unsigned int mVcount;
1617         unsigned int mIndexCount;
1618         unsigned int mFaceCount;
1619         float       *mVertices;
1620         unsigned int *mIndices;
1621 };
1622
1623
1624 #define REAL3 float3
1625 #define REAL  float
1626
1627 #define COPLANAR   (0)
1628 #define UNDER      (1)
1629 #define OVER       (2)
1630 #define SPLIT      (OVER|UNDER)
1631 #define PAPERWIDTH (0.001f)
1632
1633 float planetestepsilon = PAPERWIDTH;
1634
1635
1636 class ConvexH
1637 {
1638   public:
1639         class HalfEdge
1640         {
1641           public:
1642                 short ea;         // the other half of the edge (index into edges list)
1643                 unsigned char v;  // the vertex at the start of this edge (index into vertices list)
1644                 unsigned char p;  // the facet on which this edge lies (index into facets list)
1645                 HalfEdge(){}
1646                 HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){}
1647         };
1648         Array<REAL3> vertices;
1649         Array<HalfEdge> edges;
1650         Array<Plane>  facets;
1651         ConvexH(int vertices_size,int edges_size,int facets_size);
1652 };
1653
1654 typedef ConvexH::HalfEdge HalfEdge;
1655
1656 ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
1657         :vertices(vertices_size)
1658         ,edges(edges_size)
1659         ,facets(facets_size)
1660 {
1661         vertices.count=vertices_size;
1662         edges.count   = edges_size;
1663         facets.count  = facets_size;
1664 }
1665
1666 ConvexH *ConvexHDup(ConvexH *src) {
1667         ConvexH *dst = new ConvexH(src->vertices.count,src->edges.count,src->facets.count);
1668         memcpy(dst->vertices.element,src->vertices.element,sizeof(float3)*src->vertices.count);
1669         memcpy(dst->edges.element,src->edges.element,sizeof(HalfEdge)*src->edges.count);
1670         memcpy(dst->facets.element,src->facets.element,sizeof(Plane)*src->facets.count);
1671         return dst;
1672 }
1673
1674
1675 int PlaneTest(const Plane &p, const REAL3 &v) {
1676         REAL a  = dot(v,p.normal)+p.dist;
1677         int   flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
1678         return flag;
1679 }
1680
1681 int SplitTest(ConvexH &convex,const Plane &plane) {
1682         int flag=0;
1683         for(int i=0;i<convex.vertices.count;i++) {
1684                 flag |= PlaneTest(plane,convex.vertices[i]);
1685         }
1686         return flag;
1687 }
1688
1689 class VertFlag
1690 {
1691 public:
1692         unsigned char planetest;
1693         unsigned char junk;
1694         unsigned char undermap;
1695         unsigned char overmap;
1696 };
1697 class EdgeFlag
1698 {
1699 public:
1700         unsigned char planetest;
1701         unsigned char fixes;
1702         short undermap;
1703         short overmap;
1704 };
1705 class PlaneFlag
1706 {
1707 public:
1708         unsigned char undermap;
1709         unsigned char overmap;
1710 };
1711 class Coplanar{
1712 public:
1713         unsigned short ea;
1714         unsigned char v0;
1715         unsigned char v1;
1716 };
1717
1718 int AssertIntact(ConvexH &convex) {
1719         int i;
1720         int estart=0;
1721         for(i=0;i<convex.edges.count;i++) {
1722                 if(convex.edges[estart].p!= convex.edges[i].p) {
1723                         estart=i;
1724                 }
1725                 int inext = i+1;
1726                 if(inext>= convex.edges.count || convex.edges[inext].p != convex.edges[i].p) {
1727                         inext = estart;
1728                 }
1729                 assert(convex.edges[inext].p == convex.edges[i].p);
1730                 int nb = convex.edges[i].ea;
1731                 assert(nb!=255);
1732                 if(nb==255 || nb==-1) return 0;
1733                 assert(nb!=-1);
1734                 assert(i== convex.edges[nb].ea);
1735         }
1736         for(i=0;i<convex.edges.count;i++) {
1737                 assert(COPLANAR==PlaneTest(convex.facets[convex.edges[i].p],convex.vertices[convex.edges[i].v]));
1738                 if(COPLANAR!=PlaneTest(convex.facets[convex.edges[i].p],convex.vertices[convex.edges[i].v])) return 0;
1739                 if(convex.edges[estart].p!= convex.edges[i].p) {
1740                         estart=i;
1741                 }
1742                 int i1 = i+1;
1743                 if(i1>= convex.edges.count || convex.edges[i1].p != convex.edges[i].p) {
1744                         i1 = estart;
1745                 }
1746                 int i2 = i1+1;
1747                 if(i2>= convex.edges.count || convex.edges[i2].p != convex.edges[i].p) {
1748                         i2 = estart;
1749                 }
1750                 if(i==i2) continue; // i sliced tangent to an edge and created 2 meaningless edges
1751                 REAL3 localnormal = TriNormal(convex.vertices[convex.edges[i ].v],
1752                                                    convex.vertices[convex.edges[i1].v],
1753                                                    convex.vertices[convex.edges[i2].v]);
1754                 assert(dot(localnormal,convex.facets[convex.edges[i].p].normal)>0);
1755                 if(dot(localnormal,convex.facets[convex.edges[i].p].normal)<=0)return 0;
1756         }
1757         return 1;
1758 }
1759
1760 // back to back quads
1761 ConvexH *test_btbq() {
1762         ConvexH *convex = new ConvexH(4,8,2);
1763         convex->vertices[0] = REAL3(0,0,0);
1764         convex->vertices[1] = REAL3(1,0,0);
1765         convex->vertices[2] = REAL3(1,1,0);
1766         convex->vertices[3] = REAL3(0,1,0);
1767         convex->facets[0] = Plane(REAL3(0,0,1),0);
1768         convex->facets[1] = Plane(REAL3(0,0,-1),0);
1769         convex->edges[0]  = HalfEdge(7,0,0);
1770         convex->edges[1]  = HalfEdge(6,1,0);
1771         convex->edges[2]  = HalfEdge(5,2,0);
1772         convex->edges[3]  = HalfEdge(4,3,0);
1773
1774         convex->edges[4]  = HalfEdge(3,0,1);
1775         convex->edges[5]  = HalfEdge(2,3,1);
1776         convex->edges[6]  = HalfEdge(1,2,1);
1777         convex->edges[7]  = HalfEdge(0,1,1);
1778         AssertIntact(*convex);
1779         return convex;
1780 }
1781 ConvexH *test_cube() {
1782         ConvexH *convex = new ConvexH(8,24,6);
1783         convex->vertices[0] = REAL3(0,0,0);
1784         convex->vertices[1] = REAL3(0,0,1);
1785         convex->vertices[2] = REAL3(0,1,0);
1786         convex->vertices[3] = REAL3(0,1,1);
1787         convex->vertices[4] = REAL3(1,0,0);
1788         convex->vertices[5] = REAL3(1,0,1);
1789         convex->vertices[6] = REAL3(1,1,0);
1790         convex->vertices[7] = REAL3(1,1,1);
1791
1792         convex->facets[0] = Plane(REAL3(-1,0,0),0);
1793         convex->facets[1] = Plane(REAL3(1,0,0),-1);
1794         convex->facets[2] = Plane(REAL3(0,-1,0),0);
1795         convex->facets[3] = Plane(REAL3(0,1,0),-1);
1796         convex->facets[4] = Plane(REAL3(0,0,-1),0);
1797         convex->facets[5] = Plane(REAL3(0,0,1),-1);
1798
1799         convex->edges[0 ] = HalfEdge(11,0,0);
1800         convex->edges[1 ] = HalfEdge(23,1,0);
1801         convex->edges[2 ] = HalfEdge(15,3,0);
1802         convex->edges[3 ] = HalfEdge(16,2,0);
1803
1804         convex->edges[4 ] = HalfEdge(13,6,1);
1805         convex->edges[5 ] = HalfEdge(21,7,1);
1806         convex->edges[6 ] = HalfEdge( 9,5,1);
1807         convex->edges[7 ] = HalfEdge(18,4,1);
1808
1809         convex->edges[8 ] = HalfEdge(19,0,2);
1810         convex->edges[9 ] = HalfEdge( 6,4,2);
1811         convex->edges[10] = HalfEdge(20,5,2);
1812         convex->edges[11] = HalfEdge( 0,1,2);
1813
1814         convex->edges[12] = HalfEdge(22,3,3);
1815         convex->edges[13] = HalfEdge( 4,7,3);
1816         convex->edges[14] = HalfEdge(17,6,3);
1817         convex->edges[15] = HalfEdge( 2,2,3);
1818
1819         convex->edges[16] = HalfEdge( 3,0,4);
1820         convex->edges[17] = HalfEdge(14,2,4);
1821         convex->edges[18] = HalfEdge( 7,6,4);
1822         convex->edges[19] = HalfEdge( 8,4,4);
1823
1824         convex->edges[20] = HalfEdge(10,1,5);
1825         convex->edges[21] = HalfEdge( 5,5,5);
1826         convex->edges[22] = HalfEdge(12,7,5);
1827         convex->edges[23] = HalfEdge( 1,3,5);
1828
1829
1830         return convex;
1831 }
1832 ConvexH *ConvexHMakeCube(const REAL3 &bmin, const REAL3 &bmax) {
1833         ConvexH *convex = test_cube();
1834         convex->vertices[0] = REAL3(bmin.x,bmin.y,bmin.z);
1835         convex->vertices[1] = REAL3(bmin.x,bmin.y,bmax.z);
1836         convex->vertices[2] = REAL3(bmin.x,bmax.y,bmin.z);
1837         convex->vertices[3] = REAL3(bmin.x,bmax.y,bmax.z);
1838         convex->vertices[4] = REAL3(bmax.x,bmin.y,bmin.z);
1839         convex->vertices[5] = REAL3(bmax.x,bmin.y,bmax.z);
1840         convex->vertices[6] = REAL3(bmax.x,bmax.y,bmin.z);
1841         convex->vertices[7] = REAL3(bmax.x,bmax.y,bmax.z);
1842
1843         convex->facets[0] = Plane(REAL3(-1,0,0), bmin.x);
1844         convex->facets[1] = Plane(REAL3(1,0,0), -bmax.x);
1845         convex->facets[2] = Plane(REAL3(0,-1,0), bmin.y);
1846         convex->facets[3] = Plane(REAL3(0,1,0), -bmax.y);
1847         convex->facets[4] = Plane(REAL3(0,0,-1), bmin.z);
1848         convex->facets[5] = Plane(REAL3(0,0,1), -bmax.z);
1849         return convex;
1850 }
1851 ConvexH *ConvexHCrop(ConvexH &convex,const Plane &slice)
1852 {
1853         int i;
1854         int vertcountunder=0;
1855         int vertcountover =0;
1856         Array<int> vertscoplanar;  // existing vertex members of convex that are coplanar
1857         vertscoplanar.count=0;
1858         Array<int> edgesplit;  // existing edges that members of convex that cross the splitplane
1859         edgesplit.count=0;
1860
1861         assert(convex.edges.count<480);
1862
1863         EdgeFlag  edgeflag[512];
1864         VertFlag  vertflag[256];
1865         PlaneFlag planeflag[128];
1866         HalfEdge  tmpunderedges[512];
1867         Plane     tmpunderplanes[128];
1868         Coplanar coplanaredges[512];
1869         int coplanaredges_num=0;
1870
1871         Array<REAL3> createdverts;
1872         // do the side-of-plane tests
1873         for(i=0;i<convex.vertices.count;i++) {
1874                 vertflag[i].planetest = PlaneTest(slice,convex.vertices[i]);
1875                 if(vertflag[i].planetest == COPLANAR) {
1876                         // ? vertscoplanar.Add(i);
1877                         vertflag[i].undermap = vertcountunder++;
1878                         vertflag[i].overmap  = vertcountover++;
1879                 }
1880                 else if(vertflag[i].planetest == UNDER) {
1881                         vertflag[i].undermap = vertcountunder++;
1882                 }
1883                 else {
1884                         assert(vertflag[i].planetest == OVER);
1885                         vertflag[i].overmap  = vertcountover++;
1886                         vertflag[i].undermap = 255; // for debugging purposes
1887                 }
1888         }
1889         int vertcountunderold = vertcountunder; // for debugging only
1890
1891         int under_edge_count =0;
1892         int underplanescount=0;
1893         int e0=0;
1894
1895         for(int currentplane=0; currentplane<convex.facets.count; currentplane++) {
1896                 int estart =e0;
1897                 int enextface = 0;
1898                 int planeside = 0;
1899                 int e1 = e0+1;
1900                 int vout=-1;
1901                 int vin =-1;
1902                 int coplanaredge = -1;
1903                 do{
1904
1905                         if(e1 >= convex.edges.count || convex.edges[e1].p!=currentplane) {
1906                                 enextface = e1;
1907                                 e1=estart;
1908                         }
1909                         HalfEdge &edge0 = convex.edges[e0];
1910                         HalfEdge &edge1 = convex.edges[e1];
1911                         HalfEdge &edgea = convex.edges[edge0.ea];
1912
1913
1914                         planeside |= vertflag[edge0.v].planetest;
1915                         //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest)  == COPLANAR) {
1916                         //      assert(ecop==-1);
1917                         //      ecop=e;
1918                         //}
1919
1920
1921                         if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == OVER){
1922                                 // both endpoints over plane
1923                                 edgeflag[e0].undermap  = -1;
1924                         }
1925                         else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest)  == UNDER) {
1926                                 // at least one endpoint under, the other coplanar or under
1927
1928                                 edgeflag[e0].undermap = under_edge_count;
1929                                 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
1930                                 tmpunderedges[under_edge_count].p = underplanescount;
1931                                 if(edge0.ea < e0) {
1932                                         // connect the neighbors
1933                                         assert(edgeflag[edge0.ea].undermap !=-1);
1934                                         tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
1935                                         tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
1936                                 }
1937                                 under_edge_count++;
1938                         }
1939                         else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest)  == COPLANAR) {
1940                                 // both endpoints coplanar
1941                                 // must check a 3rd point to see if UNDER
1942                                 int e2 = e1+1;
1943                                 if(e2>=convex.edges.count || convex.edges[e2].p!=currentplane) {
1944                                         e2 = estart;
1945                                 }
1946                                 assert(convex.edges[e2].p==currentplane);
1947                                 HalfEdge &edge2 = convex.edges[e2];
1948                                 if(vertflag[edge2.v].planetest==UNDER) {
1949
1950                                         edgeflag[e0].undermap = under_edge_count;
1951                                         tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
1952                                         tmpunderedges[under_edge_count].p = underplanescount;
1953                                         tmpunderedges[under_edge_count].ea = -1;
1954                                         // make sure this edge is added to the "coplanar" list
1955                                         coplanaredge = under_edge_count;
1956                                         vout = vertflag[edge0.v].undermap;
1957                                         vin  = vertflag[edge1.v].undermap;
1958                                         under_edge_count++;
1959                                 }
1960                                 else {
1961                                         edgeflag[e0].undermap = -1;
1962                                 }
1963                         }
1964                         else if(vertflag[edge0.v].planetest == UNDER && vertflag[edge1.v].planetest == OVER) {
1965                                 // first is under 2nd is over
1966
1967                                 edgeflag[e0].undermap = under_edge_count;
1968                                 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
1969                                 tmpunderedges[under_edge_count].p = underplanescount;
1970                                 if(edge0.ea < e0) {
1971                                         assert(edgeflag[edge0.ea].undermap !=-1);
1972                                         // connect the neighbors
1973                                         tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
1974                                         tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
1975                                         vout = tmpunderedges[edgeflag[edge0.ea].undermap].v;
1976                                 }
1977                                 else {
1978                                         Plane &p0 = convex.facets[edge0.p];
1979                                         Plane &pa = convex.facets[edgea.p];
1980                                         createdverts.Add(ThreePlaneIntersection(p0,pa,slice));
1981                                         //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])));
1982                                         //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]));
1983                                         vout = vertcountunder++;
1984                                 }
1985                                 under_edge_count++;
1986                                 /// hmmm something to think about: i might be able to output this edge regarless of
1987                                 // wheter or not we know v-in yet.  ok i;ll try this now:
1988                                 tmpunderedges[under_edge_count].v = vout;
1989                                 tmpunderedges[under_edge_count].p = underplanescount;
1990                                 tmpunderedges[under_edge_count].ea = -1;
1991                                 coplanaredge = under_edge_count;
1992                                 under_edge_count++;
1993
1994                                 if(vin!=-1) {
1995                                         // we previously processed an edge  where we came under
1996                                         // now we know about vout as well
1997
1998                                         // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
1999                                 }
2000
2001                         }
2002                         else if(vertflag[edge0.v].planetest == COPLANAR && vertflag[edge1.v].planetest == OVER) {
2003                                 // first is coplanar 2nd is over
2004
2005                                 edgeflag[e0].undermap = -1;
2006                                 vout = vertflag[edge0.v].undermap;
2007                                 // I hate this but i have to make sure part of this face is UNDER before ouputting this vert
2008                                 int k=estart;
2009                                 assert(edge0.p == currentplane);
2010                                 while(!(planeside&UNDER) && k<convex.edges.count && convex.edges[k].p==edge0.p) {
2011                                         planeside |= vertflag[convex.edges[k].v].planetest;
2012                                         k++;
2013                                 }
2014                                 if(planeside&UNDER){
2015                                         tmpunderedges[under_edge_count].v = vout;
2016                                         tmpunderedges[under_edge_count].p = underplanescount;
2017                                         tmpunderedges[under_edge_count].ea = -1;
2018                                         coplanaredge = under_edge_count; // hmmm should make a note of the edge # for later on
2019                                         under_edge_count++;
2020
2021                                 }
2022                         }
2023                         else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == UNDER) {
2024                                 // first is over next is under
2025                                 // new vertex!!!
2026                                 assert(vin==-1);
2027                                 if(e0<edge0.ea) {
2028                                         Plane &p0 = convex.facets[edge0.p];
2029                                         Plane &pa = convex.facets[edgea.p];
2030                                         createdverts.Add(ThreePlaneIntersection(p0,pa,slice));
2031                                         //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]));
2032                                         //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])));
2033                                         vin = vertcountunder++;
2034                                 }
2035                                 else {
2036                                         // find the new vertex that was created by edge[edge0.ea]
2037                                         int nea = edgeflag[edge0.ea].undermap;
2038                                         assert(tmpunderedges[nea].p==tmpunderedges[nea+1].p);
2039                                         vin = tmpunderedges[nea+1].v;
2040                                         assert(vin < vertcountunder);
2041                                         assert(vin >= vertcountunderold);   // for debugging only
2042                                 }
2043                                 if(vout!=-1) {
2044                                         // we previously processed an edge  where we went over
2045                                         // now we know vin too
2046                                         // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
2047                                 }
2048                                 // output edge
2049                                 tmpunderedges[under_edge_count].v = vin;
2050                                 tmpunderedges[under_edge_count].p = underplanescount;
2051                                 edgeflag[e0].undermap = under_edge_count;
2052                                 if(e0>edge0.ea) {
2053                                         assert(edgeflag[edge0.ea].undermap !=-1);
2054                                         // connect the neighbors
2055                                         tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
2056                                         tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
2057                                 }
2058                                 assert(edgeflag[e0].undermap == under_edge_count);
2059                                 under_edge_count++;
2060                         }
2061                         else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == COPLANAR) {
2062                                 // first is over next is coplanar
2063
2064                                 edgeflag[e0].undermap = -1;
2065                                 vin = vertflag[edge1.v].undermap;
2066                                 assert(vin!=-1);
2067                                 if(vout!=-1) {
2068                                         // we previously processed an edge  where we came under
2069                                         // now we know both endpoints
2070                                         // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
2071                                 }
2072
2073                         }
2074                         else {
2075                                 assert(0);
2076                         }
2077
2078
2079                         e0=e1;
2080                         e1++; // do the modulo at the beginning of the loop
2081
2082                 } while(e0!=estart) ;
2083                 e0 = enextface;
2084                 if(planeside&UNDER) {
2085                         planeflag[currentplane].undermap = underplanescount;
2086                         tmpunderplanes[underplanescount] = convex.facets[currentplane];
2087                         underplanescount++;
2088                 }
2089                 else {
2090                         planeflag[currentplane].undermap = 0;
2091                 }
2092                 if(vout>=0 && (planeside&UNDER)) {
2093                         assert(vin>=0);
2094                         assert(coplanaredge>=0);
2095                         assert(coplanaredge!=511);
2096                         coplanaredges[coplanaredges_num].ea = coplanaredge;
2097                         coplanaredges[coplanaredges_num].v0 = vin;
2098                         coplanaredges[coplanaredges_num].v1 = vout;
2099                         coplanaredges_num++;
2100                 }
2101         }
2102
2103         // add the new plane to the mix:
2104         if(coplanaredges_num>0) {
2105                 tmpunderplanes[underplanescount++]=slice;
2106         }
2107         for(i=0;i<coplanaredges_num-1;i++) {
2108                 if(coplanaredges[i].v1 != coplanaredges[i+1].v0) {
2109                         int j = 0;
2110                         for(j=i+2;j<coplanaredges_num;j++) {
2111                                 if(coplanaredges[i].v1 == coplanaredges[j].v0) {
2112                                         Coplanar tmp = coplanaredges[i+1];
2113                                         coplanaredges[i+1] = coplanaredges[j];
2114                                         coplanaredges[j] = tmp;
2115                                         break;
2116                                 }
2117                         }
2118                         if(j>=coplanaredges_num)
2119                         {
2120                                 assert(j<coplanaredges_num);
2121                                 return NULL;
2122                         }
2123                 }
2124         }
2125         ConvexH *punder = new ConvexH(vertcountunder,under_edge_count+coplanaredges_num,underplanescount);
2126         ConvexH &under = *punder;
2127         int k=0;
2128         for(i=0;i<convex.vertices.count;i++) {
2129                 if(vertflag[i].planetest != OVER){
2130                         under.vertices[k++] = convex.vertices[i];
2131                 }
2132         }
2133         i=0;
2134         while(k<vertcountunder) {
2135                 under.vertices[k++] = createdverts[i++];
2136         }
2137         assert(i==createdverts.count);
2138
2139         for(i=0;i<coplanaredges_num;i++) {
2140                 under.edges[under_edge_count+i].p  = underplanescount-1;
2141                 under.edges[under_edge_count+i].ea = coplanaredges[i].ea;
2142                 tmpunderedges[coplanaredges[i].ea].ea = under_edge_count+i;
2143                 under.edges[under_edge_count+i].v  = coplanaredges[i].v0;
2144         }
2145
2146         memcpy(under.edges.element,tmpunderedges,sizeof(HalfEdge)*under_edge_count);
2147         memcpy(under.facets.element,tmpunderplanes,sizeof(Plane)*underplanescount);
2148         return punder;
2149 }
2150
2151
2152
2153 static int candidateplane(Plane *planes,int planes_count,ConvexH *convex,float epsilon)
2154 {
2155         int p = 0 ;
2156         REAL md= 0 ;
2157         int i;
2158         for(i=0;i<planes_count;i++)
2159         {
2160                 REAL d=0;
2161                 for(int j=0;j<convex->vertices.count;j++)
2162                 {
2163                         d = Max(d,dot(convex->vertices[j],planes[i].normal)+planes[i].dist);
2164                 }
2165                 if(i==0 || d>md)
2166                 {
2167                         p=i;
2168                         md=d;
2169                 }
2170         }
2171         return (md>epsilon)?p:-1;
2172 }
2173
2174 template<class T>
2175 inline int maxdir(const T *p,int count,const T &dir)
2176 {
2177         assert(count);
2178         int m=0;
2179         float currDotm = dot(p[0], dir);
2180         for(int i=1;i<count;i++)
2181         {
2182                 const float currDoti =  dot(p[i], dir);
2183                 if(currDoti > currDotm)
2184                 {
2185                         currDotm = currDoti;
2186                         m=i;
2187                 }
2188         }
2189         return m;
2190 }
2191
2192
2193 template<class T>
2194 int maxdirfiltered(const T *p,int count,const T &dir,Array<int> &allow)
2195 {
2196         assert(count);
2197         int m=-1;
2198         float currDotm = dot(p[0], dir);
2199         for(int i=0;i<count;i++)
2200         {
2201                 if(allow[i])
2202                 {
2203                         if(m==-1 )
2204                         {
2205                                 currDotm = dot(p[i], dir);
2206                                 m=i;
2207                         }
2208                         else
2209                         {
2210                                 const float currDoti = dot(p[i], dir);
2211                                 if (currDoti>currDotm)
2212                                 {
2213                                         currDotm = currDoti;
2214                                         m=i;
2215                                 }
2216                         }
2217                 }
2218         }
2219         assert(m!=-1);
2220         return m;
2221 }
2222
2223 float3 orth(const float3 &v)
2224 {
2225         float3 a=cross(v,float3(0,0,1));
2226         float3 b=cross(v,float3(0,1,0));
2227         return normalize((magnitude(a)>magnitude(b))?a:b);
2228 }
2229
2230
2231 template<class T>
2232 int maxdirsterid(const T *p,int count,const T &dir,Array<int> &allow)
2233 {
2234         int m=-1;
2235         while(m==-1)
2236         {
2237                 m = maxdirfiltered(p,count,dir,allow);
2238                 if(allow[m]==3) return m;
2239                 T u = orth(dir);
2240                 T v = cross(u,dir);
2241                 int ma=-1;
2242                 for(float x = 0.0f ; x<= 360.0f ; x+= 45.0f)
2243                 {
2244                         float s = sinf(DEG2RAD*(x));
2245                         float c = cosf(DEG2RAD*(x));
2246                         int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow);
2247                         if(ma==m && mb==m)
2248                         {
2249                                 allow[m]=3;
2250                                 return m;
2251                         }
2252                         if(ma!=-1 && ma!=mb)  // Yuck - this is really ugly
2253                         {
2254                                 int mc = ma;
2255                                 for(float xx = x-40.0f ; xx <= x ; xx+= 5.0f)
2256                                 {
2257                                         float s = sinf(DEG2RAD*(xx));
2258                                         float c = cosf(DEG2RAD*(xx));
2259                                         int md = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow);
2260                                         if(mc==m && md==m)
2261                                         {
2262                                                 allow[m]=3;
2263                                                 return m;
2264                                         }
2265                                         mc=md;
2266                                 }
2267                         }
2268                         ma=mb;
2269                 }
2270                 allow[m]=0;
2271                 m=-1;
2272         }
2273         assert(0);
2274         return m;
2275 }
2276
2277
2278
2279
2280 int operator ==(const int3 &a,const int3 &b)
2281 {
2282         for(int i=0;i<3;i++)
2283         {
2284                 if(a[i]!=b[i]) return 0;
2285         }
2286         return 1;
2287 }
2288
2289 int3 roll3(int3 a)
2290 {
2291         int tmp=a[0];
2292         a[0]=a[1];
2293         a[1]=a[2];
2294         a[2]=tmp;
2295         return a;
2296 }
2297 int isa(const int3 &a,const int3 &b)
2298 {
2299         return ( a==b || roll3(a)==b || a==roll3(b) );
2300 }
2301 int b2b(const int3 &a,const int3 &b)
2302 {
2303         return isa(a,int3(b[2],b[1],b[0]));
2304 }
2305 int above(float3* vertices,const int3& t, const float3 &p, float epsilon)
2306 {
2307         float3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
2308         return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
2309 }
2310 int hasedge(const int3 &t, int a,int b)
2311 {
2312         for(int i=0;i<3;i++)
2313         {
2314                 int i1= (i+1)%3;
2315                 if(t[i]==a && t[i1]==b) return 1;
2316         }
2317         return 0;
2318 }
2319 int hasvert(const int3 &t, int v)
2320 {
2321         return (t[0]==v || t[1]==v || t[2]==v) ;
2322 }
2323 int shareedge(const int3 &a,const int3 &b)
2324 {
2325         int i;
2326         for(i=0;i<3;i++)
2327         {
2328                 int i1= (i+1)%3;
2329                 if(hasedge(a,b[i1],b[i])) return 1;
2330         }
2331         return 0;
2332 }
2333
2334 class btHullTriangle;
2335
2336 //Array<btHullTriangle*> tris;
2337
2338 class btHullTriangle : public int3
2339 {
2340 public:
2341         int3 n;
2342         int id;
2343         int vmax;
2344         float rise;
2345         Array<btHullTriangle*>* tris;
2346         btHullTriangle(int a,int b,int c, Array<btHullTriangle*>* pTris):int3(a,b,c),n(-1,-1,-1)
2347         {
2348                 tris = pTris;
2349                 id = tris->count;
2350                 tris->Add(this);
2351                 vmax=-1;
2352                 rise = 0.0f;
2353         }
2354         ~btHullTriangle()
2355         {
2356                 assert((*tris)[id]==this);
2357                 (*tris)[id]=NULL;
2358         }
2359         int &neib(int a,int b);
2360 };
2361
2362
2363 int &btHullTriangle::neib(int a,int b)
2364 {
2365         static int er=-1;
2366         int i;
2367         for(i=0;i<3;i++)
2368         {
2369                 int i1=(i+1)%3;
2370                 int i2=(i+2)%3;
2371                 if((*this)[i]==a && (*this)[i1]==b) return n[i2];
2372                 if((*this)[i]==b && (*this)[i1]==a) return n[i2];
2373         }
2374         assert(0);
2375         return er;
2376 }
2377 void b2bfix(btHullTriangle* s,btHullTriangle*t, Array<btHullTriangle*>& tris)
2378 {
2379         int i;
2380         for(i=0;i<3;i++)
2381         {
2382                 int i1=(i+1)%3;
2383                 int i2=(i+2)%3;
2384                 int a = (*s)[i1];
2385                 int b = (*s)[i2];
2386                 assert(tris[s->neib(a,b)]->neib(b,a) == s->id);
2387                 assert(tris[t->neib(a,b)]->neib(b,a) == t->id);
2388                 tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
2389                 tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
2390         }
2391 }
2392
2393 void removeb2b(btHullTriangle* s,btHullTriangle*t, Array<btHullTriangle*>& tris)
2394 {
2395         b2bfix(s,t, tris);
2396         delete s;
2397         delete t;
2398 }
2399
2400 void checkit(btHullTriangle *t, Array<btHullTriangle*>& tris)
2401 {
2402         int i;
2403         assert(tris[t->id]==t);
2404         for(i=0;i<3;i++)
2405         {
2406                 int i1=(i+1)%3;
2407                 int i2=(i+2)%3;
2408                 int a = (*t)[i1];
2409                 int b = (*t)[i2];
2410                 assert(a!=b);
2411                 assert( tris[t->n[i]]->neib(b,a) == t->id);
2412         }
2413 }
2414 void extrude(btHullTriangle *t0,int v, Array<btHullTriangle*>& tris)
2415 {
2416         int3 t= *t0;
2417         int n = tris.count;
2418         btHullTriangle* ta = new btHullTriangle(v,t[1],t[2], &tris);
2419         ta->n = int3(t0->n[0],n+1,n+2);
2420         tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
2421         btHullTriangle* tb = new btHullTriangle(v,t[2],t[0], &tris);
2422         tb->n = int3(t0->n[1],n+2,n+0);
2423         tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
2424         btHullTriangle* tc = new btHullTriangle(v,t[0],t[1], &tris);
2425         tc->n = int3(t0->n[2],n+0,n+1);
2426         tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
2427         checkit(ta, tris);
2428         checkit(tb, tris);
2429         checkit(tc, tris);
2430         if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]], tris);
2431         if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]], tris);
2432         if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]], tris);
2433         delete t0;
2434
2435 }
2436
2437 btHullTriangle *extrudable(float epsilon, Array<btHullTriangle*>& tris)
2438 {
2439         int i;
2440         btHullTriangle *t=NULL;
2441         for(i=0;i<tris.count;i++)
2442         {
2443                 if(!t || (tris[i] && t->rise<tris[i]->rise))
2444                 {
2445                         t = tris[i];
2446                 }
2447         }
2448         return (t->rise >epsilon)?t:NULL ;
2449 }
2450
2451 class int4
2452 {
2453 public:
2454         int x,y,z,w;
2455         int4(){};
2456         int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;}
2457         const int& operator[](int i) const {return (&x)[i];}
2458         int& operator[](int i) {return (&x)[i];}
2459 };
2460
2461
2462
2463 int4 FindSimplex(float3 *verts,int verts_count,Array<int> &allow)
2464 {
2465         float3 basis[3];
2466         basis[0] = float3( 0.01f, 0.02f, 1.0f );
2467         int p0 = maxdirsterid(verts,verts_count, basis[0],allow);
2468         int     p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
2469         basis[0] = verts[p0]-verts[p1];
2470         if(p0==p1 || basis[0]==float3(0,0,0))
2471                 return int4(-1,-1,-1,-1);
2472         basis[1] = cross(float3(     1, 0.02f, 0),basis[0]);
2473         basis[2] = cross(float3(-0.02f,     1, 0),basis[0]);
2474         basis[1] = normalize( (magnitude(basis[1])>magnitude(basis[2])) ? basis[1]:basis[2]);
2475         int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
2476         if(p2 == p0 || p2 == p1)
2477         {
2478                 p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
2479         }
2480         if(p2 == p0 || p2 == p1)
2481                 return int4(-1,-1,-1,-1);
2482         basis[1] = verts[p2] - verts[p0];
2483         basis[2] = normalize(cross(basis[1],basis[0]));
2484         int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
2485         if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
2486         if(p3==p0||p3==p1||p3==p2)
2487                 return int4(-1,-1,-1,-1);
2488         assert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
2489         if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
2490         return int4(p0,p1,p2,p3);
2491 }
2492
2493 int calchullgen(float3 *verts,int verts_count, int vlimit,Array<btHullTriangle*>& tris)
2494 {
2495         if(verts_count <4) return 0;
2496         if(vlimit==0) vlimit=1000000000;
2497         int j;
2498         float3 bmin(*verts),bmax(*verts);
2499         Array<int> isextreme(verts_count);
2500         Array<int> allow(verts_count);
2501         for(j=0;j<verts_count;j++)
2502         {
2503                 allow.Add(1);
2504                 isextreme.Add(0);
2505                 bmin = VectorMin(bmin,verts[j]);
2506                 bmax = VectorMax(bmax,verts[j]);
2507         }
2508         float epsilon = magnitude(bmax-bmin) * 0.001f;
2509
2510
2511         int4 p = FindSimplex(verts,verts_count,allow);
2512         if(p.x==-1) return 0; // simplex failed
2513
2514
2515
2516         float3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) /4.0f;  // a valid interior point
2517         btHullTriangle *t0 = new btHullTriangle(p[2],p[3],p[1], &tris); t0->n=int3(2,3,1);
2518         btHullTriangle *t1 = new btHullTriangle(p[3],p[2],p[0], &tris); t1->n=int3(3,2,0);
2519         btHullTriangle *t2 = new btHullTriangle(p[0],p[1],p[3], &tris); t2->n=int3(0,1,3);
2520         btHullTriangle *t3 = new btHullTriangle(p[1],p[0],p[2], &tris); t3->n=int3(1,0,2);
2521         isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
2522         checkit(t0, tris);checkit(t1, tris);checkit(t2, tris);checkit(t3, tris);
2523
2524         for(j=0;j<tris.count;j++)
2525         {
2526                 btHullTriangle *t=tris[j];
2527                 assert(t);
2528                 assert(t->vmax<0);
2529                 float3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
2530                 t->vmax = maxdirsterid(verts,verts_count,n,allow);
2531                 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
2532         }
2533         btHullTriangle *te;
2534         vlimit-=4;
2535         while(vlimit >0 && (te=extrudable(epsilon, tris)))
2536         {
2537         //      int3 ti=*te;
2538                 int v=te->vmax;
2539                 assert(!isextreme[v]);  // wtf we've already done this vertex
2540                 isextreme[v]=1;
2541                 //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
2542                 j=tris.count;
2543                 while(j--) {
2544                         if(!tris[j]) continue;
2545                         int3 t=*tris[j];
2546                         if(above(verts,t,verts[v],0.01f*epsilon))
2547                         {
2548                                 extrude(tris[j],v, tris);
2549                         }
2550                 }
2551                 // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
2552                 j=tris.count;
2553                 while(j--)
2554                 {
2555                         if(!tris[j]) continue;
2556                         if(!hasvert(*tris[j],v)) break;
2557                         int3 nt=*tris[j];
2558                         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 )
2559                         {
2560                                 btHullTriangle *nb = tris[tris[j]->n[0]];
2561                                 assert(nb);assert(!hasvert(*nb,v));assert(nb->id<j);
2562                                 extrude(nb,v, tris);
2563                                 j=tris.count;
2564                         }
2565                 }
2566                 j=tris.count;
2567                 while(j--)
2568                 {
2569                         btHullTriangle *t=tris[j];
2570                         if(!t) continue;
2571                         if(t->vmax>=0) break;
2572                         float3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
2573                         t->vmax = maxdirsterid(verts,verts_count,n,allow);
2574                         if(isextreme[t->vmax])
2575                         {
2576                                 t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate.
2577                         }
2578                         else
2579                         {
2580                                 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
2581                         }
2582                 }
2583                 vlimit --;
2584         }
2585         return 1;
2586 }
2587
2588 int calchull(float3 *verts,int verts_count, int *&tris_out, int &tris_count,int vlimit, Array<btHullTriangle*>& tris)
2589 {
2590         int rc=calchullgen(verts,verts_count,  vlimit, tris) ;
2591         if(!rc) return 0;
2592         Array<int> ts;
2593         for(int i=0;i<tris.count;i++)if(tris[i])
2594         {
2595                 for(int j=0;j<3;j++)ts.Add((*tris[i])[j]);
2596                 delete tris[i];
2597         }
2598         tris_count = ts.count/3;
2599         tris_out   = ts.element;
2600         ts.element=NULL; ts.count=ts.array_size=0;
2601         tris.count=0;
2602         return 1;
2603 }
2604
2605 int calchullpbev(float3 *verts,int verts_count,int vlimit, Array<Plane> &planes,float bevangle, Array<btHullTriangle*>& tris)
2606 {
2607         int i,j;
2608         planes.count=0;
2609         int rc = calchullgen(verts,verts_count,vlimit, tris);
2610         if(!rc) return 0;
2611         for(i=0;i<tris.count;i++)if(tris[i])
2612         {
2613                 Plane p;
2614                 btHullTriangle *t = tris[i];
2615                 p.normal = TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
2616                 p.dist   = -dot(p.normal, verts[(*t)[0]]);
2617                 planes.Add(p);
2618                 for(j=0;j<3;j++)
2619                 {
2620                         if(t->n[j]<t->id) continue;
2621                         btHullTriangle *s = tris[t->n[j]];
2622                         REAL3 snormal = TriNormal(verts[(*s)[0]],verts[(*s)[1]],verts[(*s)[2]]);
2623                         if(dot(snormal,p.normal)>=cos(bevangle*DEG2RAD)) continue;
2624                         REAL3 n = normalize(snormal+p.normal);
2625                         planes.Add(Plane(n,-dot(n,verts[maxdir(verts,verts_count,n)])));
2626                 }
2627         }
2628
2629         for(i=0;i<tris.count;i++)if(tris[i])
2630         {
2631                 delete tris[i]; // delete tris[i];
2632         }
2633         tris.count=0;
2634         return 1;
2635 }
2636
2637 int overhull(Plane *planes,int planes_count,float3 *verts, int verts_count,int maxplanes,
2638                          float3 *&verts_out, int &verts_count_out,  int *&faces_out, int &faces_count_out ,float inflate)
2639 {
2640         int i,j;
2641         if(verts_count <4) return 0;
2642         maxplanes = Min(maxplanes,planes_count);
2643         float3 bmin(verts[0]),bmax(verts[0]);
2644         for(i=0;i<verts_count;i++)
2645         {
2646                 bmin = VectorMin(bmin,verts[i]);
2647                 bmax = VectorMax(bmax,verts[i]);
2648         }
2649 //      float diameter = magnitude(bmax-bmin);
2650 //      inflate *=diameter;   // RELATIVE INFLATION
2651         bmin -= float3(inflate,inflate,inflate);
2652         bmax += float3(inflate,inflate,inflate);
2653         for(i=0;i<planes_count;i++)
2654         {
2655                 planes[i].dist -= inflate;
2656         }
2657         float3 emin = bmin; // VectorMin(bmin,float3(0,0,0));
2658         float3 emax = bmax; // VectorMax(bmax,float3(0,0,0));
2659         float epsilon  = magnitude(emax-emin) * 0.025f;
2660         planetestepsilon = magnitude(emax-emin) * PAPERWIDTH;
2661         // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think.
2662         // ConvexH *convex = ConvexHMakeCube(bmin - float3(diameter,diameter,diameter),bmax+float3(diameter,diameter,diameter));
2663         ConvexH *c = ConvexHMakeCube(REAL3(bmin),REAL3(bmax));
2664         int k;
2665         while(maxplanes-- && (k=candidateplane(planes,planes_count,c,epsilon))>=0)
2666         {
2667                 ConvexH *tmp = c;
2668                 c = ConvexHCrop(*tmp,planes[k]);
2669                 if(c==NULL) {c=tmp; break;} // might want to debug this case better!!!
2670                 if(!AssertIntact(*c)) {c=tmp; break;} // might want to debug this case better too!!!
2671                 delete tmp;
2672         }
2673
2674         assert(AssertIntact(*c));
2675         //return c;
2676         faces_out = (int*)malloc(sizeof(int)*(1+c->facets.count+c->edges.count));     // new int[1+c->facets.count+c->edges.count];
2677         faces_count_out=0;
2678         i=0;
2679         faces_out[faces_count_out++]=-1;
2680         k=0;
2681         while(i<c->edges.count)
2682         {
2683                 j=1;
2684                 while(j+i<c->edges.count && c->edges[i].p==c->edges[i+j].p) { j++; }
2685                 faces_out[faces_count_out++]=j;
2686                 while(j--)
2687                 {
2688                         faces_out[faces_count_out++] = c->edges[i].v;
2689                         i++;
2690                 }
2691                 k++;
2692         }
2693         faces_out[0]=k; // number of faces.
2694         assert(k==c->facets.count);
2695         assert(faces_count_out == 1+c->facets.count+c->edges.count);
2696         verts_out = c->vertices.element; // new float3[c->vertices.count];
2697         verts_count_out = c->vertices.count;
2698         for(i=0;i<c->vertices.count;i++)
2699         {
2700                 verts_out[i] = float3(c->vertices[i]);
2701         }
2702         c->vertices.count=c->vertices.array_size=0;     c->vertices.element=NULL;
2703         delete c;
2704         return 1;
2705 }
2706
2707 int overhullv(float3 *verts, int verts_count,int maxplanes,
2708                          float3 *&verts_out, int &verts_count_out,  int *&faces_out, int &faces_count_out ,float inflate,float bevangle,int vlimit, Array<btHullTriangle*>& tris)
2709 {
2710         if(!verts_count) return 0;
2711         extern int calchullpbev(float3 *verts,int verts_count,int vlimit, Array<Plane> &planes,float bevangle, Array<btHullTriangle*>& tris) ;
2712         Array<Plane> planes;
2713         int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle, tris) ;
2714         if(!rc) return 0;
2715         return overhull(planes.element,planes.count,verts,verts_count,maxplanes,verts_out,verts_count_out,faces_out,faces_count_out,inflate);
2716 }
2717
2718
2719 bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,unsigned int vlimit,float inflate, Array<btHullTriangle*>& arrtris)
2720 {
2721
2722         int index_count;
2723         int *faces;
2724         float3 *verts_out;
2725         int     verts_count_out;
2726
2727         if(inflate==0.0f)
2728         {
2729                 int  *tris_out;
2730                 int    tris_count;
2731                 int ret = calchull( (float3 *) vertices, (int) vcount, tris_out, tris_count, vlimit, arrtris );
2732                 if(!ret) return false;
2733                 result.mIndexCount = (unsigned int) (tris_count*3);
2734                 result.mFaceCount  = (unsigned int) tris_count;
2735                 result.mVertices   = (float*) vertices;
2736                 result.mVcount     = (unsigned int) vcount;
2737                 result.mIndices    = (unsigned int *) tris_out;
2738                 return true;
2739         }
2740
2741         int ret = overhullv((float3*)vertices,vcount,35,verts_out,verts_count_out,faces,index_count,inflate,120.0f,vlimit, arrtris);
2742         if(!ret) return false;
2743
2744         Array<int3> tris;
2745         int n=faces[0];
2746         int k=1;
2747         for(int i=0;i<n;i++)
2748         {
2749                 int pn = faces[k++];
2750                 for(int j=2;j<pn;j++) tris.Add(int3(faces[k],faces[k+j-1],faces[k+j]));
2751                 k+=pn;
2752         }
2753         assert(tris.count == index_count-1-(n*3));
2754
2755         result.mIndexCount = (unsigned int) (tris.count*3);
2756         result.mFaceCount  = (unsigned int) tris.count;
2757         result.mVertices   = (float*) verts_out;
2758         result.mVcount     = (unsigned int) verts_count_out;
2759         result.mIndices    = (unsigned int *) tris.element;
2760         tris.element=NULL; tris.count = tris.array_size=0;
2761
2762         return true;
2763 }
2764
2765
2766 void ReleaseHull(PHullResult &result)
2767 {
2768         if ( result.mIndices )
2769   {
2770           free(result.mIndices);
2771         }
2772
2773         result.mVcount = 0;
2774         result.mIndexCount = 0;
2775         result.mIndices = 0;
2776         result.mVertices = 0;
2777         result.mIndices  = 0;
2778 }
2779
2780
2781 //*********************************************************************
2782 //*********************************************************************
2783 //********  HullLib header
2784 //*********************************************************************
2785 //*********************************************************************
2786
2787 //*********************************************************************
2788 //*********************************************************************
2789 //********  HullLib implementation
2790 //*********************************************************************
2791 //*********************************************************************
2792
2793 HullError HullLibrary::CreateConvexHull(const HullDesc       &desc,           // describes the input request
2794                                                                                                                                                                         HullResult           &result)         // contains the resulst
2795 {
2796         HullError ret = QE_FAIL;
2797
2798
2799         PHullResult hr;
2800
2801         unsigned int vcount = desc.mVcount;
2802         if ( vcount < 8 ) vcount = 8;
2803
2804         float *vsource  = (float *) malloc( sizeof(float)*vcount*3);
2805
2806
2807         float scale[3];
2808
2809         unsigned int ovcount;
2810
2811         bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, vsource, desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates!
2812
2813         if ( ok )
2814         {
2815
2816
2817                 if ( 1 ) // scale vertices back to their original size.
2818                 {
2819                         for (unsigned int i=0; i<ovcount; i++)
2820                         {
2821                                 float *v = &vsource[i*3];
2822                                 v[0]*=scale[0];
2823                                 v[1]*=scale[1];
2824                                 v[2]*=scale[2];
2825                         }
2826                 }
2827
2828                 float skinwidth = 0;
2829                 if ( desc.HasHullFlag(QF_SKIN_WIDTH) )
2830                         skinwidth = desc.mSkinWidth;
2831
2832                 Array<btHullTriangle*> tris;
2833                 ok = ComputeHull(ovcount,vsource,hr,desc.mMaxVertices,skinwidth, tris);
2834
2835                 if ( ok )
2836                 {
2837
2838                         // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
2839                         float *vscratch = (float *) malloc( sizeof(float)*hr.mVcount*3);
2840                         BringOutYourDead(hr.mVertices,hr.mVcount, vscratch, ovcount, hr.mIndices, hr.mIndexCount );
2841
2842                         ret = QE_OK;
2843
2844                         if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle!
2845                         {
2846                                 result.mPolygons          = false;
2847                                 result.mNumOutputVertices = ovcount;
2848                                 result.mOutputVertices    = (float *)malloc( sizeof(float)*ovcount*3);
2849                                 result.mNumFaces          = hr.mFaceCount;
2850                                 result.mNumIndices        = hr.mIndexCount;
2851
2852                                 result.mIndices           = (unsigned int *) malloc( sizeof(unsigned int)*hr.mIndexCount);
2853
2854                                 memcpy(result.mOutputVertices, vscratch, sizeof(float)*3*ovcount );
2855
2856                         if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
2857                                 {
2858
2859                                         const unsigned int *source = hr.mIndices;
2860                                                                 unsigned int *dest   = result.mIndices;
2861
2862                                         for (unsigned int i=0; i<hr.mFaceCount; i++)
2863                                         {
2864                                                 dest[0] = source[2];
2865                                                 dest[1] = source[1];
2866                                                 dest[2] = source[0];
2867                                                 dest+=3;
2868                                                 source+=3;
2869                                         }
2870
2871                                 }
2872                                 else
2873                                 {
2874                                         memcpy(result.mIndices, hr.mIndices, sizeof(unsigned int)*hr.mIndexCount);
2875                                 }
2876                         }
2877                         else
2878                         {
2879                                 result.mPolygons          = true;
2880                                 result.mNumOutputVertices = ovcount;
2881                                 result.mOutputVertices    = (float *)malloc( sizeof(float)*ovcount*3);
2882                                 result.mNumFaces          = hr.mFaceCount;
2883                                 result.mNumIndices        = hr.mIndexCount+hr.mFaceCount;
2884                                 result.mIndices           = (unsigned int *) malloc( sizeof(unsigned int)*result.mNumIndices);
2885                                 memcpy(result.mOutputVertices, vscratch, sizeof(float)*3*ovcount );
2886
2887                                 if ( 1 )
2888                                 {
2889                                         const unsigned int *source = hr.mIndices;
2890                                                                 unsigned int *dest   = result.mIndices;
2891                                         for (unsigned int i=0; i<hr.mFaceCount; i++)
2892                                         {
2893                                                 dest[0] = 3;
2894                                                 if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
2895                                                 {
2896                                                         dest[1] = source[2];
2897                                                         dest[2] = source[1];
2898                                                         dest[3] = source[0];
2899                                                 }
2900                                                 else
2901                                                 {
2902                                                         dest[1] = source[0];
2903                                                         dest[2] = source[1];
2904                                                         dest[3] = source[2];
2905                                                 }
2906
2907                                                 dest+=4;
2908                                                 source+=3;
2909                                         }
2910                                 }
2911                         }
2912                         ReleaseHull(hr);
2913                         if ( vscratch )
2914                         {
2915                                 free(vscratch);
2916                         }
2917                 }
2918         }
2919
2920         if ( vsource )
2921         {
2922                 free(vsource);
2923         }
2924
2925
2926         return ret;
2927 }
2928
2929
2930
2931 HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it.
2932 {
2933         if ( result.mOutputVertices )
2934         {
2935                 free(result.mOutputVertices);
2936                 result.mOutputVertices = 0;
2937         }
2938         if ( result.mIndices )
2939         {
2940                 free(result.mIndices);
2941                 result.mIndices = 0;
2942         }
2943         return QE_OK;
2944 }
2945
2946
2947 static void addPoint(unsigned int &vcount,float *p,float x,float y,float z)
2948 {
2949         float *dest = &p[vcount*3];
2950         dest[0] = x;
2951         dest[1] = y;
2952         dest[2] = z;
2953         vcount++;
2954 }
2955
2956
2957 float GetDist(float px,float py,float pz,const float *p2)
2958 {
2959
2960         float dx = px - p2[0];
2961         float dy = py - p2[1];
2962         float dz = pz - p2[2];
2963
2964         return dx*dx+dy*dy+dz*dz;
2965 }
2966
2967
2968
2969 bool  HullLibrary::CleanupVertices(unsigned int svcount,
2970                                                                                                                                 const float *svertices,
2971                                                                                                                                 unsigned int stride,
2972                                                                                                                                 unsigned int &vcount,       // output number of vertices
2973                                                                                                                                 float *vertices,                 // location to store the results.
2974                                                                                                                                 float  normalepsilon,
2975                                                                                                                                 float *scale)
2976 {
2977         if ( svcount == 0 ) return false;
2978
2979
2980 #define EPSILON 0.000001f /* close enough to consider two floating point numbers to be 'the same'. */
2981
2982         vcount = 0;
2983
2984         float recip[3];
2985
2986         if ( scale )
2987         {
2988                 scale[0] = 1;
2989                 scale[1] = 1;
2990                 scale[2] = 1;
2991         }
2992
2993         float bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
2994         float bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
2995
2996         const char *vtx = (const char *) svertices;
2997
2998         if ( 1 )
2999         {
3000                 for (unsigned int i=0; i<svcount; i++)
3001                 {
3002                         const float *p = (const float *) vtx;
3003
3004                         vtx+=stride;
3005
3006                         for (int j=0; j<3; j++)
3007                         {
3008                                 if ( p[j] < bmin[j] ) bmin[j] = p[j];
3009                                 if ( p[j] > bmax[j] ) bmax[j] = p[j];
3010                         }
3011                 }
3012         }
3013
3014         float dx = bmax[0] - bmin[0];
3015         float dy = bmax[1] - bmin[1];
3016         float dz = bmax[2] - bmin[2];
3017
3018         float center[3];
3019
3020         center[0] = dx*0.5f + bmin[0];
3021         center[1] = dy*0.5f + bmin[1];
3022         center[2] = dz*0.5f + bmin[2];
3023
3024         if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
3025         {
3026
3027                 float len = FLT_MAX;
3028
3029                 if ( dx > EPSILON && dx < len ) len = dx;
3030                 if ( dy > EPSILON && dy < len ) len = dy;
3031                 if ( dz > EPSILON && dz < len ) len = dz;
3032
3033                 if ( len == FLT_MAX )
3034                 {
3035                         dx = dy = dz = 0.01f; // one centimeter
3036                 }
3037                 else
3038                 {
3039                         if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge.
3040                         if ( dy < EPSILON ) dy = len * 0.05f;
3041                         if ( dz < EPSILON ) dz = len * 0.05f;
3042                 }
3043
3044                 float x1 = center[0] - dx;
3045                 float x2 = center[0] + dx;
3046
3047                 float y1 = center[1] - dy;
3048                 float y2 = center[1] + dy;
3049
3050                 float z1 = center[2] - dz;
3051                 float z2 = center[2] + dz;
3052
3053                 addPoint(vcount,vertices,x1,y1,z1);
3054                 addPoint(vcount,vertices,x2,y1,z1);
3055                 addPoint(vcount,vertices,x2,y2,z1);
3056                 addPoint(vcount,vertices,x1,y2,z1);
3057                 addPoint(vcount,vertices,x1,y1,z2);
3058                 addPoint(vcount,vertices,x2,y1,z2);
3059                 addPoint(vcount,vertices,x2,y2,z2);
3060                 addPoint(vcount,vertices,x1,y2,z2);
3061
3062                 return true; // return cube
3063
3064
3065         }
3066         else
3067         {
3068                 if ( scale )
3069                 {
3070                         scale[0] = dx;
3071                         scale[1] = dy;
3072                         scale[2] = dz;
3073
3074                         recip[0] = 1 / dx;
3075                         recip[1] = 1 / dy;
3076                         recip[2] = 1 / dz;
3077
3078                         center[0]*=recip[0];
3079                         center[1]*=recip[1];
3080                         center[2]*=recip[2];
3081
3082                 }
3083
3084         }
3085
3086
3087
3088         vtx = (const char *) svertices;
3089
3090         for (unsigned int i=0; i<svcount; i++)
3091         {
3092
3093                 const float *p = (const float *)vtx;
3094                 vtx+=stride;
3095
3096                 float px = p[0];
3097                 float py = p[1];
3098                 float pz = p[2];
3099
3100                 if ( scale )
3101                 {
3102                         px = px*recip[0]; // normalize
3103                         py = py*recip[1]; // normalize
3104                         pz = pz*recip[2]; // normalize
3105                 }
3106
3107                 if ( 1 )
3108                 {
3109                         unsigned int j;
3110
3111                         for (j=0; j<vcount; j++)
3112                         {
3113                                 float *v = &vertices[j*3];
3114
3115                                 float x = v[0];
3116                                 float y = v[1];
3117                                 float z = v[2];
3118
3119                                 float dx = fabsf(x - px );
3120                                 float dy = fabsf(y - py );
3121                                 float dz = fabsf(z - pz );
3122
3123                                 if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
3124                                 {
3125                                         // ok, it is close enough to the old one
3126                                         // now let us see if it is further from the center of the point cloud than the one we already recorded.
3127                                         // in which case we keep this one instead.
3128
3129                                         float dist1 = GetDist(px,py,pz,center);
3130                                         float dist2 = GetDist(v[0],v[1],v[2],center);
3131
3132                                         if ( dist1 > dist2 )
3133                                         {
3134                                                 v[0] = px;
3135                                                 v[1] = py;
3136                                                 v[2] = pz;
3137                                         }
3138
3139                                         break;
3140                                 }
3141                         }
3142
3143                         if ( j == vcount )
3144                         {
3145                                 float *dest = &vertices[vcount*3];
3146                                 dest[0] = px;
3147                                 dest[1] = py;
3148                                 dest[2] = pz;
3149                                 vcount++;
3150                         }
3151                 }
3152         }
3153
3154         // ok..now make sure we didn't prune so many vertices it is now invalid.
3155         if ( 1 )
3156         {
3157                 float bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
3158                 float bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
3159
3160                 for (unsigned int i=0; i<vcount; i++)
3161                 {
3162                         const float *p = &vertices[i*3];
3163                         for (int j=0; j<3; j++)
3164                         {
3165                                 if ( p[j] < bmin[j] ) bmin[j] = p[j];
3166                                 if ( p[j] > bmax[j] ) bmax[j] = p[j];
3167                         }
3168                 }
3169
3170                 float dx = bmax[0] - bmin[0];
3171                 float dy = bmax[1] - bmin[1];
3172                 float dz = bmax[2] - bmin[2];
3173
3174                 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
3175                 {
3176                         float cx = dx*0.5f + bmin[0];
3177                         float cy = dy*0.5f + bmin[1];
3178                         float cz = dz*0.5f + bmin[2];
3179
3180                         float len = FLT_MAX;
3181
3182                         if ( dx >= EPSILON && dx < len ) len = dx;
3183                         if ( dy >= EPSILON && dy < len ) len = dy;
3184                         if ( dz >= EPSILON && dz < len ) len = dz;
3185
3186                         if ( len == FLT_MAX )
3187                         {
3188                                 dx = dy = dz = 0.01f; // one centimeter
3189                         }
3190                         else
3191                         {
3192                                 if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge.
3193                                 if ( dy < EPSILON ) dy = len * 0.05f;
3194                                 if ( dz < EPSILON ) dz = len * 0.05f;
3195                         }
3196
3197                         float x1 = cx - dx;
3198                         float x2 = cx + dx;
3199
3200                         float y1 = cy - dy;
3201                         float y2 = cy + dy;
3202
3203                         float z1 = cz - dz;
3204                         float z2 = cz + dz;
3205
3206                         vcount = 0; // add box
3207
3208                         addPoint(vcount,vertices,x1,y1,z1);
3209                         addPoint(vcount,vertices,x2,y1,z1);
3210                         addPoint(vcount,vertices,x2,y2,z1);
3211                         addPoint(vcount,vertices,x1,y2,z1);
3212                         addPoint(vcount,vertices,x1,y1,z2);
3213                         addPoint(vcount,vertices,x2,y1,z2);
3214                         addPoint(vcount,vertices,x2,y2,z2);
3215                         addPoint(vcount,vertices,x1,y2,z2);
3216
3217                         return true;
3218                 }
3219         }
3220
3221         return true;
3222 }
3223
3224 void HullLibrary::BringOutYourDead(const float *verts,unsigned int vcount, float *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
3225 {
3226         unsigned int *used = (unsigned int *)malloc(sizeof(unsigned int)*vcount);
3227         memset(used,0,sizeof(unsigned int)*vcount);
3228
3229         ocount = 0;
3230
3231         for (unsigned int i=0; i<indexcount; i++)
3232         {
3233                 unsigned int v = indices[i]; // original array index
3234
3235                 assert( v >= 0 && v < vcount );
3236
3237                 if ( used[v] ) // if already remapped
3238                 {
3239                         indices[i] = used[v]-1; // index to new array
3240                 }
3241                 else
3242                 {
3243
3244                         indices[i] = ocount;      // new index mapping
3245
3246                         overts[ocount*3+0] = verts[v*3+0]; // copy old vert to new vert array
3247                         overts[ocount*3+1] = verts[v*3+1];
3248                         overts[ocount*3+2] = verts[v*3+2];
3249
3250                         ocount++; // increment output vert count
3251
3252                         assert( ocount >=0 && ocount <= vcount );
3253
3254                         used[v] = ocount; // assign new index remapping
3255                 }
3256         }
3257
3258         free(used);
3259 }
3260
3261 }