diff options
Diffstat (limited to 'tests/bullet/Extras/ConvexDecomposition/cd_hull.cpp')
| -rw-r--r-- | tests/bullet/Extras/ConvexDecomposition/cd_hull.cpp | 3260 |
1 files changed, 3260 insertions, 0 deletions
diff --git a/tests/bullet/Extras/ConvexDecomposition/cd_hull.cpp b/tests/bullet/Extras/ConvexDecomposition/cd_hull.cpp new file mode 100644 index 00000000..b85b6494 --- /dev/null +++ b/tests/bullet/Extras/ConvexDecomposition/cd_hull.cpp @@ -0,0 +1,3260 @@ +#include "float_math.h" + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <assert.h> +#include <math.h> +#include <float.h> + +/*---------------------------------------------------------------------- + Copyright (c) 2004 Open Dynamics Framework Group + www.physicstools.org + All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are permitted provided + that the following conditions are met: + + Redistributions of source code must retain the above copyright notice, this list of conditions + and the following disclaimer. + + Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + Neither the name of the Open Dynamics Framework Group nor the names of its contributors may + be used to endorse or promote products derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, + INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER + IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +-----------------------------------------------------------------------*/ + +// http://codesuppository.blogspot.com +// +// mailto: jratcliff@infiniplex.net +// +// http://www.amillionpixels.us +// + +#include "cd_hull.h" + +using namespace ConvexDecomposition; + +/*---------------------------------------------------------------------- + Copyright (c) 2004 Open Dynamics Framework Group + www.physicstools.org + All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are permitted provided + that the following conditions are met: + + Redistributions of source code must retain the above copyright notice, this list of conditions + and the following disclaimer. + + Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + Neither the name of the Open Dynamics Framework Group nor the names of its contributors may + be used to endorse or promote products derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, + INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER + IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +-----------------------------------------------------------------------*/ + +#define PI 3.14159264f + +//***************************************************** +//***************************************************** +//********* Stan Melax's vector math template needed +//********* to use his hull building code. +//***************************************************** +//***************************************************** + +#define DEG2RAD (PI / 180.0f) +#define RAD2DEG (180.0f / PI) +#define SQRT_OF_2 (1.4142135f) +#define OFFSET(Class,Member) (((char*) (&(((Class*)NULL)-> Member )))- ((char*)NULL)) + +namespace ConvexDecomposition +{ + + +int argmin(float a[],int n); +float sqr(float a); +float clampf(float a) ; +float Round(float a,float precision); +float Interpolate(const float &f0,const float &f1,float alpha) ; + +template <class T> +void Swap(T &a,T &b) +{ + T tmp = a; + a=b; + b=tmp; +} + + + +template <class T> +T Max(const T &a,const T &b) +{ + return (a>b)?a:b; +} + +template <class T> +T Min(const T &a,const T &b) +{ + return (a<b)?a:b; +} + +//---------------------------------- + +class int3 +{ +public: + int x,y,z; + int3(){}; + int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;} + const int& operator[](int i) const {return (&x)[i];} + int& operator[](int i) {return (&x)[i];} +}; + + +//-------- 2D -------- + +class float2 +{ +public: + float x,y; + float2(){x=0;y=0;}; + float2(float _x,float _y){x=_x;y=_y;} + float& operator[](int i) {assert(i>=0&&i<2);return ((float*)this)[i];} + const float& operator[](int i) const {assert(i>=0&&i<2);return ((float*)this)[i];} +}; +inline float2 operator-( const float2& a, const float2& b ){return float2(a.x-b.x,a.y-b.y);} +inline float2 operator+( const float2& a, const float2& b ){return float2(a.x+b.x,a.y+b.y);} + +//--------- 3D --------- + +class float3 // 3D +{ + public: + float x,y,z; + float3(){x=0;y=0;z=0;}; + float3(float _x,float _y,float _z){x=_x;y=_y;z=_z;}; + //operator float *() { return &x;}; + float& operator[](int i) {assert(i>=0&&i<3);return ((float*)this)[i];} + const float& operator[](int i) const {assert(i>=0&&i<3);return ((float*)this)[i];} +# ifdef PLUGIN_3DSMAX + float3(const Point3 &p):x(p.x),y(p.y),z(p.z){} + operator Point3(){return *((Point3*)this);} +# endif +}; + + +float3& operator+=( float3 &a, const float3& b ); +float3& operator-=( float3 &a ,const float3& b ); +float3& operator*=( float3 &v ,const float s ); +float3& operator/=( float3 &v, const float s ); + +float magnitude( const float3& v ); +float3 normalize( const float3& v ); +float3 safenormalize(const float3 &v); +float3 vabs(const float3 &v); +float3 operator+( const float3& a, const float3& b ); +float3 operator-( const float3& a, const float3& b ); +float3 operator-( const float3& v ); +float3 operator*( const float3& v, const float s ); +float3 operator*( const float s, const float3& v ); +float3 operator/( const float3& v, const float s ); +inline int operator==( const float3 &a, const float3 &b ) { return (a.x==b.x && a.y==b.y && a.z==b.z); } +inline int operator!=( const float3 &a, const float3 &b ) { return (a.x!=b.x || a.y!=b.y || a.z!=b.z); } +// due to ambiguity and inconsistent standards ther are no overloaded operators for mult such as va*vb. +float dot( const float3& a, const float3& b ); +float3 cmul( const float3 &a, const float3 &b); +float3 cross( const float3& a, const float3& b ); +float3 Interpolate(const float3 &v0,const float3 &v1,float alpha); +float3 Round(const float3& a,float precision); +float3 VectorMax(const float3 &a, const float3 &b); +float3 VectorMin(const float3 &a, const float3 &b); + + + +class float3x3 +{ + public: + float3 x,y,z; // the 3 rows of the Matrix + float3x3(){} + 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){} + float3x3(float3 _x,float3 _y,float3 _z):x(_x),y(_y),z(_z){} + float3& operator[](int i) {assert(i>=0&&i<3);return (&x)[i];} + const float3& operator[](int i) const {assert(i>=0&&i<3);return (&x)[i];} + float& operator()(int r, int c) {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];} + const float& operator()(int r, int c) const {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];} +}; +float3x3 Transpose( const float3x3& m ); +float3 operator*( const float3& v , const float3x3& m ); +float3 operator*( const float3x3& m , const float3& v ); +float3x3 operator*( const float3x3& m , const float& s ); +float3x3 operator*( const float3x3& ma, const float3x3& mb ); +float3x3 operator/( const float3x3& a, const float& s ) ; +float3x3 operator+( const float3x3& a, const float3x3& b ); +float3x3 operator-( const float3x3& a, const float3x3& b ); +float3x3 &operator+=( float3x3& a, const float3x3& b ); +float3x3 &operator-=( float3x3& a, const float3x3& b ); +float3x3 &operator*=( float3x3& a, const float& s ); +float Determinant(const float3x3& m ); +float3x3 Inverse(const float3x3& a); // its just 3x3 so we simply do that cofactor method + + +//-------- 4D Math -------- + +class float4 +{ +public: + float x,y,z,w; + float4(){x=0;y=0;z=0;w=0;}; + float4(float _x,float _y,float _z,float _w){x=_x;y=_y;z=_z;w=_w;} + float4(const float3 &v,float _w){x=v.x;y=v.y;z=v.z;w=_w;} + //operator float *() { return &x;}; + float& operator[](int i) {assert(i>=0&&i<4);return ((float*)this)[i];} + const float& operator[](int i) const {assert(i>=0&&i<4);return ((float*)this)[i];} + const float3& xyz() const { return *((float3*)this);} + float3& xyz() { return *((float3*)this);} +}; + + +struct D3DXMATRIX; + +class float4x4 +{ + public: + float4 x,y,z,w; // the 4 rows + float4x4(){} + float4x4(const float4 &_x, const float4 &_y, const float4 &_z, const float4 &_w):x(_x),y(_y),z(_z),w(_w){} + float4x4(float m00, float m01, float m02, float m03, + float m10, float m11, float m12, float m13, + float m20, float m21, float m22, float m23, + float m30, float m31, float m32, float m33 ) + :x(m00,m01,m02,m03),y(m10,m11,m12,m13),z(m20,m21,m22,m23),w(m30,m31,m32,m33){} + float& operator()(int r, int c) {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];} + const float& operator()(int r, int c) const {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];} + operator float* () {return &x.x;} + operator const float* () const {return &x.x;} + operator struct D3DXMATRIX* () { return (struct D3DXMATRIX*) this;} + operator const struct D3DXMATRIX* () const { return (struct D3DXMATRIX*) this;} +}; + + +int operator==( const float4 &a, const float4 &b ); +float4 Homogenize(const float3 &v3,const float &w=1.0f); // Turns a 3D float3 4D vector4 by appending w +float4 cmul( const float4 &a, const float4 &b); +float4 operator*( const float4 &v, float s); +float4 operator*( float s, const float4 &v); +float4 operator+( const float4 &a, const float4 &b); +float4 operator-( const float4 &a, const float4 &b); +float4x4 operator*( const float4x4& a, const float4x4& b ); +float4 operator*( const float4& v, const float4x4& m ); +float4x4 Inverse(const float4x4 &m); +float4x4 MatrixRigidInverse(const float4x4 &m); +float4x4 MatrixTranspose(const float4x4 &m); +float4x4 MatrixPerspectiveFov(float fovy, float Aspect, float zn, float zf ); +float4x4 MatrixTranslation(const float3 &t); +float4x4 MatrixRotationZ(const float angle_radians); +float4x4 MatrixLookAt(const float3& eye, const float3& at, const float3& up); +int operator==( const float4x4 &a, const float4x4 &b ); + + +//-------- Quaternion ------------ + +class Quaternion :public float4 +{ + public: + Quaternion() { x = y = z = 0.0f; w = 1.0f; } + 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; } + Quaternion(float _x, float _y, float _z, float _w){x=_x;y=_y;z=_z;w=_w;} + float angle() const { return acosf(w)*2.0f; } + 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)); } + float3 xdir() const { return float3( 1-2*(y*y+z*z), 2*(x*y+w*z), 2*(x*z-w*y) ); } + float3 ydir() const { return float3( 2*(x*y-w*z),1-2*(x*x+z*z), 2*(y*z+w*x) ); } + float3 zdir() const { return float3( 2*(x*z+w*y), 2*(y*z-w*x),1-2*(x*x+y*y) ); } + float3x3 getmatrix() const { return float3x3( xdir(), ydir(), zdir() ); } + operator float3x3() { return getmatrix(); } + void Normalize(); +}; + +Quaternion& operator*=(Quaternion& a, float s ); +Quaternion operator*( const Quaternion& a, float s ); +Quaternion operator*( const Quaternion& a, const Quaternion& b); +Quaternion operator+( const Quaternion& a, const Quaternion& b ); +Quaternion normalize( Quaternion a ); +float dot( const Quaternion &a, const Quaternion &b ); +float3 operator*( const Quaternion& q, const float3& v ); +float3 operator*( const float3& v, const Quaternion& q ); +Quaternion slerp( Quaternion a, const Quaternion& b, float interp ); +Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,float alpha); +Quaternion RotationArc(float3 v0, float3 v1 ); // returns quat q where q*v0=v1 +Quaternion Inverse(const Quaternion &q); +float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v); + + +//------ Euler Angle ----- + +Quaternion YawPitchRoll( float yaw, float pitch, float roll ); +float Yaw( const Quaternion& q ); +float Pitch( const Quaternion& q ); +float Roll( Quaternion q ); +float Yaw( const float3& v ); +float Pitch( const float3& v ); + + +//------- Plane ---------- + +class Plane +{ + public: + float3 normal; + float dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + Plane(const float3 &n,float d):normal(n),dist(d){} + Plane():normal(),dist(0){} + void Transform(const float3 &position, const Quaternion &orientation); +}; + +inline Plane PlaneFlip(const Plane &plane){return Plane(-plane.normal,-plane.dist);} +inline int operator==( const Plane &a, const Plane &b ) { return (a.normal==b.normal && a.dist==b.dist); } +inline int coplanar( const Plane &a, const Plane &b ) { return (a==b || a==PlaneFlip(b)); } + + +//--------- Utility Functions ------ + +float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1); +float3 PlaneProject(const Plane &plane, const float3 &point); +float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a); // projects a onto infinite line p0p1 +float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a); +float3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2); +int PolyHit(const float3 *vert,const int n,const float3 &v0, const float3 &v1, float3 *impact=NULL, float3 *normal=NULL); +int BoxInside(const float3 &p,const float3 &bmin, const float3 &bmax) ; +int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax, float3 *impact); +float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint=NULL, float3 *vpoint=NULL); +float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2); +float3 NormalOf(const float3 *vert, const int n); +Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir0, const float3 &dir1); + + +float sqr(float a) {return a*a;} +float clampf(float a) {return Min(1.0f,Max(0.0f,a));} + + +float Round(float a,float precision) +{ + return floorf(0.5f+a/precision)*precision; +} + + +float Interpolate(const float &f0,const float &f1,float alpha) +{ + return f0*(1-alpha) + f1*alpha; +} + + +int argmin(float a[],int n) +{ + int r=0; + for(int i=1;i<n;i++) + { + if(a[i]<a[r]) + { + r = i; + } + } + return r; +} + + + +//------------ float3 (3D) -------------- + + + +float3 operator+( const float3& a, const float3& b ) +{ + return float3(a.x+b.x, a.y+b.y, a.z+b.z); +} + + +float3 operator-( const float3& a, const float3& b ) +{ + return float3( a.x-b.x, a.y-b.y, a.z-b.z ); +} + + +float3 operator-( const float3& v ) +{ + return float3( -v.x, -v.y, -v.z ); +} + + +float3 operator*( const float3& v, float s ) +{ + return float3( v.x*s, v.y*s, v.z*s ); +} + + +float3 operator*( float s, const float3& v ) +{ + return float3( v.x*s, v.y*s, v.z*s ); +} + + +float3 operator/( const float3& v, float s ) +{ + return v*(1.0f/s); +} + +float dot( const float3& a, const float3& b ) +{ + return a.x*b.x + a.y*b.y + a.z*b.z; +} + +float3 cmul( const float3 &v1, const float3 &v2) +{ + return float3(v1.x*v2.x, v1.y*v2.y, v1.z*v2.z); +} + + +float3 cross( const float3& a, const float3& b ) +{ + return float3( a.y*b.z - a.z*b.y, + a.z*b.x - a.x*b.z, + a.x*b.y - a.y*b.x ); +} + + + + +float3& operator+=( float3& a , const float3& b ) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; + return a; +} + + +float3& operator-=( float3& a , const float3& b ) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; + return a; +} + + +float3& operator*=(float3& v , float s ) +{ + v.x *= s; + v.y *= s; + v.z *= s; + return v; +} + + +float3& operator/=(float3& v , float s ) +{ + float sinv = 1.0f / s; + v.x *= sinv; + v.y *= sinv; + v.z *= sinv; + return v; +} + +float3 vabs(const float3 &v) +{ + return float3(fabsf(v.x),fabsf(v.y),fabsf(v.z)); +} + + +float magnitude( const float3& v ) +{ + return sqrtf(sqr(v.x) + sqr( v.y)+ sqr(v.z)); +} + + + +float3 normalize( const float3 &v ) +{ + // this routine, normalize, is ok, provided magnitude works!! + float d=magnitude(v); + if (d==0) + { + printf("Cant normalize ZERO vector\n"); + assert(0);// yes this could go here + d=0.1f; + } + d = 1/d; + return float3(v.x*d,v.y*d,v.z*d); +} + +float3 safenormalize(const float3 &v) +{ + if(magnitude(v)<=0.0f) + { + return float3(1,0,0); + } + return normalize(v); +} + +float3 Round(const float3 &a,float precision) +{ + return float3(Round(a.x,precision),Round(a.y,precision),Round(a.z,precision)); +} + + +float3 Interpolate(const float3 &v0,const float3 &v1,float alpha) +{ + return v0*(1-alpha) + v1*alpha; +} + +float3 VectorMin(const float3 &a,const float3 &b) +{ + return float3(Min(a.x,b.x),Min(a.y,b.y),Min(a.z,b.z)); +} +float3 VectorMax(const float3 &a,const float3 &b) +{ + return float3(Max(a.x,b.x),Max(a.y,b.y),Max(a.z,b.z)); +} + +// the statement v1*v2 is ambiguous since there are 3 types +// of vector multiplication +// - componantwise (for example combining colors) +// - dot product +// - cross product +// Therefore we never declare/implement this function. +// So we will never see: float3 operator*(float3 a,float3 b) + + + + +//------------ float3x3 --------------- +float Determinant(const float3x3 &m) +{ + 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 + -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 ; +} + +float3x3 Inverse(const float3x3 &a) +{ + float3x3 b; + float d=Determinant(a); + assert(d!=0); + for(int i=0;i<3;i++) + { + for(int j=0;j<3;j++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + int j1=(j+1)%3; + int j2=(j+2)%3; + // reverse indexs i&j to take transpose + b[j][i] = (a[i1][j1]*a[i2][j2]-a[i1][j2]*a[i2][j1])/d; + } + } + // Matrix check=a*b; // Matrix 'check' should be the identity (or close to it) + return b; +} + + +float3x3 Transpose( const float3x3& m ) +{ + return float3x3( float3(m.x.x,m.y.x,m.z.x), + float3(m.x.y,m.y.y,m.z.y), + float3(m.x.z,m.y.z,m.z.z)); +} + + +float3 operator*(const float3& v , const float3x3 &m ) { + return float3((m.x.x*v.x + m.y.x*v.y + m.z.x*v.z), + (m.x.y*v.x + m.y.y*v.y + m.z.y*v.z), + (m.x.z*v.x + m.y.z*v.y + m.z.z*v.z)); +} +float3 operator*(const float3x3 &m,const float3& v ) { + return float3(dot(m.x,v),dot(m.y,v),dot(m.z,v)); +} + + +float3x3 operator*( const float3x3& a, const float3x3& b ) +{ + return float3x3(a.x*b,a.y*b,a.z*b); +} + +float3x3 operator*( const float3x3& a, const float& s ) +{ + return float3x3(a.x*s, a.y*s ,a.z*s); +} +float3x3 operator/( const float3x3& a, const float& s ) +{ + float t=1/s; + return float3x3(a.x*t, a.y*t ,a.z*t); +} +float3x3 operator+( const float3x3& a, const float3x3& b ) +{ + return float3x3(a.x+b.x, a.y+b.y, a.z+b.z); +} +float3x3 operator-( const float3x3& a, const float3x3& b ) +{ + return float3x3(a.x-b.x, a.y-b.y, a.z-b.z); +} +float3x3 &operator+=( float3x3& a, const float3x3& b ) +{ + a.x+=b.x; + a.y+=b.y; + a.z+=b.z; + return a; +} +float3x3 &operator-=( float3x3& a, const float3x3& b ) +{ + a.x-=b.x; + a.y-=b.y; + a.z-=b.z; + return a; +} +float3x3 &operator*=( float3x3& a, const float& s ) +{ + a.x*=s; + a.y*=s; + a.z*=s; + return a; +} + + + +float3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2){ + float3x3 mp =Transpose(float3x3(p0.normal,p1.normal,p2.normal)); + float3x3 mi = Inverse(mp); + float3 b(p0.dist,p1.dist,p2.dist); + return -b * mi; +} + + +//--------------- 4D ---------------- + +float4 operator*( const float4& v, const float4x4& m ) +{ + return v.x*m.x + v.y*m.y + v.z*m.z + v.w*m.w; // yes this actually works +} + +int operator==( const float4 &a, const float4 &b ) +{ + return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w); +} + + +// Dont implement m*v for now, since that might confuse us +// All our transforms are based on multiplying the "row" vector on the left +//float4 operator*(const float4x4& m , const float4& v ) +//{ +// return float4(dot(v,m.x),dot(v,m.y),dot(v,m.z),dot(v,m.w)); +//} + + + +float4 cmul( const float4 &a, const float4 &b) +{ + return float4(a.x*b.x,a.y*b.y,a.z*b.z,a.w*b.w); +} + + +float4 operator*( const float4 &v, float s) +{ + return float4(v.x*s,v.y*s,v.z*s,v.w*s); +} + + +float4 operator*( float s, const float4 &v) +{ + return float4(v.x*s,v.y*s,v.z*s,v.w*s); +} + + +float4 operator+( const float4 &a, const float4 &b) +{ + return float4(a.x+b.x,a.y+b.y,a.z+b.z,a.w+b.w); +} + + + +float4 operator-( const float4 &a, const float4 &b) +{ + return float4(a.x-b.x,a.y-b.y,a.z-b.z,a.w-b.w); +} + + +float4 Homogenize(const float3 &v3,const float &w) +{ + return float4(v3.x,v3.y,v3.z,w); +} + + + +float4x4 operator*( const float4x4& a, const float4x4& b ) +{ + return float4x4(a.x*b,a.y*b,a.z*b,a.w*b); +} + +float4x4 MatrixTranspose(const float4x4 &m) +{ + return float4x4( + m.x.x, m.y.x, m.z.x, m.w.x, + m.x.y, m.y.y, m.z.y, m.w.y, + m.x.z, m.y.z, m.z.z, m.w.z, + m.x.w, m.y.w, m.z.w, m.w.w ); +} + +float4x4 MatrixRigidInverse(const float4x4 &m) +{ + float4x4 trans_inverse = MatrixTranslation(-m.w.xyz()); + float4x4 rot = m; + rot.w = float4(0,0,0,1); + return trans_inverse * MatrixTranspose(rot); +} + + +float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf ) +{ + float h = 1.0f/tanf(fovy/2.0f); // view space height + float w = h / aspect ; // view space width + return float4x4( + w, 0, 0 , 0, + 0, h, 0 , 0, + 0, 0, zf/(zn-zf) , -1, + 0, 0, zn*zf/(zn-zf) , 0 ); +} + + + +float4x4 MatrixLookAt(const float3& eye, const float3& at, const float3& up) +{ + float4x4 m; + m.w.w = 1.0f; + m.w.xyz() = eye; + m.z.xyz() = normalize(eye-at); + m.x.xyz() = normalize(cross(up,m.z.xyz())); + m.y.xyz() = cross(m.z.xyz(),m.x.xyz()); + return MatrixRigidInverse(m); +} + + +float4x4 MatrixTranslation(const float3 &t) +{ + return float4x4( + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + t.x,t.y,t.z,1 ); +} + + +float4x4 MatrixRotationZ(const float angle_radians) +{ + float s = sinf(angle_radians); + float c = cosf(angle_radians); + return float4x4( + c, s, 0, 0, + -s, c, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1 ); +} + + + +int operator==( const float4x4 &a, const float4x4 &b ) +{ + return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w); +} + + +float4x4 Inverse(const float4x4 &m) +{ + float4x4 d; + float *dst = &d.x.x; + float tmp[12]; /* temp array for pairs */ + float src[16]; /* array of transpose source matrix */ + float det; /* determinant */ + /* transpose matrix */ + for ( int i = 0; i < 4; i++) { + src[i] = m(i,0) ; + src[i + 4] = m(i,1); + src[i + 8] = m(i,2); + src[i + 12] = m(i,3); + } + /* calculate pairs for first 8 elements (cofactors) */ + tmp[0] = src[10] * src[15]; + tmp[1] = src[11] * src[14]; + tmp[2] = src[9] * src[15]; + tmp[3] = src[11] * src[13]; + tmp[4] = src[9] * src[14]; + tmp[5] = src[10] * src[13]; + tmp[6] = src[8] * src[15]; + tmp[7] = src[11] * src[12]; + tmp[8] = src[8] * src[14]; + tmp[9] = src[10] * src[12]; + tmp[10] = src[8] * src[13]; + tmp[11] = src[9] * src[12]; + /* calculate first 8 elements (cofactors) */ + dst[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7]; + dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7]; + dst[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7]; + dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7]; + dst[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7]; + dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7]; + dst[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6]; + dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6]; + dst[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3]; + dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3]; + dst[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3]; + dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3]; + dst[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3]; + dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3]; + dst[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2]; + dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2]; + /* calculate pairs for second 8 elements (cofactors) */ + tmp[0] = src[2]*src[7]; + tmp[1] = src[3]*src[6]; + tmp[2] = src[1]*src[7]; + tmp[3] = src[3]*src[5]; + tmp[4] = src[1]*src[6]; + tmp[5] = src[2]*src[5]; + tmp[6] = src[0]*src[7]; + tmp[7] = src[3]*src[4]; + tmp[8] = src[0]*src[6]; + tmp[9] = src[2]*src[4]; + tmp[10] = src[0]*src[5]; + tmp[11] = src[1]*src[4]; + /* calculate second 8 elements (cofactors) */ + dst[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15]; + dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15]; + dst[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15]; + dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15]; + dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15]; + dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15]; + dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14]; + dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14]; + dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9]; + dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10]; + dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10]; + dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8]; + dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8]; + dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9]; + dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9]; + dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8]; + /* calculate determinant */ + det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3]; + /* calculate matrix inverse */ + det = 1/det; + for ( int j = 0; j < 16; j++) + dst[j] *= det; + return d; +} + + +//--------- Quaternion -------------- + +Quaternion operator*( const Quaternion& a, const Quaternion& b ) +{ + Quaternion c; + c.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; + c.x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y; + c.y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x; + c.z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w; + return c; +} + + +Quaternion operator*( const Quaternion& a, float b ) +{ + return Quaternion(a.x*b, a.y*b, a.z*b ,a.w*b); +} + +Quaternion Inverse(const Quaternion &q) +{ + return Quaternion(-q.x,-q.y,-q.z,q.w); +} + +Quaternion& operator*=( Quaternion& q, const float s ) +{ + q.x *= s; + q.y *= s; + q.z *= s; + q.w *= s; + return q; +} +void Quaternion::Normalize() +{ + float m = sqrtf(sqr(w)+sqr(x)+sqr(y)+sqr(z)); + if(m<0.000000001f) { + w=1.0f; + x=y=z=0.0f; + return; + } + (*this) *= (1.0f/m); +} + +float3 operator*( const Quaternion& q, const float3& v ) +{ + // The following is equivalent to: + //return (q.getmatrix() * v); + float qx2 = q.x*q.x; + float qy2 = q.y*q.y; + float qz2 = q.z*q.z; + + float qxqy = q.x*q.y; + float qxqz = q.x*q.z; + float qxqw = q.x*q.w; + float qyqz = q.y*q.z; + float qyqw = q.y*q.w; + float qzqw = q.z*q.w; + return float3( + (1-2*(qy2+qz2))*v.x + (2*(qxqy-qzqw))*v.y + (2*(qxqz+qyqw))*v.z , + (2*(qxqy+qzqw))*v.x + (1-2*(qx2+qz2))*v.y + (2*(qyqz-qxqw))*v.z , + (2*(qxqz-qyqw))*v.x + (2*(qyqz+qxqw))*v.y + (1-2*(qx2+qy2))*v.z ); +} + +float3 operator*( const float3& v, const Quaternion& q ) +{ + assert(0); // must multiply with the quat on the left + return float3(0.0f,0.0f,0.0f); +} + +Quaternion operator+( const Quaternion& a, const Quaternion& b ) +{ + return Quaternion(a.x+b.x, a.y+b.y, a.z+b.z, a.w+b.w); +} + +float dot( const Quaternion &a,const Quaternion &b ) +{ + return (a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z); +} + +Quaternion normalize( Quaternion a ) +{ + float m = sqrtf(sqr(a.w)+sqr(a.x)+sqr(a.y)+sqr(a.z)); + if(m<0.000000001) + { + a.w=1; + a.x=a.y=a.z=0; + return a; + } + return a * (1/m); +} + +Quaternion slerp( Quaternion a, const Quaternion& b, float interp ) +{ + if(dot(a,b) <0.0) + { + a.w=-a.w; + a.x=-a.x; + a.y=-a.y; + a.z=-a.z; + } + float d = dot(a,b); + if(d>=1.0) { + return a; + } + float theta = acosf(d); + if(theta==0.0f) { return(a);} + return a*(sinf(theta-interp*theta)/sinf(theta)) + b*(sinf(interp*theta)/sinf(theta)); +} + + +Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,float alpha) { + return slerp(q0,q1,alpha); +} + + +Quaternion YawPitchRoll( float yaw, float pitch, float roll ) +{ + roll *= DEG2RAD; + yaw *= DEG2RAD; + pitch *= DEG2RAD; + 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); +} + +float Yaw( const Quaternion& q ) +{ + float3 v; + v=q.ydir(); + return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG; +} + +float Pitch( const Quaternion& q ) +{ + float3 v; + v=q.ydir(); + return atan2f(v.z,sqrtf(sqr(v.x)+sqr(v.y)))*RAD2DEG; +} + +float Roll( Quaternion q ) +{ + q = Quaternion(float3(0.0f,0.0f,1.0f),-Yaw(q)*DEG2RAD) *q; + q = Quaternion(float3(1.0f,0.0f,0.0f),-Pitch(q)*DEG2RAD) *q; + return atan2f(-q.xdir().z,q.xdir().x)*RAD2DEG; +} + +float Yaw( const float3& v ) +{ + return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG; +} + +float Pitch( const float3& v ) +{ + return atan2f(v.z,sqrtf(sqr(v.x)+sqr(v.y)))*RAD2DEG; +} + + +//------------- Plane -------------- + + +void Plane::Transform(const float3 &position, const Quaternion &orientation) { + // Transforms the plane to the space defined by the + // given position/orientation. + float3 newnormal; + float3 origin; + + newnormal = Inverse(orientation)*normal; + origin = Inverse(orientation)*(-normal*dist - position); + + normal = newnormal; + dist = -dot(newnormal, origin); +} + + + + +//--------- utility functions ------------- + +// RotationArc() +// Given two vectors v0 and v1 this function +// returns quaternion q where q*v0==v1. +// Routine taken from game programming gems. +Quaternion RotationArc(float3 v0,float3 v1){ + Quaternion q; + v0 = normalize(v0); // Comment these two lines out if you know its not needed. + v1 = normalize(v1); // If vector is already unit length then why do it again? + float3 c = cross(v0,v1); + float d = dot(v0,v1); + if(d<=-1.0f) { return Quaternion(1,0,0,0);} // 180 about x axis + float s = sqrtf((1+d)*2); + q.x = c.x / s; + q.y = c.y / s; + q.z = c.z / s; + q.w = s /2.0f; + return q; +} + + +float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v) +{ + // builds a 4x4 transformation matrix based on orientation q and translation v + float qx2 = q.x*q.x; + float qy2 = q.y*q.y; + float qz2 = q.z*q.z; + + float qxqy = q.x*q.y; + float qxqz = q.x*q.z; + float qxqw = q.x*q.w; + float qyqz = q.y*q.z; + float qyqw = q.y*q.w; + float qzqw = q.z*q.w; + + return float4x4( + 1-2*(qy2+qz2), + 2*(qxqy+qzqw), + 2*(qxqz-qyqw), + 0 , + 2*(qxqy-qzqw), + 1-2*(qx2+qz2), + 2*(qyqz+qxqw), + 0 , + 2*(qxqz+qyqw), + 2*(qyqz-qxqw), + 1-2*(qx2+qy2), + 0 , + v.x , + v.y , + v.z , + 1.0f ); +} + + +float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1) +{ + // returns the point where the line p0-p1 intersects the plane n&d + float3 dif; + dif = p1-p0; + float dn= dot(plane.normal,dif); + float t = -(plane.dist+dot(plane.normal,p0) )/dn; + return p0 + (dif*t); +} + +float3 PlaneProject(const Plane &plane, const float3 &point) +{ + return point - plane.normal * (dot(point,plane.normal)+plane.dist); +} + +float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a) +{ + float3 w; + w = p1-p0; + float t= dot(w,(a-p0)) / (sqr(w.x)+sqr(w.y)+sqr(w.z)); + return p0+ w*t; +} + + +float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a) +{ + float3 w; + w = p1-p0; + float t= dot(w,(a-p0)) / (sqr(w.x)+sqr(w.y)+sqr(w.z)); + return t; +} + + + +float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2) +{ + // return the normal of the triangle + // inscribed by v0, v1, and v2 + float3 cp=cross(v1-v0,v2-v1); + float m=magnitude(cp); + if(m==0) return float3(1,0,0); + return cp*(1.0f/m); +} + + + +int BoxInside(const float3 &p, const float3 &bmin, const float3 &bmax) +{ + return (p.x >= bmin.x && p.x <=bmax.x && + p.y >= bmin.y && p.y <=bmax.y && + p.z >= bmin.z && p.z <=bmax.z ); +} + + +int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax,float3 *impact) +{ + if(BoxInside(v0,bmin,bmax)) + { + *impact=v0; + return 1; + } + if(v0.x<=bmin.x && v1.x>=bmin.x) + { + float a = (bmin.x-v0.x)/(v1.x-v0.x); + //v.x = bmin.x; + float vy = (1-a) *v0.y + a*v1.y; + float vz = (1-a) *v0.z + a*v1.z; + if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z) + { + impact->x = bmin.x; + impact->y = vy; + impact->z = vz; + return 1; + } + } + else if(v0.x >= bmax.x && v1.x <= bmax.x) + { + float a = (bmax.x-v0.x)/(v1.x-v0.x); + //v.x = bmax.x; + float vy = (1-a) *v0.y + a*v1.y; + float vz = (1-a) *v0.z + a*v1.z; + if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z) + { + impact->x = bmax.x; + impact->y = vy; + impact->z = vz; + return 1; + } + } + if(v0.y<=bmin.y && v1.y>=bmin.y) + { + float a = (bmin.y-v0.y)/(v1.y-v0.y); + float vx = (1-a) *v0.x + a*v1.x; + //v.y = bmin.y; + float vz = (1-a) *v0.z + a*v1.z; + if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z) + { + impact->x = vx; + impact->y = bmin.y; + impact->z = vz; + return 1; + } + } + else if(v0.y >= bmax.y && v1.y <= bmax.y) + { + float a = (bmax.y-v0.y)/(v1.y-v0.y); + float vx = (1-a) *v0.x + a*v1.x; + // vy = bmax.y; + float vz = (1-a) *v0.z + a*v1.z; + if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z) + { + impact->x = vx; + impact->y = bmax.y; + impact->z = vz; + return 1; + } + } + if(v0.z<=bmin.z && v1.z>=bmin.z) + { + float a = (bmin.z-v0.z)/(v1.z-v0.z); + float vx = (1-a) *v0.x + a*v1.x; + float vy = (1-a) *v0.y + a*v1.y; + // v.z = bmin.z; + if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x) + { + impact->x = vx; + impact->y = vy; + impact->z = bmin.z; + return 1; + } + } + else if(v0.z >= bmax.z && v1.z <= bmax.z) + { + float a = (bmax.z-v0.z)/(v1.z-v0.z); + float vx = (1-a) *v0.x + a*v1.x; + float vy = (1-a) *v0.y + a*v1.y; + // v.z = bmax.z; + if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x) + { + impact->x = vx; + impact->y = vy; + impact->z = bmax.z; + return 1; + } + } + return 0; +} + + +float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint, float3 *vpoint) +{ + float3 cp; + cp = normalize(cross(udir,vdir)); + + float distu = -dot(cp,ustart); + float distv = -dot(cp,vstart); + float dist = (float)fabs(distu-distv); + if(upoint) + { + Plane plane; + plane.normal = normalize(cross(vdir,cp)); + plane.dist = -dot(plane.normal,vstart); + *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); + } + if(vpoint) + { + Plane plane; + plane.normal = normalize(cross(udir,cp)); + plane.dist = -dot(plane.normal,ustart); + *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); + } + return dist; +} + + +Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir1, const float3 &dir2) +{ + // routine taken from game programming gems. + // Implement track ball functionality to spin stuf on the screen + // cop center of projection + // cor center of rotation + // dir1 old mouse direction + // dir2 new mouse direction + // pretend there is a sphere around cor. Then find the points + // where dir1 and dir2 intersect that sphere. Find the + // rotation that takes the first point to the second. + float m; + // compute plane + float3 nrml = cor - cop; + float fudgefactor = 1.0f/(magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop + nrml = normalize(nrml); + float dist = -dot(nrml,cor); + float3 u= PlaneLineIntersection(Plane(nrml,dist),cop,cop+dir1); + u=u-cor; + u=u*fudgefactor; + m= magnitude(u); + if(m>1) + { + u/=m; + } + else + { + u=u - (nrml * sqrtf(1-m*m)); + } + float3 v= PlaneLineIntersection(Plane(nrml,dist),cop,cop+dir2); + v=v-cor; + v=v*fudgefactor; + m= magnitude(v); + if(m>1) + { + v/=m; + } + else + { + v=v - (nrml * sqrtf(1-m*m)); + } + return RotationArc(u,v); +} + + +int countpolyhit=0; +int PolyHit(const float3 *vert, const int n, const float3 &v0, const float3 &v1, float3 *impact, float3 *normal) +{ + countpolyhit++; + int i; + float3 nrml(0,0,0); + for(i=0;i<n;i++) + { + int i1=(i+1)%n; + int i2=(i+2)%n; + nrml = nrml + cross(vert[i1]-vert[i],vert[i2]-vert[i1]); + } + + float m = magnitude(nrml); + if(m==0.0) + { + return 0; + } + nrml = nrml * (1.0f/m); + float dist = -dot(nrml,vert[0]); + float d0,d1; + if((d0=dot(v0,nrml)+dist) <0 || (d1=dot(v1,nrml)+dist) >0) + { + return 0; + } + + float3 the_point; + // By using the cached plane distances d0 and d1 + // we can optimize the following: + // the_point = planelineintersection(nrml,dist,v0,v1); + float a = d0/(d0-d1); + the_point = v0*(1-a) + v1*a; + + + int inside=1; + for(int j=0;inside && j<n;j++) + { + // let inside = 0 if outside + float3 pp1,pp2,side; + pp1 = vert[j] ; + pp2 = vert[(j+1)%n]; + side = cross((pp2-pp1),(the_point-pp1)); + inside = (dot(nrml,side) >= 0.0); + } + if(inside) + { + if(normal){*normal=nrml;} + if(impact){*impact=the_point;} + } + return inside; +} + +//************************************************************************** +//************************************************************************** +//*** Stan Melax's array template, needed to compile his hull generation code +//************************************************************************** +//************************************************************************** + +template <class Type> class ArrayRet; +template <class Type> class Array { + public: + Array(int s=0); + Array(Array<Type> &array); + Array(ArrayRet<Type> &array); + ~Array(); + void allocate(int s); + void SetSize(int s); + void Pack(); + Type& Add(Type); + void AddUnique(Type); + int Contains(Type); + void Insert(Type,int); + int IndexOf(Type); + void Remove(Type); + void DelIndex(int i); + Type * element; + int count; + int array_size; + const Type &operator[](int i) const { assert(i>=0 && i<count); return element[i]; } + Type &operator[](int i) { assert(i>=0 && i<count); return element[i]; } + Type &Pop() { assert(count); count--; return element[count]; } + Array<Type> &operator=(Array<Type> &array); + Array<Type> &operator=(ArrayRet<Type> &array); + // operator ArrayRet<Type> &() { return *(ArrayRet<Type> *)this;} // this worked but i suspect could be dangerous +}; + +template <class Type> class ArrayRet:public Array<Type> +{ +}; + +template <class Type> Array<Type>::Array(int s) +{ + count=0; + array_size = 0; + element = NULL; + if(s) + { + allocate(s); + } +} + + +template <class Type> Array<Type>::Array(Array<Type> &array) +{ + count=0; + array_size = 0; + element = NULL; + for(int i=0;i<array.count;i++) + { + Add(array[i]); + } +} + + +template <class Type> Array<Type>::Array(ArrayRet<Type> &array) +{ + *this = array; +} +template <class Type> Array<Type> &Array<Type>::operator=(ArrayRet<Type> &array) +{ + count=array.count; + array_size = array.array_size; + element = array.element; + array.element=NULL; + array.count=0; + array.array_size=0; + return *this; +} + + +template <class Type> Array<Type> &Array<Type>::operator=(Array<Type> &array) +{ + count=0; + for(int i=0;i<array.count;i++) + { + Add(array[i]); + } + return *this; +} + +template <class Type> Array<Type>::~Array() +{ + if (element != NULL) + { + free(element); + } + count=0;array_size=0;element=NULL; +} + +template <class Type> void Array<Type>::allocate(int s) +{ + assert(s>0); + assert(s>=count); + Type *old = element; + array_size =s; + element = (Type *) malloc( sizeof(Type)*array_size); + assert(element); + for(int i=0;i<count;i++) + { + element[i]=old[i]; + } + if(old) + { + free(old); + } +} + +template <class Type> void Array<Type>::SetSize(int s) +{ + if(s==0) + { + if(element) + { + free(element); + element = NULL; + } + array_size = s; + } + else + { + allocate(s); + } + count=s; +} + +template <class Type> void Array<Type>::Pack() +{ + allocate(count); +} + +template <class Type> Type& Array<Type>::Add(Type t) +{ + assert(count<=array_size); + if(count==array_size) + { + allocate((array_size)?array_size *2:16); + } + element[count++] = t; + return element[count-1]; +} + +template <class Type> int Array<Type>::Contains(Type t) +{ + int i; + int found=0; + for(i=0;i<count;i++) + { + if(element[i] == t) found++; + } + return found; +} + +template <class Type> void Array<Type>::AddUnique(Type t) +{ + if(!Contains(t)) Add(t); +} + + +template <class Type> void Array<Type>::DelIndex(int i) +{ + assert(i<count); + count--; + while(i<count) + { + element[i] = element[i+1]; + i++; + } +} + +template <class Type> void Array<Type>::Remove(Type t) +{ + int i; + for(i=0;i<count;i++) + { + if(element[i] == t) + { + break; + } + } + assert(i<count); // assert object t is in the array. + DelIndex(i); + for(i=0;i<count;i++) + { + assert(element[i] != t); + } +} + +template <class Type> void Array<Type>::Insert(Type t,int k) +{ + int i=count; + Add(t); // to allocate space + while(i>k) + { + element[i]=element[i-1]; + i--; + } + assert(i==k); + element[k]=t; +} + + +template <class Type> int Array<Type>::IndexOf(Type t) +{ + int i; + for(i=0;i<count;i++) + { + if(element[i] == t) + { + return i; + } + } + assert(0); + return -1; +} + + + +//********************************************************************* +//********************************************************************* +//******** Hull header +//********************************************************************* +//********************************************************************* + +class PHullResult +{ +public: + + PHullResult(void) + { + mVcount = 0; + mIndexCount = 0; + mFaceCount = 0; + mVertices = 0; + mIndices = 0; + } + + unsigned int mVcount; + unsigned int mIndexCount; + unsigned int mFaceCount; + float *mVertices; + unsigned int *mIndices; +}; + + +#define REAL3 float3 +#define REAL float + +#define COPLANAR (0) +#define UNDER (1) +#define OVER (2) +#define SPLIT (OVER|UNDER) +#define PAPERWIDTH (0.001f) + +float planetestepsilon = PAPERWIDTH; + + +class ConvexH +{ + public: + class HalfEdge + { + public: + short ea; // the other half of the edge (index into edges list) + unsigned char v; // the vertex at the start of this edge (index into vertices list) + unsigned char p; // the facet on which this edge lies (index into facets list) + HalfEdge(){} + HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){} + }; + Array<REAL3> vertices; + Array<HalfEdge> edges; + Array<Plane> facets; + ConvexH(int vertices_size,int edges_size,int facets_size); +}; + +typedef ConvexH::HalfEdge HalfEdge; + +ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) + :vertices(vertices_size) + ,edges(edges_size) + ,facets(facets_size) +{ + vertices.count=vertices_size; + edges.count = edges_size; + facets.count = facets_size; +} + +ConvexH *ConvexHDup(ConvexH *src) { + ConvexH *dst = new ConvexH(src->vertices.count,src->edges.count,src->facets.count); + memcpy(dst->vertices.element,src->vertices.element,sizeof(float3)*src->vertices.count); + memcpy(dst->edges.element,src->edges.element,sizeof(HalfEdge)*src->edges.count); + memcpy(dst->facets.element,src->facets.element,sizeof(Plane)*src->facets.count); + return dst; +} + + +int PlaneTest(const Plane &p, const REAL3 &v) { + REAL a = dot(v,p.normal)+p.dist; + int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); + return flag; +} + +int SplitTest(ConvexH &convex,const Plane &plane) { + int flag=0; + for(int i=0;i<convex.vertices.count;i++) { + flag |= PlaneTest(plane,convex.vertices[i]); + } + return flag; +} + +class VertFlag +{ +public: + unsigned char planetest; + unsigned char junk; + unsigned char undermap; + unsigned char overmap; +}; +class EdgeFlag +{ +public: + unsigned char planetest; + unsigned char fixes; + short undermap; + short overmap; +}; +class PlaneFlag +{ +public: + unsigned char undermap; + unsigned char overmap; +}; +class Coplanar{ +public: + unsigned short ea; + unsigned char v0; + unsigned char v1; +}; + +int AssertIntact(ConvexH &convex) { + int i; + int estart=0; + for(i=0;i<convex.edges.count;i++) { + if(convex.edges[estart].p!= convex.edges[i].p) { + estart=i; + } + int inext = i+1; + if(inext>= convex.edges.count || convex.edges[inext].p != convex.edges[i].p) { + inext = estart; + } + assert(convex.edges[inext].p == convex.edges[i].p); + int nb = convex.edges[i].ea; + assert(nb!=255); + if(nb==255 || nb==-1) return 0; + assert(nb!=-1); + assert(i== convex.edges[nb].ea); + } + for(i=0;i<convex.edges.count;i++) { + assert(COPLANAR==PlaneTest(convex.facets[convex.edges[i].p],convex.vertices[convex.edges[i].v])); + if(COPLANAR!=PlaneTest(convex.facets[convex.edges[i].p],convex.vertices[convex.edges[i].v])) return 0; + if(convex.edges[estart].p!= convex.edges[i].p) { + estart=i; + } + int i1 = i+1; + if(i1>= convex.edges.count || convex.edges[i1].p != convex.edges[i].p) { + i1 = estart; + } + int i2 = i1+1; + if(i2>= convex.edges.count || convex.edges[i2].p != convex.edges[i].p) { + i2 = estart; + } + if(i==i2) continue; // i sliced tangent to an edge and created 2 meaningless edges + REAL3 localnormal = TriNormal(convex.vertices[convex.edges[i ].v], + convex.vertices[convex.edges[i1].v], + convex.vertices[convex.edges[i2].v]); + assert(dot(localnormal,convex.facets[convex.edges[i].p].normal)>0); + if(dot(localnormal,convex.facets[convex.edges[i].p].normal)<=0)return 0; + } + return 1; +} + +// back to back quads +ConvexH *test_btbq() { + ConvexH *convex = new ConvexH(4,8,2); + convex->vertices[0] = REAL3(0,0,0); + convex->vertices[1] = REAL3(1,0,0); + convex->vertices[2] = REAL3(1,1,0); + convex->vertices[3] = REAL3(0,1,0); + convex->facets[0] = Plane(REAL3(0,0,1),0); + convex->facets[1] = Plane(REAL3(0,0,-1),0); + convex->edges[0] = HalfEdge(7,0,0); + convex->edges[1] = HalfEdge(6,1,0); + convex->edges[2] = HalfEdge(5,2,0); + convex->edges[3] = HalfEdge(4,3,0); + + convex->edges[4] = HalfEdge(3,0,1); + convex->edges[5] = HalfEdge(2,3,1); + convex->edges[6] = HalfEdge(1,2,1); + convex->edges[7] = HalfEdge(0,1,1); + AssertIntact(*convex); + return convex; +} +ConvexH *test_cube() { + ConvexH *convex = new ConvexH(8,24,6); + convex->vertices[0] = REAL3(0,0,0); + convex->vertices[1] = REAL3(0,0,1); + convex->vertices[2] = REAL3(0,1,0); + convex->vertices[3] = REAL3(0,1,1); + convex->vertices[4] = REAL3(1,0,0); + convex->vertices[5] = REAL3(1,0,1); + convex->vertices[6] = REAL3(1,1,0); + convex->vertices[7] = REAL3(1,1,1); + + convex->facets[0] = Plane(REAL3(-1,0,0),0); + convex->facets[1] = Plane(REAL3(1,0,0),-1); + convex->facets[2] = Plane(REAL3(0,-1,0),0); + convex->facets[3] = Plane(REAL3(0,1,0),-1); + convex->facets[4] = Plane(REAL3(0,0,-1),0); + convex->facets[5] = Plane(REAL3(0,0,1),-1); + + convex->edges[0 ] = HalfEdge(11,0,0); + convex->edges[1 ] = HalfEdge(23,1,0); + convex->edges[2 ] = HalfEdge(15,3,0); + convex->edges[3 ] = HalfEdge(16,2,0); + + convex->edges[4 ] = HalfEdge(13,6,1); + convex->edges[5 ] = HalfEdge(21,7,1); + convex->edges[6 ] = HalfEdge( 9,5,1); + convex->edges[7 ] = HalfEdge(18,4,1); + + convex->edges[8 ] = HalfEdge(19,0,2); + convex->edges[9 ] = HalfEdge( 6,4,2); + convex->edges[10] = HalfEdge(20,5,2); + convex->edges[11] = HalfEdge( 0,1,2); + + convex->edges[12] = HalfEdge(22,3,3); + convex->edges[13] = HalfEdge( 4,7,3); + convex->edges[14] = HalfEdge(17,6,3); + convex->edges[15] = HalfEdge( 2,2,3); + + convex->edges[16] = HalfEdge( 3,0,4); + convex->edges[17] = HalfEdge(14,2,4); + convex->edges[18] = HalfEdge( 7,6,4); + convex->edges[19] = HalfEdge( 8,4,4); + + convex->edges[20] = HalfEdge(10,1,5); + convex->edges[21] = HalfEdge( 5,5,5); + convex->edges[22] = HalfEdge(12,7,5); + convex->edges[23] = HalfEdge( 1,3,5); + + + return convex; +} +ConvexH *ConvexHMakeCube(const REAL3 &bmin, const REAL3 &bmax) { + ConvexH *convex = test_cube(); + convex->vertices[0] = REAL3(bmin.x,bmin.y,bmin.z); + convex->vertices[1] = REAL3(bmin.x,bmin.y,bmax.z); + convex->vertices[2] = REAL3(bmin.x,bmax.y,bmin.z); + convex->vertices[3] = REAL3(bmin.x,bmax.y,bmax.z); + convex->vertices[4] = REAL3(bmax.x,bmin.y,bmin.z); + convex->vertices[5] = REAL3(bmax.x,bmin.y,bmax.z); + convex->vertices[6] = REAL3(bmax.x,bmax.y,bmin.z); + convex->vertices[7] = REAL3(bmax.x,bmax.y,bmax.z); + + convex->facets[0] = Plane(REAL3(-1,0,0), bmin.x); + convex->facets[1] = Plane(REAL3(1,0,0), -bmax.x); + convex->facets[2] = Plane(REAL3(0,-1,0), bmin.y); + convex->facets[3] = Plane(REAL3(0,1,0), -bmax.y); + convex->facets[4] = Plane(REAL3(0,0,-1), bmin.z); + convex->facets[5] = Plane(REAL3(0,0,1), -bmax.z); + return convex; +} +ConvexH *ConvexHCrop(ConvexH &convex,const Plane &slice) +{ + int i; + int vertcountunder=0; + int vertcountover =0; + Array<int> vertscoplanar; // existing vertex members of convex that are coplanar + vertscoplanar.count=0; + Array<int> edgesplit; // existing edges that members of convex that cross the splitplane + edgesplit.count=0; + + assert(convex.edges.count<480); + + EdgeFlag edgeflag[512]; + VertFlag vertflag[256]; + PlaneFlag planeflag[128]; + HalfEdge tmpunderedges[512]; + Plane tmpunderplanes[128]; + Coplanar coplanaredges[512]; + int coplanaredges_num=0; + + Array<REAL3> createdverts; + // do the side-of-plane tests + for(i=0;i<convex.vertices.count;i++) { + vertflag[i].planetest = PlaneTest(slice,convex.vertices[i]); + if(vertflag[i].planetest == COPLANAR) { + // ? vertscoplanar.Add(i); + vertflag[i].undermap = vertcountunder++; + vertflag[i].overmap = vertcountover++; + } + else if(vertflag[i].planetest == UNDER) { + vertflag[i].undermap = vertcountunder++; + } + else { + assert(vertflag[i].planetest == OVER); + vertflag[i].overmap = vertcountover++; + vertflag[i].undermap = 255; // for debugging purposes + } + } + int vertcountunderold = vertcountunder; // for debugging only + + int under_edge_count =0; + int underplanescount=0; + int e0=0; + + for(int currentplane=0; currentplane<convex.facets.count; currentplane++) { + int estart =e0; + int enextface = 0; + int planeside = 0; + int e1 = e0+1; + int vout=-1; + int vin =-1; + int coplanaredge = -1; + do{ + + if(e1 >= convex.edges.count || convex.edges[e1].p!=currentplane) { + enextface = e1; + e1=estart; + } + HalfEdge &edge0 = convex.edges[e0]; + HalfEdge &edge1 = convex.edges[e1]; + HalfEdge &edgea = convex.edges[edge0.ea]; + + + planeside |= vertflag[edge0.v].planetest; + //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest) == COPLANAR) { + // assert(ecop==-1); + // ecop=e; + //} + + + if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == OVER){ + // both endpoints over plane + edgeflag[e0].undermap = -1; + } + else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == UNDER) { + // at least one endpoint under, the other coplanar or under + + edgeflag[e0].undermap = under_edge_count; + tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; + tmpunderedges[under_edge_count].p = underplanescount; + if(edge0.ea < e0) { + // connect the neighbors + assert(edgeflag[edge0.ea].undermap !=-1); + tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; + tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count; + } + under_edge_count++; + } + else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == COPLANAR) { + // both endpoints coplanar + // must check a 3rd point to see if UNDER + int e2 = e1+1; + if(e2>=convex.edges.count || convex.edges[e2].p!=currentplane) { + e2 = estart; + } + assert(convex.edges[e2].p==currentplane); + HalfEdge &edge2 = convex.edges[e2]; + if(vertflag[edge2.v].planetest==UNDER) { + + edgeflag[e0].undermap = under_edge_count; + tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; + tmpunderedges[under_edge_count].p = underplanescount; + tmpunderedges[under_edge_count].ea = -1; + // make sure this edge is added to the "coplanar" list + coplanaredge = under_edge_count; + vout = vertflag[edge0.v].undermap; + vin = vertflag[edge1.v].undermap; + under_edge_count++; + } + else { + edgeflag[e0].undermap = -1; + } + } + else if(vertflag[edge0.v].planetest == UNDER && vertflag[edge1.v].planetest == OVER) { + // first is under 2nd is over + + edgeflag[e0].undermap = under_edge_count; + tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; + tmpunderedges[under_edge_count].p = underplanescount; + if(edge0.ea < e0) { + assert(edgeflag[edge0.ea].undermap !=-1); + // connect the neighbors + tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; + tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count; + vout = tmpunderedges[edgeflag[edge0.ea].undermap].v; + } + else { + Plane &p0 = convex.facets[edge0.p]; + Plane &pa = convex.facets[edgea.p]; + createdverts.Add(ThreePlaneIntersection(p0,pa,slice)); + //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); + //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); + vout = vertcountunder++; + } + under_edge_count++; + /// hmmm something to think about: i might be able to output this edge regarless of + // wheter or not we know v-in yet. ok i;ll try this now: + tmpunderedges[under_edge_count].v = vout; + tmpunderedges[under_edge_count].p = underplanescount; + tmpunderedges[under_edge_count].ea = -1; + coplanaredge = under_edge_count; + under_edge_count++; + + if(vin!=-1) { + // we previously processed an edge where we came under + // now we know about vout as well + + // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! + } + + } + else if(vertflag[edge0.v].planetest == COPLANAR && vertflag[edge1.v].planetest == OVER) { + // first is coplanar 2nd is over + + edgeflag[e0].undermap = -1; + vout = vertflag[edge0.v].undermap; + // I hate this but i have to make sure part of this face is UNDER before ouputting this vert + int k=estart; + assert(edge0.p == currentplane); + while(!(planeside&UNDER) && k<convex.edges.count && convex.edges[k].p==edge0.p) { + planeside |= vertflag[convex.edges[k].v].planetest; + k++; + } + if(planeside&UNDER){ + tmpunderedges[under_edge_count].v = vout; + tmpunderedges[under_edge_count].p = underplanescount; + tmpunderedges[under_edge_count].ea = -1; + coplanaredge = under_edge_count; // hmmm should make a note of the edge # for later on + under_edge_count++; + + } + } + else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == UNDER) { + // first is over next is under + // new vertex!!! + assert(vin==-1); + if(e0<edge0.ea) { + Plane &p0 = convex.facets[edge0.p]; + Plane &pa = convex.facets[edgea.p]; + createdverts.Add(ThreePlaneIntersection(p0,pa,slice)); + //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); + //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); + vin = vertcountunder++; + } + else { + // find the new vertex that was created by edge[edge0.ea] + int nea = edgeflag[edge0.ea].undermap; + assert(tmpunderedges[nea].p==tmpunderedges[nea+1].p); + vin = tmpunderedges[nea+1].v; + assert(vin < vertcountunder); + assert(vin >= vertcountunderold); // for debugging only + } + if(vout!=-1) { + // we previously processed an edge where we went over + // now we know vin too + // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! + } + // output edge + tmpunderedges[under_edge_count].v = vin; + tmpunderedges[under_edge_count].p = underplanescount; + edgeflag[e0].undermap = under_edge_count; + if(e0>edge0.ea) { + assert(edgeflag[edge0.ea].undermap !=-1); + // connect the neighbors + tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; + tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count; + } + assert(edgeflag[e0].undermap == under_edge_count); + under_edge_count++; + } + else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == COPLANAR) { + // first is over next is coplanar + + edgeflag[e0].undermap = -1; + vin = vertflag[edge1.v].undermap; + assert(vin!=-1); + if(vout!=-1) { + // we previously processed an edge where we came under + // now we know both endpoints + // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! + } + + } + else { + assert(0); + } + + + e0=e1; + e1++; // do the modulo at the beginning of the loop + + } while(e0!=estart) ; + e0 = enextface; + if(planeside&UNDER) { + planeflag[currentplane].undermap = underplanescount; + tmpunderplanes[underplanescount] = convex.facets[currentplane]; + underplanescount++; + } + else { + planeflag[currentplane].undermap = 0; + } + if(vout>=0 && (planeside&UNDER)) { + assert(vin>=0); + assert(coplanaredge>=0); + assert(coplanaredge!=511); + coplanaredges[coplanaredges_num].ea = coplanaredge; + coplanaredges[coplanaredges_num].v0 = vin; + coplanaredges[coplanaredges_num].v1 = vout; + coplanaredges_num++; + } + } + + // add the new plane to the mix: + if(coplanaredges_num>0) { + tmpunderplanes[underplanescount++]=slice; + } + for(i=0;i<coplanaredges_num-1;i++) { + if(coplanaredges[i].v1 != coplanaredges[i+1].v0) { + int j = 0; + for(j=i+2;j<coplanaredges_num;j++) { + if(coplanaredges[i].v1 == coplanaredges[j].v0) { + Coplanar tmp = coplanaredges[i+1]; + coplanaredges[i+1] = coplanaredges[j]; + coplanaredges[j] = tmp; + break; + } + } + if(j>=coplanaredges_num) + { + assert(j<coplanaredges_num); + return NULL; + } + } + } + ConvexH *punder = new ConvexH(vertcountunder,under_edge_count+coplanaredges_num,underplanescount); + ConvexH &under = *punder; + int k=0; + for(i=0;i<convex.vertices.count;i++) { + if(vertflag[i].planetest != OVER){ + under.vertices[k++] = convex.vertices[i]; + } + } + i=0; + while(k<vertcountunder) { + under.vertices[k++] = createdverts[i++]; + } + assert(i==createdverts.count); + + for(i=0;i<coplanaredges_num;i++) { + under.edges[under_edge_count+i].p = underplanescount-1; + under.edges[under_edge_count+i].ea = coplanaredges[i].ea; + tmpunderedges[coplanaredges[i].ea].ea = under_edge_count+i; + under.edges[under_edge_count+i].v = coplanaredges[i].v0; + } + + memcpy(under.edges.element,tmpunderedges,sizeof(HalfEdge)*under_edge_count); + memcpy(under.facets.element,tmpunderplanes,sizeof(Plane)*underplanescount); + return punder; +} + + + +static int candidateplane(Plane *planes,int planes_count,ConvexH *convex,float epsilon) +{ + int p = 0 ; + REAL md= 0 ; + int i; + for(i=0;i<planes_count;i++) + { + REAL d=0; + for(int j=0;j<convex->vertices.count;j++) + { + d = Max(d,dot(convex->vertices[j],planes[i].normal)+planes[i].dist); + } + if(i==0 || d>md) + { + p=i; + md=d; + } + } + return (md>epsilon)?p:-1; +} + +template<class T> +inline int maxdir(const T *p,int count,const T &dir) +{ + assert(count); + int m=0; + float currDotm = dot(p[0], dir); + for(int i=1;i<count;i++) + { + const float currDoti = dot(p[i], dir); + if(currDoti > currDotm) + { + currDotm = currDoti; + m=i; + } + } + return m; +} + + +template<class T> +int maxdirfiltered(const T *p,int count,const T &dir,Array<int> &allow) +{ + assert(count); + int m=-1; + float currDotm = dot(p[0], dir); + for(int i=0;i<count;i++) + { + if(allow[i]) + { + if(m==-1 ) + { + m=i; + } + else + { + const float currDoti = dot(p[i], dir); + if (currDoti>currDotm) + { + currDotm = currDoti; + m=i; + } + } + } + } + assert(m!=-1); + return m; +} + +float3 orth(const float3 &v) +{ + float3 a=cross(v,float3(0,0,1)); + float3 b=cross(v,float3(0,1,0)); + return normalize((magnitude(a)>magnitude(b))?a:b); +} + + +template<class T> +int maxdirsterid(const T *p,int count,const T &dir,Array<int> &allow) +{ + int m=-1; + while(m==-1) + { + m = maxdirfiltered(p,count,dir,allow); + if(allow[m]==3) return m; + T u = orth(dir); + T v = cross(u,dir); + int ma=-1; + for(float x = 0.0f ; x<= 360.0f ; x+= 45.0f) + { + float s = sinf(DEG2RAD*(x)); + float c = cosf(DEG2RAD*(x)); + int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow); + if(ma==m && mb==m) + { + allow[m]=3; + return m; + } + if(ma!=-1 && ma!=mb) // Yuck - this is really ugly + { + int mc = ma; + for(float xx = x-40.0f ; xx <= x ; xx+= 5.0f) + { + float s = sinf(DEG2RAD*(xx)); + float c = cosf(DEG2RAD*(xx)); + int md = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow); + if(mc==m && md==m) + { + allow[m]=3; + return m; + } + mc=md; + } + } + ma=mb; + } + allow[m]=0; + m=-1; + } + assert(0); + return m; +} + + + + +int operator ==(const int3 &a,const int3 &b) +{ + for(int i=0;i<3;i++) + { + if(a[i]!=b[i]) return 0; + } + return 1; +} + +int3 roll3(int3 a) +{ + int tmp=a[0]; + a[0]=a[1]; + a[1]=a[2]; + a[2]=tmp; + return a; +} +int isa(const int3 &a,const int3 &b) +{ + return ( a==b || roll3(a)==b || a==roll3(b) ); +} +int b2b(const int3 &a,const int3 &b) +{ + return isa(a,int3(b[2],b[1],b[0])); +} +int above(float3* vertices,const int3& t, const float3 &p, float epsilon) +{ + float3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); + return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? +} +int hasedge(const int3 &t, int a,int b) +{ + for(int i=0;i<3;i++) + { + int i1= (i+1)%3; + if(t[i]==a && t[i1]==b) return 1; + } + return 0; +} +int hasvert(const int3 &t, int v) +{ + return (t[0]==v || t[1]==v || t[2]==v) ; +} +int shareedge(const int3 &a,const int3 &b) +{ + int i; + for(i=0;i<3;i++) + { + int i1= (i+1)%3; + if(hasedge(a,b[i1],b[i])) return 1; + } + return 0; +} + +class btHullTriangle; + +//Array<btHullTriangle*> tris; + +class btHullTriangle : public int3 +{ +public: + int3 n; + int id; + int vmax; + float rise; + Array<btHullTriangle*>* tris; + btHullTriangle(int a,int b,int c, Array<btHullTriangle*>* pTris):int3(a,b,c),n(-1,-1,-1) + { + tris = pTris; + id = tris->count; + tris->Add(this); + vmax=-1; + rise = 0.0f; + } + ~btHullTriangle() + { + assert((*tris)[id]==this); + (*tris)[id]=NULL; + } + int &neib(int a,int b); +}; + + +int &btHullTriangle::neib(int a,int b) +{ + static int er=-1; + int i; + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + if((*this)[i]==a && (*this)[i1]==b) return n[i2]; + if((*this)[i]==b && (*this)[i1]==a) return n[i2]; + } + assert(0); + return er; +} +void b2bfix(btHullTriangle* s,btHullTriangle*t, Array<btHullTriangle*>& tris) +{ + int i; + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + int a = (*s)[i1]; + int b = (*s)[i2]; + assert(tris[s->neib(a,b)]->neib(b,a) == s->id); + assert(tris[t->neib(a,b)]->neib(b,a) == t->id); + tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a); + tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); + } +} + +void removeb2b(btHullTriangle* s,btHullTriangle*t, Array<btHullTriangle*>& tris) +{ + b2bfix(s,t, tris); + delete s; + delete t; +} + +void checkit(btHullTriangle *t, Array<btHullTriangle*>& tris) +{ + int i; + assert(tris[t->id]==t); + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + int a = (*t)[i1]; + int b = (*t)[i2]; + assert(a!=b); + assert( tris[t->n[i]]->neib(b,a) == t->id); + } +} +void extrude(btHullTriangle *t0,int v, Array<btHullTriangle*>& tris) +{ + int3 t= *t0; + int n = tris.count; + btHullTriangle* ta = new btHullTriangle(v,t[1],t[2], &tris); + ta->n = int3(t0->n[0],n+1,n+2); + tris[t0->n[0]]->neib(t[1],t[2]) = n+0; + btHullTriangle* tb = new btHullTriangle(v,t[2],t[0], &tris); + tb->n = int3(t0->n[1],n+2,n+0); + tris[t0->n[1]]->neib(t[2],t[0]) = n+1; + btHullTriangle* tc = new btHullTriangle(v,t[0],t[1], &tris); + tc->n = int3(t0->n[2],n+0,n+1); + tris[t0->n[2]]->neib(t[0],t[1]) = n+2; + checkit(ta, tris); + checkit(tb, tris); + checkit(tc, tris); + if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]], tris); + if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]], tris); + if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]], tris); + delete t0; + +} + +btHullTriangle *extrudable(float epsilon, Array<btHullTriangle*>& tris) +{ + int i; + btHullTriangle *t=NULL; + for(i=0;i<tris.count;i++) + { + if(!t || (tris[i] && t->rise<tris[i]->rise)) + { + t = tris[i]; + } + } + return (t->rise >epsilon)?t:NULL ; +} + +class int4 +{ +public: + int x,y,z,w; + int4(){}; + int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;} + const int& operator[](int i) const {return (&x)[i];} + int& operator[](int i) {return (&x)[i];} +}; + + + +int4 FindSimplex(float3 *verts,int verts_count,Array<int> &allow) +{ + float3 basis[3]; + basis[0] = float3( 0.01f, 0.02f, 1.0f ); + int p0 = maxdirsterid(verts,verts_count, basis[0],allow); + int p1 = maxdirsterid(verts,verts_count,-basis[0],allow); + basis[0] = verts[p0]-verts[p1]; + if(p0==p1 || basis[0]==float3(0,0,0)) + return int4(-1,-1,-1,-1); + basis[1] = cross(float3( 1, 0.02f, 0),basis[0]); + basis[2] = cross(float3(-0.02f, 1, 0),basis[0]); + basis[1] = normalize( (magnitude(basis[1])>magnitude(basis[2])) ? basis[1]:basis[2]); + int p2 = maxdirsterid(verts,verts_count,basis[1],allow); + if(p2 == p0 || p2 == p1) + { + p2 = maxdirsterid(verts,verts_count,-basis[1],allow); + } + if(p2 == p0 || p2 == p1) + return int4(-1,-1,-1,-1); + basis[1] = verts[p2] - verts[p0]; + basis[2] = normalize(cross(basis[1],basis[0])); + int p3 = maxdirsterid(verts,verts_count,basis[2],allow); + if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow); + if(p3==p0||p3==p1||p3==p2) + return int4(-1,-1,-1,-1); + assert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); + if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);} + return int4(p0,p1,p2,p3); +} + +int calchullgen(float3 *verts,int verts_count, int vlimit,Array<btHullTriangle*>& tris) +{ + if(verts_count <4) return 0; + if(vlimit==0) vlimit=1000000000; + int j; + float3 bmin(*verts),bmax(*verts); + Array<int> isextreme(verts_count); + Array<int> allow(verts_count); + for(j=0;j<verts_count;j++) + { + allow.Add(1); + isextreme.Add(0); + bmin = VectorMin(bmin,verts[j]); + bmax = VectorMax(bmax,verts[j]); + } + float epsilon = magnitude(bmax-bmin) * 0.001f; + + + int4 p = FindSimplex(verts,verts_count,allow); + if(p.x==-1) return 0; // simplex failed + + + + float3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) /4.0f; // a valid interior point + btHullTriangle *t0 = new btHullTriangle(p[2],p[3],p[1], &tris); t0->n=int3(2,3,1); + btHullTriangle *t1 = new btHullTriangle(p[3],p[2],p[0], &tris); t1->n=int3(3,2,0); + btHullTriangle *t2 = new btHullTriangle(p[0],p[1],p[3], &tris); t2->n=int3(0,1,3); + btHullTriangle *t3 = new btHullTriangle(p[1],p[0],p[2], &tris); t3->n=int3(1,0,2); + isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; + checkit(t0, tris);checkit(t1, tris);checkit(t2, tris);checkit(t3, tris); + + for(j=0;j<tris.count;j++) + { + btHullTriangle *t=tris[j]; + assert(t); + assert(t->vmax<0); + float3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); + t->vmax = maxdirsterid(verts,verts_count,n,allow); + t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); + } + btHullTriangle *te; + vlimit-=4; + while(vlimit >0 && (te=extrudable(epsilon, tris))) + { + int3 ti=*te; + int v=te->vmax; + assert(!isextreme[v]); // wtf we've already done this vertex + isextreme[v]=1; + //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already + j=tris.count; + while(j--) { + if(!tris[j]) continue; + int3 t=*tris[j]; + if(above(verts,t,verts[v],0.01f*epsilon)) + { + extrude(tris[j],v, tris); + } + } + // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle + j=tris.count; + while(j--) + { + if(!tris[j]) continue; + if(!hasvert(*tris[j],v)) break; + int3 nt=*tris[j]; + 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 ) + { + btHullTriangle *nb = tris[tris[j]->n[0]]; + assert(nb);assert(!hasvert(*nb,v));assert(nb->id<j); + extrude(nb,v, tris); + j=tris.count; + } + } + j=tris.count; + while(j--) + { + btHullTriangle *t=tris[j]; + if(!t) continue; + if(t->vmax>=0) break; + float3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); + t->vmax = maxdirsterid(verts,verts_count,n,allow); + if(isextreme[t->vmax]) + { + t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate. + } + else + { + t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); + } + } + vlimit --; + } + return 1; +} + +int calchull(float3 *verts,int verts_count, int *&tris_out, int &tris_count,int vlimit, Array<btHullTriangle*>& tris) +{ + int rc=calchullgen(verts,verts_count, vlimit, tris) ; + if(!rc) return 0; + Array<int> ts; + for(int i=0;i<tris.count;i++)if(tris[i]) + { + for(int j=0;j<3;j++)ts.Add((*tris[i])[j]); + delete tris[i]; + } + tris_count = ts.count/3; + tris_out = ts.element; + ts.element=NULL; ts.count=ts.array_size=0; + tris.count=0; + return 1; +} + +int calchullpbev(float3 *verts,int verts_count,int vlimit, Array<Plane> &planes,float bevangle, Array<btHullTriangle*>& tris) +{ + int i,j; + planes.count=0; + int rc = calchullgen(verts,verts_count,vlimit, tris); + if(!rc) return 0; + for(i=0;i<tris.count;i++)if(tris[i]) + { + Plane p; + btHullTriangle *t = tris[i]; + p.normal = TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); + p.dist = -dot(p.normal, verts[(*t)[0]]); + planes.Add(p); + for(j=0;j<3;j++) + { + if(t->n[j]<t->id) continue; + btHullTriangle *s = tris[t->n[j]]; + REAL3 snormal = TriNormal(verts[(*s)[0]],verts[(*s)[1]],verts[(*s)[2]]); + if(dot(snormal,p.normal)>=cos(bevangle*DEG2RAD)) continue; + REAL3 n = normalize(snormal+p.normal); + planes.Add(Plane(n,-dot(n,verts[maxdir(verts,verts_count,n)]))); + } + } + + for(i=0;i<tris.count;i++)if(tris[i]) + { + delete tris[i]; // delete tris[i]; + } + tris.count=0; + return 1; +} + +int overhull(Plane *planes,int planes_count,float3 *verts, int verts_count,int maxplanes, + float3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out ,float inflate) +{ + int i,j; + if(verts_count <4) return 0; + maxplanes = Min(maxplanes,planes_count); + float3 bmin(verts[0]),bmax(verts[0]); + for(i=0;i<verts_count;i++) + { + bmin = VectorMin(bmin,verts[i]); + bmax = VectorMax(bmax,verts[i]); + } +// float diameter = magnitude(bmax-bmin); +// inflate *=diameter; // RELATIVE INFLATION + bmin -= float3(inflate,inflate,inflate); + bmax += float3(inflate,inflate,inflate); + for(i=0;i<planes_count;i++) + { + planes[i].dist -= inflate; + } + float3 emin = bmin; // VectorMin(bmin,float3(0,0,0)); + float3 emax = bmax; // VectorMax(bmax,float3(0,0,0)); + float epsilon = magnitude(emax-emin) * 0.025f; + planetestepsilon = magnitude(emax-emin) * PAPERWIDTH; + // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think. + // ConvexH *convex = ConvexHMakeCube(bmin - float3(diameter,diameter,diameter),bmax+float3(diameter,diameter,diameter)); + ConvexH *c = ConvexHMakeCube(REAL3(bmin),REAL3(bmax)); + int k; + while(maxplanes-- && (k=candidateplane(planes,planes_count,c,epsilon))>=0) + { + ConvexH *tmp = c; + c = ConvexHCrop(*tmp,planes[k]); + if(c==NULL) {c=tmp; break;} // might want to debug this case better!!! + if(!AssertIntact(*c)) {c=tmp; break;} // might want to debug this case better too!!! + delete tmp; + } + + assert(AssertIntact(*c)); + //return c; + faces_out = (int*)malloc(sizeof(int)*(1+c->facets.count+c->edges.count)); // new int[1+c->facets.count+c->edges.count]; + faces_count_out=0; + i=0; + faces_out[faces_count_out++]=-1; + k=0; + while(i<c->edges.count) + { + j=1; + while(j+i<c->edges.count && c->edges[i].p==c->edges[i+j].p) { j++; } + faces_out[faces_count_out++]=j; + while(j--) + { + faces_out[faces_count_out++] = c->edges[i].v; + i++; + } + k++; + } + faces_out[0]=k; // number of faces. + assert(k==c->facets.count); + assert(faces_count_out == 1+c->facets.count+c->edges.count); + verts_out = c->vertices.element; // new float3[c->vertices.count]; + verts_count_out = c->vertices.count; + for(i=0;i<c->vertices.count;i++) + { + verts_out[i] = float3(c->vertices[i]); + } + c->vertices.count=c->vertices.array_size=0; c->vertices.element=NULL; + delete c; + return 1; +} + +int overhullv(float3 *verts, int verts_count,int maxplanes, + float3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out ,float inflate,float bevangle,int vlimit, Array<btHullTriangle*>& tris) +{ + if(!verts_count) return 0; + extern int calchullpbev(float3 *verts,int verts_count,int vlimit, Array<Plane> &planes,float bevangle, Array<btHullTriangle*>& tris) ; + Array<Plane> planes; + int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle, tris) ; + if(!rc) return 0; + return overhull(planes.element,planes.count,verts,verts_count,maxplanes,verts_out,verts_count_out,faces_out,faces_count_out,inflate); +} + + +bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,unsigned int vlimit,float inflate, Array<btHullTriangle*>& arrtris) +{ + + int index_count; + int *faces; + float3 *verts_out; + int verts_count_out; + + if(inflate==0.0f) + { + int *tris_out; + int tris_count; + int ret = calchull( (float3 *) vertices, (int) vcount, tris_out, tris_count, vlimit, arrtris ); + if(!ret) return false; + result.mIndexCount = (unsigned int) (tris_count*3); + result.mFaceCount = (unsigned int) tris_count; + result.mVertices = (float*) vertices; + result.mVcount = (unsigned int) vcount; + result.mIndices = (unsigned int *) tris_out; + return true; + } + + int ret = overhullv((float3*)vertices,vcount,35,verts_out,verts_count_out,faces,index_count,inflate,120.0f,vlimit, arrtris); + if(!ret) return false; + + Array<int3> tris; + int n=faces[0]; + int k=1; + for(int i=0;i<n;i++) + { + int pn = faces[k++]; + for(int j=2;j<pn;j++) tris.Add(int3(faces[k],faces[k+j-1],faces[k+j])); + k+=pn; + } + assert(tris.count == index_count-1-(n*3)); + + result.mIndexCount = (unsigned int) (tris.count*3); + result.mFaceCount = (unsigned int) tris.count; + result.mVertices = (float*) verts_out; + result.mVcount = (unsigned int) verts_count_out; + result.mIndices = (unsigned int *) tris.element; + tris.element=NULL; tris.count = tris.array_size=0; + + return true; +} + + +void ReleaseHull(PHullResult &result) +{ + if ( result.mIndices ) + { + free(result.mIndices); + } + + result.mVcount = 0; + result.mIndexCount = 0; + result.mIndices = 0; + result.mVertices = 0; + result.mIndices = 0; +} + + +//********************************************************************* +//********************************************************************* +//******** HullLib header +//********************************************************************* +//********************************************************************* + +//********************************************************************* +//********************************************************************* +//******** HullLib implementation +//********************************************************************* +//********************************************************************* + +HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request + HullResult &result) // contains the resulst +{ + HullError ret = QE_FAIL; + + + PHullResult hr; + + unsigned int vcount = desc.mVcount; + if ( vcount < 8 ) vcount = 8; + + float *vsource = (float *) malloc( sizeof(float)*vcount*3); + + + float scale[3]; + + unsigned int ovcount; + + bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, vsource, desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! + + if ( ok ) + { + + + if ( 1 ) // scale vertices back to their original size. + { + for (unsigned int i=0; i<ovcount; i++) + { + float *v = &vsource[i*3]; + v[0]*=scale[0]; + v[1]*=scale[1]; + v[2]*=scale[2]; + } + } + + float skinwidth = 0; + if ( desc.HasHullFlag(QF_SKIN_WIDTH) ) + skinwidth = desc.mSkinWidth; + + Array<btHullTriangle*> tris; + ok = ComputeHull(ovcount,vsource,hr,desc.mMaxVertices,skinwidth, tris); + + if ( ok ) + { + + // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. + float *vscratch = (float *) malloc( sizeof(float)*hr.mVcount*3); + BringOutYourDead(hr.mVertices,hr.mVcount, vscratch, ovcount, hr.mIndices, hr.mIndexCount ); + + ret = QE_OK; + + if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle! + { + result.mPolygons = false; + result.mNumOutputVertices = ovcount; + result.mOutputVertices = (float *)malloc( sizeof(float)*ovcount*3); + result.mNumFaces = hr.mFaceCount; + result.mNumIndices = hr.mIndexCount; + + result.mIndices = (unsigned int *) malloc( sizeof(unsigned int)*hr.mIndexCount); + + memcpy(result.mOutputVertices, vscratch, sizeof(float)*3*ovcount ); + + if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) + { + + const unsigned int *source = hr.mIndices; + unsigned int *dest = result.mIndices; + + for (unsigned int i=0; i<hr.mFaceCount; i++) + { + dest[0] = source[2]; + dest[1] = source[1]; + dest[2] = source[0]; + dest+=3; + source+=3; + } + + } + else + { + memcpy(result.mIndices, hr.mIndices, sizeof(unsigned int)*hr.mIndexCount); + } + } + else + { + result.mPolygons = true; + result.mNumOutputVertices = ovcount; + result.mOutputVertices = (float *)malloc( sizeof(float)*ovcount*3); + result.mNumFaces = hr.mFaceCount; + result.mNumIndices = hr.mIndexCount+hr.mFaceCount; + result.mIndices = (unsigned int *) malloc( sizeof(unsigned int)*result.mNumIndices); + memcpy(result.mOutputVertices, vscratch, sizeof(float)*3*ovcount ); + + if ( 1 ) + { + const unsigned int *source = hr.mIndices; + unsigned int *dest = result.mIndices; + for (unsigned int i=0; i<hr.mFaceCount; i++) + { + dest[0] = 3; + if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) + { + dest[1] = source[2]; + dest[2] = source[1]; + dest[3] = source[0]; + } + else + { + dest[1] = source[0]; + dest[2] = source[1]; + dest[3] = source[2]; + } + + dest+=4; + source+=3; + } + } + } + ReleaseHull(hr); + if ( vscratch ) + { + free(vscratch); + } + } + } + + if ( vsource ) + { + free(vsource); + } + + + return ret; +} + + + +HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it. +{ + if ( result.mOutputVertices ) + { + free(result.mOutputVertices); + result.mOutputVertices = 0; + } + if ( result.mIndices ) + { + free(result.mIndices); + result.mIndices = 0; + } + return QE_OK; +} + + +static void addPoint(unsigned int &vcount,float *p,float x,float y,float z) +{ + float *dest = &p[vcount*3]; + dest[0] = x; + dest[1] = y; + dest[2] = z; + vcount++; +} + + +float GetDist(float px,float py,float pz,const float *p2) +{ + + float dx = px - p2[0]; + float dy = py - p2[1]; + float dz = pz - p2[2]; + + return dx*dx+dy*dy+dz*dz; +} + + + +bool HullLibrary::CleanupVertices(unsigned int svcount, + const float *svertices, + unsigned int stride, + unsigned int &vcount, // output number of vertices + float *vertices, // location to store the results. + float normalepsilon, + float *scale) +{ + if ( svcount == 0 ) return false; + + +#define EPSILON 0.000001f /* close enough to consider two floating point numbers to be 'the same'. */ + + vcount = 0; + + float recip[3]; + + if ( scale ) + { + scale[0] = 1; + scale[1] = 1; + scale[2] = 1; + } + + float bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; + float bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; + + const char *vtx = (const char *) svertices; + + if ( 1 ) + { + for (unsigned int i=0; i<svcount; i++) + { + const float *p = (const float *) vtx; + + vtx+=stride; + + for (int j=0; j<3; j++) + { + if ( p[j] < bmin[j] ) bmin[j] = p[j]; + if ( p[j] > bmax[j] ) bmax[j] = p[j]; + } + } + } + + float dx = bmax[0] - bmin[0]; + float dy = bmax[1] - bmin[1]; + float dz = bmax[2] - bmin[2]; + + float center[3]; + + center[0] = dx*0.5f + bmin[0]; + center[1] = dy*0.5f + bmin[1]; + center[2] = dz*0.5f + bmin[2]; + + if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 ) + { + + float len = FLT_MAX; + + if ( dx > EPSILON && dx < len ) len = dx; + if ( dy > EPSILON && dy < len ) len = dy; + if ( dz > EPSILON && dz < len ) len = dz; + + if ( len == FLT_MAX ) + { + dx = dy = dz = 0.01f; // one centimeter + } + else + { + if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge. + if ( dy < EPSILON ) dy = len * 0.05f; + if ( dz < EPSILON ) dz = len * 0.05f; + } + + float x1 = center[0] - dx; + float x2 = center[0] + dx; + + float y1 = center[1] - dy; + float y2 = center[1] + dy; + + float z1 = center[2] - dz; + float z2 = center[2] + dz; + + addPoint(vcount,vertices,x1,y1,z1); + addPoint(vcount,vertices,x2,y1,z1); + addPoint(vcount,vertices,x2,y2,z1); + addPoint(vcount,vertices,x1,y2,z1); + addPoint(vcount,vertices,x1,y1,z2); + addPoint(vcount,vertices,x2,y1,z2); + addPoint(vcount,vertices,x2,y2,z2); + addPoint(vcount,vertices,x1,y2,z2); + + return true; // return cube + + + } + else + { + if ( scale ) + { + scale[0] = dx; + scale[1] = dy; + scale[2] = dz; + + recip[0] = 1 / dx; + recip[1] = 1 / dy; + recip[2] = 1 / dz; + + center[0]*=recip[0]; + center[1]*=recip[1]; + center[2]*=recip[2]; + + } + + } + + + + vtx = (const char *) svertices; + + for (unsigned int i=0; i<svcount; i++) + { + + const float *p = (const float *)vtx; + vtx+=stride; + + float px = p[0]; + float py = p[1]; + float pz = p[2]; + + if ( scale ) + { + px = px*recip[0]; // normalize + py = py*recip[1]; // normalize + pz = pz*recip[2]; // normalize + } + + if ( 1 ) + { + unsigned int j; + + for (j=0; j<vcount; j++) + { + float *v = &vertices[j*3]; + + float x = v[0]; + float y = v[1]; + float z = v[2]; + + float dx = fabsf(x - px ); + float dy = fabsf(y - py ); + float dz = fabsf(z - pz ); + + if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon ) + { + // ok, it is close enough to the old one + // now let us see if it is further from the center of the point cloud than the one we already recorded. + // in which case we keep this one instead. + + float dist1 = GetDist(px,py,pz,center); + float dist2 = GetDist(v[0],v[1],v[2],center); + + if ( dist1 > dist2 ) + { + v[0] = px; + v[1] = py; + v[2] = pz; + } + + break; + } + } + + if ( j == vcount ) + { + float *dest = &vertices[vcount*3]; + dest[0] = px; + dest[1] = py; + dest[2] = pz; + vcount++; + } + } + } + + // ok..now make sure we didn't prune so many vertices it is now invalid. + if ( 1 ) + { + float bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; + float bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; + + for (unsigned int i=0; i<vcount; i++) + { + const float *p = &vertices[i*3]; + for (int j=0; j<3; j++) + { + if ( p[j] < bmin[j] ) bmin[j] = p[j]; + if ( p[j] > bmax[j] ) bmax[j] = p[j]; + } + } + + float dx = bmax[0] - bmin[0]; + float dy = bmax[1] - bmin[1]; + float dz = bmax[2] - bmin[2]; + + if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) + { + float cx = dx*0.5f + bmin[0]; + float cy = dy*0.5f + bmin[1]; + float cz = dz*0.5f + bmin[2]; + + float len = FLT_MAX; + + if ( dx >= EPSILON && dx < len ) len = dx; + if ( dy >= EPSILON && dy < len ) len = dy; + if ( dz >= EPSILON && dz < len ) len = dz; + + if ( len == FLT_MAX ) + { + dx = dy = dz = 0.01f; // one centimeter + } + else + { + if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge. + if ( dy < EPSILON ) dy = len * 0.05f; + if ( dz < EPSILON ) dz = len * 0.05f; + } + + float x1 = cx - dx; + float x2 = cx + dx; + + float y1 = cy - dy; + float y2 = cy + dy; + + float z1 = cz - dz; + float z2 = cz + dz; + + vcount = 0; // add box + + addPoint(vcount,vertices,x1,y1,z1); + addPoint(vcount,vertices,x2,y1,z1); + addPoint(vcount,vertices,x2,y2,z1); + addPoint(vcount,vertices,x1,y2,z1); + addPoint(vcount,vertices,x1,y1,z2); + addPoint(vcount,vertices,x2,y1,z2); + addPoint(vcount,vertices,x2,y2,z2); + addPoint(vcount,vertices,x1,y2,z2); + + return true; + } + } + + return true; +} + +void HullLibrary::BringOutYourDead(const float *verts,unsigned int vcount, float *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount) +{ + unsigned int *used = (unsigned int *)malloc(sizeof(unsigned int)*vcount); + memset(used,0,sizeof(unsigned int)*vcount); + + ocount = 0; + + for (unsigned int i=0; i<indexcount; i++) + { + unsigned int v = indices[i]; // original array index + + assert( v >= 0 && v < vcount ); + + if ( used[v] ) // if already remapped + { + indices[i] = used[v]-1; // index to new array + } + else + { + + indices[i] = ocount; // new index mapping + + overts[ocount*3+0] = verts[v*3+0]; // copy old vert to new vert array + overts[ocount*3+1] = verts[v*3+1]; + overts[ocount*3+2] = verts[v*3+2]; + + ocount++; // increment output vert count + + assert( ocount >=0 && ocount <= vcount ); + + used[v] = ocount; // assign new index remapping + } + } + + free(used); +} + +} |