From d7489fd34884bcb25da1a5cdd209a4cecd0fd9c2 Mon Sep 17 00:00:00 2001 From: Ruslan Baratov Date: Tue, 5 Dec 2017 19:44:06 +0300 Subject: [PATCH] 'set ff=unix' --- CMakeLists.txt | 152 +- stanhull.cpp | 6424 ++++++++++++++++++++++++------------------------ stanhull.h | 312 +-- 3 files changed, 3444 insertions(+), 3444 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 47e5827..f354db7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,76 +1,76 @@ -# Copyright (c) 2016 David Hirvonen -# Copyright (c) 2017 Ruslan Baratov -# All rights reserved. - -cmake_minimum_required(VERSION 3.2) -project(stanhull VERSION 1.0.0) - -add_library(stanhull stanhull.cpp stanhull.h) - -set(generated_headers "${CMAKE_CURRENT_BINARY_DIR}/generated_headers") -set(stanhull_export "${generated_headers}/stanhull/STANHULL_EXPORT.h") -include(GenerateExportHeader) -generate_export_header(stanhull EXPORT_FILE_NAME ${stanhull_export}) - -target_include_directories( - stanhull - PUBLIC - "$" - "$" -) - -# Installation (https://github.com/forexample/package-example) { - -set(config_install_dir "lib/cmake/${PROJECT_NAME}") -set(include_install_dir "include") - -set(generated_dir "${CMAKE_CURRENT_BINARY_DIR}/generated") - -set(version_config "${generated_dir}/${PROJECT_NAME}ConfigVersion.cmake") -set(project_config "${generated_dir}/${PROJECT_NAME}Config.cmake") -set(TARGETS_EXPORT_NAME "${PROJECT_NAME}Targets") -set(namespace "${PROJECT_NAME}::") - -include(CMakePackageConfigHelpers) - -# Use: -# * PROJECT_VERSION -write_basic_package_version_file( - "${version_config}" COMPATIBILITY SameMajorVersion -) - -# Use variables: -# * TARGETS_EXPORT_NAME -# * PROJECT_NAME -configure_package_config_file( - "cmake/Config.cmake.in" - "${project_config}" - INSTALL_DESTINATION "${config_install_dir}" -) - -install( - TARGETS stanhull - EXPORT "${TARGETS_EXPORT_NAME}" - LIBRARY DESTINATION "lib" - ARCHIVE DESTINATION "lib" - RUNTIME DESTINATION "bin" - INCLUDES DESTINATION "${include_install_dir}" -) - -install( - FILES "${stanhull_export}" stanhull.h - DESTINATION "${include_install_dir}/${PROJECT_NAME}" -) - -install( - FILES "${project_config}" "${version_config}" - DESTINATION "${config_install_dir}" -) - -install( - EXPORT "${TARGETS_EXPORT_NAME}" - NAMESPACE "${namespace}" - DESTINATION "${config_install_dir}" -) - -# } +# Copyright (c) 2016 David Hirvonen +# Copyright (c) 2017 Ruslan Baratov +# All rights reserved. + +cmake_minimum_required(VERSION 3.2) +project(stanhull VERSION 1.0.0) + +add_library(stanhull stanhull.cpp stanhull.h) + +set(generated_headers "${CMAKE_CURRENT_BINARY_DIR}/generated_headers") +set(stanhull_export "${generated_headers}/stanhull/STANHULL_EXPORT.h") +include(GenerateExportHeader) +generate_export_header(stanhull EXPORT_FILE_NAME ${stanhull_export}) + +target_include_directories( + stanhull + PUBLIC + "$" + "$" +) + +# Installation (https://github.com/forexample/package-example) { + +set(config_install_dir "lib/cmake/${PROJECT_NAME}") +set(include_install_dir "include") + +set(generated_dir "${CMAKE_CURRENT_BINARY_DIR}/generated") + +set(version_config "${generated_dir}/${PROJECT_NAME}ConfigVersion.cmake") +set(project_config "${generated_dir}/${PROJECT_NAME}Config.cmake") +set(TARGETS_EXPORT_NAME "${PROJECT_NAME}Targets") +set(namespace "${PROJECT_NAME}::") + +include(CMakePackageConfigHelpers) + +# Use: +# * PROJECT_VERSION +write_basic_package_version_file( + "${version_config}" COMPATIBILITY SameMajorVersion +) + +# Use variables: +# * TARGETS_EXPORT_NAME +# * PROJECT_NAME +configure_package_config_file( + "cmake/Config.cmake.in" + "${project_config}" + INSTALL_DESTINATION "${config_install_dir}" +) + +install( + TARGETS stanhull + EXPORT "${TARGETS_EXPORT_NAME}" + LIBRARY DESTINATION "lib" + ARCHIVE DESTINATION "lib" + RUNTIME DESTINATION "bin" + INCLUDES DESTINATION "${include_install_dir}" +) + +install( + FILES "${stanhull_export}" stanhull.h + DESTINATION "${include_install_dir}/${PROJECT_NAME}" +) + +install( + FILES "${project_config}" "${version_config}" + DESTINATION "${config_install_dir}" +) + +install( + EXPORT "${TARGETS_EXPORT_NAME}" + NAMESPACE "${namespace}" + DESTINATION "${config_install_dir}" +) + +# } diff --git a/stanhull.cpp b/stanhull.cpp index adfebfa..9ffcca4 100644 --- a/stanhull.cpp +++ b/stanhull.cpp @@ -1,3212 +1,3212 @@ -#include -#include -#include -#include -#include -#include - -#include - -/*---------------------------------------------------------------------- - 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. ------------------------------------------------------------------------*/ - -// Modified by Lasse Oorni for Urho3D - -// Urho3D: use a namespace to not clash with Urho3D's inbuilt math types -namespace StanHull -{ - -#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)) - - - -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 -void Swap(T &a,T &b) -{ - T tmp = a; - a=b; - b=tmp; -} - - - -template -T Max(const T &a,const T &b) -{ - return (a>b)?a:b; -} - -template -T Min(const T &a,const T &b) -{ - return (a=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=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 ) -{ - static 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 ) -{ - static 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. - static float3 newnormal; - static 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){ - static 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 - static 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) -{ - static 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;i0) - { - return 0; - } - - static 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= 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 ArrayRet; -template class Array { - public: - Array(int s=0); - Array(Array &array); - Array(ArrayRet &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=0 && i &operator=(Array &array); - Array &operator=(ArrayRet &array); - // operator ArrayRet &() { return *(ArrayRet *)this;} // this worked but i suspect could be dangerous -}; - -template class ArrayRet:public Array -{ -}; - -template Array::Array(int s) -{ - count=0; - array_size = 0; - element = NULL; - if(s) - { - allocate(s); - } -} - - -template Array::Array(Array &array) -{ - count=0; - array_size = 0; - element = NULL; - for(int i=0;i Array::Array(ArrayRet &array) -{ - *this = array; -} -template Array &Array::operator=(ArrayRet &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 Array &Array::operator=(Array &array) -{ - count=0; - for(int i=0;i Array::~Array() -{ - if (element != NULL) - { - free(element); - } - count=0;array_size=0;element=NULL; -} - -template void Array::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 void Array::SetSize(int s) -{ - if(s==0) - { - if(element) - { - free(element); - element = NULL; - } - array_size = s; - } - else - { - allocate(s); - } - count=s; -} - -template void Array::Pack() -{ - allocate(count); -} - -template Type& Array::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 int Array::Contains(Type t) -{ - int i; - int found=0; - for(i=0;i void Array::AddUnique(Type t) -{ - if(!Contains(t)) Add(t); -} - - -template void Array::DelIndex(int i) -{ - assert(i void Array::Remove(Type t) -{ - int i; - for(i=0;i void Array::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 int Array::IndexOf(Type t) -{ - int i; - for(i=0;i vertices; - Array edges; - Array 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.edges.count || convex.edges[inext].p != convex.edges[i].p) { - inext = estart; - } - assert(convex.edges[inext].p == convex.edges[i].p); - HalfEdge &edge = convex.edges[i]; - 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 || 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; - int edgecountunder=0; - int edgecountover =0; - int planecountunder=0; - int planecountover =0; - static Array vertscoplanar; // existing vertex members of convex that are coplanar - vertscoplanar.count=0; - static Array 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 createdverts; - // do the side-of-plane tests - for(i=0;i= 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= 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) - { - assert(jvertices.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 -inline int maxdir(const T *p,int count,const T &dir) -{ - assert(count); - int m=0; - for(int i=1;idot(p[m],dir)) m=i; - } - return m; -} - - -template -int maxdirfiltered(const T *p,int count,const T &dir,Array &allow) -{ - assert(count); - int m=-1; - for(int i=0;idot(p[m],dir)) 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 -int maxdirsterid(const T *p,int count,const T &dir,Array &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 Tri; - -Array tris; - -class Tri : public int3 -{ -public: - int3 n; - int id; - int vmax; - float rise; - Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) - { - id = tris.count; - tris.Add(this); - vmax=-1; - rise = 0.0f; - } - ~Tri() - { - assert(tris[id]==this); - tris[id]=NULL; - } - int &neib(int a,int b); -}; - - -int &Tri::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(Tri* s,Tri*t) -{ - 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(Tri* s,Tri*t) -{ - b2bfix(s,t); - delete s; - delete t; -} - -void checkit(Tri *t) -{ - 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(Tri *t0,int v) -{ - int3 t= *t0; - int n = tris.count; - Tri* ta = new Tri(v,t[1],t[2]); - ta->n = int3(t0->n[0],n+1,n+2); - tris[t0->n[0]]->neib(t[1],t[2]) = n+0; - Tri* tb = new Tri(v,t[2],t[0]); - tb->n = int3(t0->n[1],n+2,n+0); - tris[t0->n[1]]->neib(t[2],t[0]) = n+1; - Tri* tc = new Tri(v,t[0],t[1]); - tc->n = int3(t0->n[2],n+0,n+1); - tris[t0->n[2]]->neib(t[0],t[1]) = n+2; - checkit(ta); - checkit(tb); - checkit(tc); - if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]]); - if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]]); - if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]]); - delete t0; - -} - -Tri *extrudable(float epsilon) -{ - int i; - Tri *t=NULL; - for(i=0;iriserise)) - { - 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 &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) -{ - if(verts_count <4) return 0; - if(vlimit==0) vlimit=1000000000; - int j; - float3 bmin(*verts),bmax(*verts); - Array isextreme(verts_count); - Array allow(verts_count); - for(j=0;jn=int3(2,3,1); - Tri *t1 = new Tri(p[3],p[2],p[0]); t1->n=int3(3,2,0); - Tri *t2 = new Tri(p[0],p[1],p[3]); t2->n=int3(0,1,3); - Tri *t3 = new Tri(p[1],p[0],p[2]); t3->n=int3(1,0,2); - isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; - checkit(t0);checkit(t1);checkit(t2);checkit(t3); - - for(j=0;jvmax<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]]); - } - Tri *te; - vlimit-=4; - while(vlimit >0 && (te=extrudable(epsilon))) - { - 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; - int newstart=j; - while(j--) { - if(!tris[j]) continue; - int3 t=*tris[j]; - if(above(verts,t,verts[v],0.01f*epsilon)) - { - extrude(tris[j],v); - } - } - // 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 ) - { - Tri *nb = tris[tris[j]->n[0]]; - assert(nb);assert(!hasvert(*nb,v));assert(nb->idvmax>=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) -{ - int rc=calchullgen(verts,verts_count, vlimit) ; - if(!rc) return 0; - Array ts; - for(int i=0;i &planes,float bevangle) -{ - int i,j; - planes.count=0; - int rc = calchullgen(verts,verts_count,vlimit); - if(!rc) return 0; - for(i=0;in[j]id) continue; - Tri *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=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(iedges.count) - { - j=1; - while(j+iedges.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;ivertices.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) -{ - if(!verts_count) return 0; - extern int calchullpbev(float3 *verts,int verts_count,int vlimit, Array &planes,float bevangle) ; - Array planes; - int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle) ; - 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) -{ - - 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 ); - 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); - if(!ret) return false; - - Array tris; - int n=faces[0]; - int k=1; - for(int i=0;i 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 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 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= 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); -} - -} - +#include +#include +#include +#include +#include +#include + +#include + +/*---------------------------------------------------------------------- + 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. +-----------------------------------------------------------------------*/ + +// Modified by Lasse Oorni for Urho3D + +// Urho3D: use a namespace to not clash with Urho3D's inbuilt math types +namespace StanHull +{ + +#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)) + + + +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 +void Swap(T &a,T &b) +{ + T tmp = a; + a=b; + b=tmp; +} + + + +template +T Max(const T &a,const T &b) +{ + return (a>b)?a:b; +} + +template +T Min(const T &a,const T &b) +{ + return (a=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=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 ) +{ + static 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 ) +{ + static 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. + static float3 newnormal; + static 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){ + static 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 + static 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) +{ + static 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;i0) + { + return 0; + } + + static 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= 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 ArrayRet; +template class Array { + public: + Array(int s=0); + Array(Array &array); + Array(ArrayRet &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=0 && i &operator=(Array &array); + Array &operator=(ArrayRet &array); + // operator ArrayRet &() { return *(ArrayRet *)this;} // this worked but i suspect could be dangerous +}; + +template class ArrayRet:public Array +{ +}; + +template Array::Array(int s) +{ + count=0; + array_size = 0; + element = NULL; + if(s) + { + allocate(s); + } +} + + +template Array::Array(Array &array) +{ + count=0; + array_size = 0; + element = NULL; + for(int i=0;i Array::Array(ArrayRet &array) +{ + *this = array; +} +template Array &Array::operator=(ArrayRet &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 Array &Array::operator=(Array &array) +{ + count=0; + for(int i=0;i Array::~Array() +{ + if (element != NULL) + { + free(element); + } + count=0;array_size=0;element=NULL; +} + +template void Array::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 void Array::SetSize(int s) +{ + if(s==0) + { + if(element) + { + free(element); + element = NULL; + } + array_size = s; + } + else + { + allocate(s); + } + count=s; +} + +template void Array::Pack() +{ + allocate(count); +} + +template Type& Array::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 int Array::Contains(Type t) +{ + int i; + int found=0; + for(i=0;i void Array::AddUnique(Type t) +{ + if(!Contains(t)) Add(t); +} + + +template void Array::DelIndex(int i) +{ + assert(i void Array::Remove(Type t) +{ + int i; + for(i=0;i void Array::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 int Array::IndexOf(Type t) +{ + int i; + for(i=0;i vertices; + Array edges; + Array 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.edges.count || convex.edges[inext].p != convex.edges[i].p) { + inext = estart; + } + assert(convex.edges[inext].p == convex.edges[i].p); + HalfEdge &edge = convex.edges[i]; + 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 || 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; + int edgecountunder=0; + int edgecountover =0; + int planecountunder=0; + int planecountover =0; + static Array vertscoplanar; // existing vertex members of convex that are coplanar + vertscoplanar.count=0; + static Array 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 createdverts; + // do the side-of-plane tests + for(i=0;i= 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= 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) + { + assert(jvertices.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 +inline int maxdir(const T *p,int count,const T &dir) +{ + assert(count); + int m=0; + for(int i=1;idot(p[m],dir)) m=i; + } + return m; +} + + +template +int maxdirfiltered(const T *p,int count,const T &dir,Array &allow) +{ + assert(count); + int m=-1; + for(int i=0;idot(p[m],dir)) 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 +int maxdirsterid(const T *p,int count,const T &dir,Array &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 Tri; + +Array tris; + +class Tri : public int3 +{ +public: + int3 n; + int id; + int vmax; + float rise; + Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) + { + id = tris.count; + tris.Add(this); + vmax=-1; + rise = 0.0f; + } + ~Tri() + { + assert(tris[id]==this); + tris[id]=NULL; + } + int &neib(int a,int b); +}; + + +int &Tri::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(Tri* s,Tri*t) +{ + 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(Tri* s,Tri*t) +{ + b2bfix(s,t); + delete s; + delete t; +} + +void checkit(Tri *t) +{ + 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(Tri *t0,int v) +{ + int3 t= *t0; + int n = tris.count; + Tri* ta = new Tri(v,t[1],t[2]); + ta->n = int3(t0->n[0],n+1,n+2); + tris[t0->n[0]]->neib(t[1],t[2]) = n+0; + Tri* tb = new Tri(v,t[2],t[0]); + tb->n = int3(t0->n[1],n+2,n+0); + tris[t0->n[1]]->neib(t[2],t[0]) = n+1; + Tri* tc = new Tri(v,t[0],t[1]); + tc->n = int3(t0->n[2],n+0,n+1); + tris[t0->n[2]]->neib(t[0],t[1]) = n+2; + checkit(ta); + checkit(tb); + checkit(tc); + if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]]); + if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]]); + if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]]); + delete t0; + +} + +Tri *extrudable(float epsilon) +{ + int i; + Tri *t=NULL; + for(i=0;iriserise)) + { + 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 &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) +{ + if(verts_count <4) return 0; + if(vlimit==0) vlimit=1000000000; + int j; + float3 bmin(*verts),bmax(*verts); + Array isextreme(verts_count); + Array allow(verts_count); + for(j=0;jn=int3(2,3,1); + Tri *t1 = new Tri(p[3],p[2],p[0]); t1->n=int3(3,2,0); + Tri *t2 = new Tri(p[0],p[1],p[3]); t2->n=int3(0,1,3); + Tri *t3 = new Tri(p[1],p[0],p[2]); t3->n=int3(1,0,2); + isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; + checkit(t0);checkit(t1);checkit(t2);checkit(t3); + + for(j=0;jvmax<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]]); + } + Tri *te; + vlimit-=4; + while(vlimit >0 && (te=extrudable(epsilon))) + { + 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; + int newstart=j; + while(j--) { + if(!tris[j]) continue; + int3 t=*tris[j]; + if(above(verts,t,verts[v],0.01f*epsilon)) + { + extrude(tris[j],v); + } + } + // 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 ) + { + Tri *nb = tris[tris[j]->n[0]]; + assert(nb);assert(!hasvert(*nb,v));assert(nb->idvmax>=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) +{ + int rc=calchullgen(verts,verts_count, vlimit) ; + if(!rc) return 0; + Array ts; + for(int i=0;i &planes,float bevangle) +{ + int i,j; + planes.count=0; + int rc = calchullgen(verts,verts_count,vlimit); + if(!rc) return 0; + for(i=0;in[j]id) continue; + Tri *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=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(iedges.count) + { + j=1; + while(j+iedges.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;ivertices.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) +{ + if(!verts_count) return 0; + extern int calchullpbev(float3 *verts,int verts_count,int vlimit, Array &planes,float bevangle) ; + Array planes; + int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle) ; + 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) +{ + + 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 ); + 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); + if(!ret) return false; + + Array tris; + int n=faces[0]; + int k=1; + for(int i=0;i 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 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 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= 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); +} + +} + diff --git a/stanhull.h b/stanhull.h index 6150261..2584d95 100644 --- a/stanhull.h +++ b/stanhull.h @@ -1,156 +1,156 @@ -#ifndef STANHULL_H -#define STANHULL_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. ------------------------------------------------------------------------*/ - -#include - -// Modified by Lasse Oorni for Urho3D - -// Urho3D: use a namespace to not clash with Urho3D's inbuilt math types -namespace StanHull -{ - -class HullResult -{ -public: - HullResult(void) - { - mPolygons = true; - mNumOutputVertices = 0; - mOutputVertices = 0; - mNumFaces = 0; - mNumIndices = 0; - mIndices = 0; - } - bool mPolygons; // true if indices represents polygons, false indices are triangles - unsigned int mNumOutputVertices; // number of vertices in the output hull - float *mOutputVertices; // array of vertices, 3 floats each x,y,z - unsigned int mNumFaces; // the number of faces produced - unsigned int mNumIndices; // the total number of indices - unsigned int *mIndices; // pointer to indices. - -// If triangles, then indices are array indexes into the vertex list. -// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. -}; - -enum HullFlag -{ - QF_TRIANGLES = (1<<0), // report results as triangles, not polygons. - QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices. - QF_SKIN_WIDTH = (1<<2), // extrude hull based on this skin width - QF_DEFAULT = 0 -}; - - -class HullDesc -{ -public: - HullDesc(void) - { - mFlags = QF_DEFAULT; - mVcount = 0; - mVertices = 0; - mVertexStride = sizeof(float)*3; - mNormalEpsilon = 0.001f; - mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. - mMaxFaces = 4096; - mSkinWidth = 0.01f; // default is one centimeter - }; - - HullDesc(HullFlag flag, - unsigned int vcount, - const float *vertices, - unsigned int stride) - { - mFlags = flag; - mVcount = vcount; - mVertices = vertices; - mVertexStride = stride; - mNormalEpsilon = 0.001f; - mMaxVertices = 4096; - mSkinWidth = 0.01f; // default is one centimeter - } - - bool HasHullFlag(HullFlag flag) const - { - if ( mFlags & flag ) return true; - return false; - } - - void SetHullFlag(HullFlag flag) - { - mFlags|=flag; - } - - void ClearHullFlag(HullFlag flag) - { - mFlags&=~flag; - } - - unsigned int mFlags; // flags to use when generating the convex hull. - unsigned int mVcount; // number of vertices in the input point cloud - const float *mVertices; // the array of vertices. - unsigned int mVertexStride; // the stride of each vertex, in bytes. - float mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. - float mSkinWidth; - unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! - unsigned int mMaxFaces; -}; - -enum HullError -{ - QE_OK, // success! - QE_FAIL // failed. -}; - -class STANHULL_EXPORT HullLibrary -{ -public: - - HullError CreateConvexHull(const HullDesc &desc, // describes the input request - HullResult &result); // contains the resulst - - HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it. - -private: - - void BringOutYourDead(const float *verts,unsigned int vcount, float *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount); - - bool 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); -}; - -} - -#endif // STANHULL_H +#ifndef STANHULL_H +#define STANHULL_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. +-----------------------------------------------------------------------*/ + +#include + +// Modified by Lasse Oorni for Urho3D + +// Urho3D: use a namespace to not clash with Urho3D's inbuilt math types +namespace StanHull +{ + +class HullResult +{ +public: + HullResult(void) + { + mPolygons = true; + mNumOutputVertices = 0; + mOutputVertices = 0; + mNumFaces = 0; + mNumIndices = 0; + mIndices = 0; + } + bool mPolygons; // true if indices represents polygons, false indices are triangles + unsigned int mNumOutputVertices; // number of vertices in the output hull + float *mOutputVertices; // array of vertices, 3 floats each x,y,z + unsigned int mNumFaces; // the number of faces produced + unsigned int mNumIndices; // the total number of indices + unsigned int *mIndices; // pointer to indices. + +// If triangles, then indices are array indexes into the vertex list. +// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. +}; + +enum HullFlag +{ + QF_TRIANGLES = (1<<0), // report results as triangles, not polygons. + QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices. + QF_SKIN_WIDTH = (1<<2), // extrude hull based on this skin width + QF_DEFAULT = 0 +}; + + +class HullDesc +{ +public: + HullDesc(void) + { + mFlags = QF_DEFAULT; + mVcount = 0; + mVertices = 0; + mVertexStride = sizeof(float)*3; + mNormalEpsilon = 0.001f; + mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. + mMaxFaces = 4096; + mSkinWidth = 0.01f; // default is one centimeter + }; + + HullDesc(HullFlag flag, + unsigned int vcount, + const float *vertices, + unsigned int stride) + { + mFlags = flag; + mVcount = vcount; + mVertices = vertices; + mVertexStride = stride; + mNormalEpsilon = 0.001f; + mMaxVertices = 4096; + mSkinWidth = 0.01f; // default is one centimeter + } + + bool HasHullFlag(HullFlag flag) const + { + if ( mFlags & flag ) return true; + return false; + } + + void SetHullFlag(HullFlag flag) + { + mFlags|=flag; + } + + void ClearHullFlag(HullFlag flag) + { + mFlags&=~flag; + } + + unsigned int mFlags; // flags to use when generating the convex hull. + unsigned int mVcount; // number of vertices in the input point cloud + const float *mVertices; // the array of vertices. + unsigned int mVertexStride; // the stride of each vertex, in bytes. + float mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. + float mSkinWidth; + unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! + unsigned int mMaxFaces; +}; + +enum HullError +{ + QE_OK, // success! + QE_FAIL // failed. +}; + +class STANHULL_EXPORT HullLibrary +{ +public: + + HullError CreateConvexHull(const HullDesc &desc, // describes the input request + HullResult &result); // contains the resulst + + HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it. + +private: + + void BringOutYourDead(const float *verts,unsigned int vcount, float *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount); + + bool 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); +}; + +} + +#endif // STANHULL_H