static bool notExist( ref btVector3 planeEquation, btList<btVector3> planeEquations )
{
int numbrushes = planeEquations.Count;
for( int i = 0; i < numbrushes; i++ )
{
btVector3 N1 = planeEquations[i];
if( planeEquation.dot( ref N1 ) > (double)( 0.999 ) )
{
return false;
}
}
return true;
}
public static void getPlaneEquationsFromVertices( btList<btVector3> vertices, btList<btVector3> planeEquationsOut )
{
int numvertices = vertices.Count;
// brute force:
for( int i = 0; i < numvertices; i++ )
{
btVector3 N1 = vertices[i];
for( int j = i + 1; j < numvertices; j++ )
{
btVector3 N2 = vertices[j];
for( int k = j + 1; k < numvertices; k++ )
{
btVector3 N3 = vertices[k];
btVector3 planeEquation, edge0, edge1;
N2.Sub( ref N1, out edge0 );
N3.Sub( ref N1, out edge1 );
double normalSign = (double)( 1.0 );
for( int ww = 0; ww < 2; ww++ )
{
btVector3 tmp;
edge0.cross( ref edge1, out tmp );
tmp.Mult( normalSign, out planeEquation );
if( planeEquation.length2() > (double)( 0.0001 ) )
{
planeEquation.normalize();
if( notExist( ref planeEquation, planeEquationsOut ) )
{
planeEquation[3] = -planeEquation.dot( ref N1 );
//check if inside, and replace supportingVertexOut if needed
if( areVerticesBehindPlane( ref planeEquation, vertices, (double)( 0.01 ) ) )
{
planeEquationsOut.Add( planeEquation );
}
}
}
normalSign = (double)( -1.0 );
}
}
}
}
}
public static bool isPointInsidePlanes( btList<btVector3> planeEquations
, ref btVector3 point, double margin )
{
btVector3[] pe = planeEquations.InternalArray;
int numbrushes = planeEquations.Count;
for( int i = 0; i < numbrushes; i++ )
{
double dist = ( pe[i].dot( ref point ) ) + (double)( pe[i][3] ) - margin;
if( dist > btScalar.BT_ZERO )
{
return false;
}
}
return true;
}
public static bool areVerticesBehindPlane( ref btVector3 planeNormal, btList<btVector3> vertices, double margin )
{
btVector3[] va = vertices.InternalArray;
int numvertices = vertices.Count;
for( int i = 0; i < numvertices; i++ )
{
//ref btVector3 N1 = vertices[i];
double dist = (double)( planeNormal.dot( ref va[i] ) ) + (double)( planeNormal[3] ) - margin;
if( dist > btScalar.BT_ZERO )
{
return false;
}
}
return true;
}
public static int Allocate( btList<int> ifree,
btList<sStkNPS> stock,
sStkNPS value )
{
//int i;
//if(ifree.Count>0)
//{
// i = ifree[ifree.Count - 1];
// ifree.pop_back();
// stock[i] = value;
//}
//else
//{
// i=stock.Count;
// stock.push_back(value);
//}
//return(i);
return 0;
}
public void OptimizeTopDown( int bu_threshold )
{
// threshhold defaults to 128
if( Root != null )
{
btList<btDbvtNode> leafs = new btList<btDbvtNode>( m_leaves );
FetchLeafs( this, Root, leafs );
Root = TopDown( this, leafs, bu_threshold );
}
}
public static btDbvtVolume FromPoints( btList<btList<btVector3>> points )
{
return new btDbvtVolume();
}
public static btDbvtVolume FromPoints( btList<btVector3> points )
{
btDbvtVolume box;
box._min = box._max = points[0];
for( int i = 1; i < points.Count; ++i )
{
btVector3 temp = points[i];
//SetMin(ref box._min, ref temp);
//SetMax(ref box._max, ref temp);
box._min.setMin( ref temp );
box._max.setMin( ref temp );
}
return ( box );
}
public void OptimizeBottomUp()
{
if( Root != null )
{
btList<btDbvtNode> leafs = new btList<btDbvtNode>( m_leaves );
FetchLeafs( this, Root, leafs );
BottomUp( this, leafs );
Root = leafs[0];
}
}
public ConvexH( int vertices_size, int edges_size, int facets_size )
{
vertices = new btList<btVector3>( vertices_size );
edges = new btList<HalfEdge>( edges_size );
facets = new btList<btPlane>( facets_size );
}
int calchull( btVector3[] verts, int verts_count, out TUIntArray tris_out, out int tris_count, int vlimit )
{
int rc = calchullgen( verts, verts_count, vlimit );
if( rc == 0 )
{
tris_out = null;
tris_count = 0;
return 0;
}
btList<int> ts = new btList<int>();
int i;
for( i = 0; i < m_tris.Count; i++ )
{
if( m_tris[i] != null )
{
for( int j = 0; j < 3; j++ )
ts.Add( ( m_tris[i].v)[j] );
deAllocateTriangle( m_tris[i] );
}
}
tris_count = ts.Count / 3;
tris_out = new TUIntArray( ts.Count );
for( i = 0; i < ts.Count; i++ )
{
tris_out[i] = (uint)( ts[i] );
}
m_tris.Count = ( 0 );
return 1;
}
public static void Split( btList<btDbvtNode> leaves, btList<btDbvtNode> left, btList<btDbvtNode> right, ref btVector3 org, ref btVector3 axis )
{
left.Count = ( 0 );
right.Count = ( 0 );
for( int i = 0, ni = leaves.Count; i < ni; ++i )
{
btVector3 tmp;
leaves[i].volume.Center().Sub( ref org, out tmp );
if( btVector3.dot( ref axis, ref tmp ) < 0 )
{
left.Add( leaves[i] );
}
else
{
right.Add( leaves[i] );
}
}
}
public static void getVerticesFromPlaneEquations( btList<btVector3> planeEquations, btList<btVector3> verticesOut )
{
int numbrushes = planeEquations.Count;
// brute force:
for( int i = 0; i < numbrushes; i++ )
{
btVector3 N1 = planeEquations[i];
for( int j = i + 1; j < numbrushes; j++ )
{
btVector3 N2 = planeEquations[j];
for( int k = j + 1; k < numbrushes; k++ )
{
btVector3 N3 = planeEquations[k];
btVector3 n2n3;
N2.cross( ref N3, out n2n3 );
btVector3 n3n1;
N3.cross( ref N1, out n3n1 );
btVector3 n1n2;
N1.cross( ref N2, out n1n2 );
if( ( n2n3.length2() > (double)( 0.0001 ) ) &&
( n3n1.length2() > (double)( 0.0001 ) ) &&
( n1n2.length2() > (double)( 0.0001 ) ) )
{
//point P out of 3 plane equations:
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
//P = -------------------------------------------------------------------------
// N1 . ( N2 * N3 )
double quotient = ( N1.dot( ref n2n3 ) );
if( btScalar.btFabs( quotient ) > (double)( 0.000001 ) )
{
quotient = (double)( -1.0 ) / quotient;
n2n3.Mult( N1[3], out n2n3 );
n3n1.Mult( N2[3], out n3n1 );
n1n2.Mult( N3[3], out n1n2 );
btVector3 potentialVertex = n2n3;
potentialVertex.Add( ref n3n1, out potentialVertex );
potentialVertex.Add( ref n1n2, out potentialVertex );
potentialVertex.Mult( quotient, out potentialVertex );
//check if inside, and replace supportingVertexOut if needed
if( isPointInsidePlanes( planeEquations, ref potentialVertex, (double)( 0.01 ) ) )
{
verticesOut.Add( potentialVertex );
}
}
}
}
}
}
}
static void MycollideTT( btDbvt.btDbvtNode root0,
btDbvt.btDbvtNode root1,
ref btTransform xform,
btCompoundCompoundLeafCallback callback )
{
if( root0 != null && root1 != null )
{
int depth = 1;
int treshold = btDbvt.DOUBLE_STACKSIZE - 4;
btList<btDbvt.sStkNN> stkStack = new btList<btDbvt.sStkNN>( btDbvt.DOUBLE_STACKSIZE );
stkStack[0].Initialize( root0, root1 );
do
{
btDbvt.sStkNN p = stkStack[--depth];
if( MyIntersect( p.a.volume, p.b.volume, ref xform ) )
{
if( depth > treshold )
{
stkStack.Capacity = ( stkStack.Count * 2 );
treshold = stkStack.Count - 4;
}
if( p.a.IsInternal() )
{
if( p.b.IsInternal() )
{
stkStack[depth++].Initialize( p.a._children0, p.b._children0 );
stkStack[depth++].Initialize( p.a._children1, p.b._children0 );
stkStack[depth++].Initialize( p.a._children0, p.b._children1 );
stkStack[depth++].Initialize( p.a._children1, p.b._children1 );
}
else
{
stkStack[depth++].Initialize( p.a._children0, p.b );
stkStack[depth++].Initialize( p.a._children1, p.b );
}
}
else
{
if( p.b.IsInternal() )
{
stkStack[depth++].Initialize( p.a, p.b._children0 );
stkStack[depth++].Initialize( p.a, p.b._children1 );
}
else
{
callback.Process( p.a, p.b );
}
}
}
} while( depth != 0 );
}
}
bool CleanupVertices( int svcount,
btVector3[] svertices,
int stride,
out int vcount, // output number of vertices
btList<btVector3> vertices, // location to store the results.
double normalepsilon,
ref btVector3 scale)
{
if( svcount == 0 ) {
vcount = 0;
return false;
}
m_vertexIndexMapping.Count =( 0 );
vcount = 0;
double[] recip = new double[3];
//if( scale )
{
scale[0] = 1;
scale[1] = 1;
scale[2] = 1;
}
double[] bmin = new double[3] { btScalar.BT_MAX_FLOAT, btScalar.BT_MAX_FLOAT, btScalar.BT_MAX_FLOAT };
double[] bmax = new double[3] { btScalar.BT_MIN_FLOAT, btScalar.BT_MIN_FLOAT, btScalar.BT_MIN_FLOAT };
//char vtx = (char)svertices;
// if ( 1 )
{
for( uint i = 0; i < svcount; i++ )
{
for( int j = 0; j < 3; j++ )
{
if( svertices[i][j] < bmin[j] ) bmin[j] = svertices[i][j];
if( svertices[i][j] > bmax[j] ) bmax[j] = svertices[i][j];
}
}
}
double dx = bmax[0] - bmin[0];
double dy = bmax[1] - bmin[1];
double dz = bmax[2] - bmin[2];
btVector3 center = new btVector3( dx * (double)( 0.5 ) + bmin[0],
dy * (double)( 0.5 ) + bmin[1],
dz * (double)( 0.5 ) + bmin[2] );
if( dx < btScalar.SIMD_EPSILON || dy < btScalar.SIMD_EPSILON || dz < btScalar.SIMD_EPSILON || svcount < 3 )
{
double len = btScalar.BT_MAX_FLOAT;
if( dx > btScalar.SIMD_EPSILON & dx < len ) len = dx;
if( dy > btScalar.SIMD_EPSILON && dy < len ) len = dy;
if( dz > btScalar.SIMD_EPSILON && dz < len ) len = dz;
if( len == btScalar.BT_MAX_FLOAT )
{
dx = dy = dz = (double)( 0.01 ); // one centimeter
}
else
{
if( dx < btScalar.SIMD_EPSILON ) dx = len * (double)( 0.05 ); // 1/5th the shortest non-zero edge.
if( dy < btScalar.SIMD_EPSILON ) dy = len * (double)( 0.05 );
if( dz < btScalar.SIMD_EPSILON ) dz = len * (double)( 0.05 );
}
double x1 = center.x - dx;
double x2 = center.x + dx;
double y1 = center.y - dy;
double y2 = center.y + dy;
double z1 = center.z - dz;
double z2 = center.z + dz;
addPoint( ref vcount, vertices, x1, y1, z1 );
addPoint( ref vcount, vertices, x2, y1, z1 );
addPoint( ref vcount, vertices, x2, y2, z1 );
addPoint( ref vcount, vertices, x1, y2, z1 );
addPoint( ref vcount, vertices, x1, y1, z2 );
addPoint( ref vcount, vertices, x2, y1, z2 );
addPoint( ref vcount, vertices, x2, y2, z2 );
addPoint( ref 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.x = recip[0];
center.y = recip[1];
center.z = recip[2];
}
}
//vtx = (stringchar)svertices;
for( uint i = 0; i < svcount; i++ )
{
//btVector3 p = (btVector3)vtx;
//vtx += stride;
double px = svertices[i].x;
double py = svertices[i].y;
double pz = svertices[i].z;
//if( scale )
{
px = px * recip[0]; // normalize
py = py * recip[1]; // normalize
pz = pz * recip[2]; // normalize
}
// if ( 1 )
{
int j;
btVector3[] v = vertices.InternalArray;
for( j = 0; j < vcount; j++ )
{
/// XXX might be broken
double x = v[j][0];
double y = v[j][1];
double z = v[j][2];
dx = btScalar.btFabs( x - px );
dy = btScalar.btFabs( y - py );
dz = btScalar.btFabs( z - pz );
if( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
{
// ok, it is close enough to the old one
// now let us see if it is further from the center of the point cloud than the one we already recorded.
// in which case we keep this one instead.
double dist1 = GetDist( px, py, pz, ref center );
double dist2 = GetDist( v[j][0], v[j][1], v[j][2], ref center );
if( dist1 > dist2 )
{
v[j][0] = px;
v[j][1] = py;
v[j][2] = pz;
}
break;
}
}
if( j == vcount )
{
vertices.InternalArray[vcount][0] = px;
vertices.InternalArray[vcount][1] = py;
vertices.InternalArray[vcount][2] = pz;
vcount++;
}
m_vertexIndexMapping.Add( j );
}
}
// ok..now make sure we didn't prune so many vertices it is now invalid.
// if ( 1 )
{
/* reset min max */
bmin = new double[] { btScalar.BT_MAX_FLOAT, btScalar.BT_MAX_FLOAT, btScalar.BT_MAX_FLOAT };
bmax = new double[] { btScalar.BT_MIN_FLOAT, btScalar.BT_MIN_FLOAT, btScalar.BT_MIN_FLOAT };
for( int i = 0; i < vcount; i++ )
{
btVector3 p = vertices[i];
for( int j = 0; j < 3; j++ )
{
if( p[j] < bmin[j] ) bmin[j] = p[j];
if( p[j] > bmax[j] ) bmax[j] = p[j];
}
}
dx = bmax[0] - bmin[0];
dy = bmax[1] - bmin[1];
dz = bmax[2] - bmin[2];
if( dx < btScalar.SIMD_EPSILON || dy < btScalar.SIMD_EPSILON || dz < btScalar.SIMD_EPSILON || vcount < 3 )
{
double cx = dx * 0.5 + bmin[0];
double cy = dy * 0.5 + bmin[1];
double cz = dz * 0.5 + bmin[2];
double len = btScalar.BT_MAX_FLOAT;
if( dx >= btScalar.SIMD_EPSILON && dx < len ) len = dx;
if( dy >= btScalar.SIMD_EPSILON && dy < len ) len = dy;
if( dz >= btScalar.SIMD_EPSILON && dz < len ) len = dz;
if( len == btScalar.BT_MAX_FLOAT )
{
dx = dy = dz = (double)( 0.01 ); // one centimeter
}
else
{
if( dx < btScalar.SIMD_EPSILON ) dx = len * (double)( 0.05 ); // 1/5th the shortest non-zero edge.
if( dy < btScalar.SIMD_EPSILON ) dy = len * (double)( 0.05 );
if( dz < btScalar.SIMD_EPSILON ) dz = len * (double)( 0.05 );
}
double x1 = cx - dx;
double x2 = cx + dx;
double y1 = cy - dy;
double y2 = cy + dy;
double z1 = cz - dz;
double z2 = cz + dz;
vcount = 0; // add box
addPoint( ref vcount, vertices, x1, y1, z1 );
addPoint( ref vcount, vertices, x2, y1, z1 );
addPoint( ref vcount, vertices, x2, y2, z1 );
addPoint( ref vcount, vertices, x1, y2, z1 );
addPoint( ref vcount, vertices, x1, y1, z2 );
addPoint( ref vcount, vertices, x2, y1, z2 );
addPoint( ref vcount, vertices, x2, y2, z2 );
addPoint( ref vcount, vertices, x1, y2, z2 );
return true;
}
}
return true;
}
static void addPoint( ref int vcount, btList<btVector3> p, double x, double y, double z )
{
// XXX, might be broken
p.InternalArray[vcount].x = x;
p.InternalArray[vcount].y = y;
p.InternalArray[vcount].z = z;
vcount++;
}
//*****************************
//*****************************
//*[]* HullLib header
//*****************************
//*****************************
//*****************************
//*****************************
//*[]* HullLib implementation
//*****************************
//*****************************
HullError CreateConvexHull( HullDesc desc, // describes the input request
HullResult result) // contains the resulst
{
HullError ret = HullError.QE_FAIL;
PHullResult hr = new PHullResult();
int vcount = desc.mVcount;
if( vcount < 8 ) vcount = 8;
btList<btVector3> vertexSource = new btList<btVector3>();
vertexSource.Count = vertexSource.Capacity = ( (int)vcount );
btVector3 scale = btVector3.Zero;
int ovcount;
bool ok = CleanupVertices( desc.mVcount, desc.mVertices.InternalArray, desc.mVertexStride
, out ovcount, vertexSource, desc.mNormalEpsilon, ref scale ); // normalize point cloud, remove duplicates!
if( ok )
{
// if ( 1 ) // scale vertices back to their original size.
{
for( uint i = 0; i < ovcount; i++ )
{
btVector3 v = vertexSource[(int)( i )];
v[0] = scale[0];
v[1] = scale[1];
v[2] = scale[2];
}
}
ok = ComputeHull( ovcount, vertexSource.InternalArray, hr, desc.mMaxVertices );
if( ok )
{
// re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
btList<btVector3> vertexScratch = new btList<btVector3>( (int)hr.mVcount );
BringOutYourDead( hr.mVertices, hr.mVcount, vertexScratch, out ovcount, hr.m_Indices, hr.mIndexCount );
ret = HullError.QE_OK;
if( desc.HasHullFlag( HullFlag.QF_TRIANGLES ) ) // if he wants the results as triangle!
{
result.mPolygons = false;
result.mNumOutputVertices = ovcount;
result.m_OutputVertices.Count = result.m_OutputVertices.Capacity = ovcount;
result.mNumFaces = hr.mFaceCount;
result.mNumIndices = hr.mIndexCount;
result.m_Indices.Count = result.m_Indices.Capacity = hr.mIndexCount;
for( int j = 0; j < ovcount; j++ )
result.m_OutputVertices[j] = vertexScratch[j];
//memcpy( result.m_OutputVertices, vertexScratch, sizeof( btVector3 ) * ovcount );
uint[] source = hr.m_Indices.InternalArray;
uint[] dest = result.m_Indices.InternalArray;
if( desc.HasHullFlag( HullFlag.QF_REVERSE_ORDER ) )
{
for( uint i = 0; i < hr.mFaceCount; i++ )
{
//dest = source;
dest[i*3+0] = source[i*3+2];
dest[i * 3 + 1] = source[i * 3 + 1];
dest[i * 3 + 2] = source[i * 3 + 0];
//dest += 3;
//source += 3;
}
}
else
{
for( int i = 0; i < 3*hr.mIndexCount; i++ )
dest[i] = source[i];
//memcpy( result.m_Indices, hr.m_Indices, sizeof( uint ) * hr.mIndexCount );
}
}
else
{
result.mPolygons = true;
result.mNumOutputVertices = ovcount;
result.m_OutputVertices.Count = result.m_OutputVertices.Capacity = ovcount;
result.mNumFaces = hr.mFaceCount;
result.mNumIndices = hr.mIndexCount + hr.mFaceCount;
result.m_Indices.Count = result.m_Indices.Capacity = ( result.mNumIndices );
{
btVector3[] dest = result.m_OutputVertices.InternalArray;
btVector3[] source = vertexScratch.InternalArray;
for( int i = 0; i < 3 * hr.mIndexCount; i++ )
dest[i] = source[i];
}
//memcpy( result.m_OutputVertices, vertexScratch, sizeof( btVector3 ) * ovcount );
// if ( 1 )
{
uint[] source = hr.m_Indices.InternalArray;
uint[] dest = result.m_Indices.InternalArray;
for( uint i = 0; i < hr.mFaceCount; i++ )
{
dest[i*4 + 0] = 3;
if( desc.HasHullFlag( HullFlag.QF_REVERSE_ORDER ) )
{
dest[i * 4 + 1] = source[i * 3 + 2];
dest[i * 4 + 2] = source[i * 3 + 1];
dest[i * 4 + 3] = source[i * 3 + 0];
}
else
{
dest[i * 4 + 1] = source[i * 3 + 0];
dest[i * 4 + 2] = source[i * 3 + 1];
dest[i * 4 + 3] = source[i*3+2];
}
//dest += 4;
//source += 3;
}
}
}
ReleaseHull( hr );
}
}
return ret;
}
//
// depth is defaulted to -1
public static void FetchLeafs( btDbvt pdbvt, btDbvtNode root, btList<btDbvtNode> leafs )
{
FetchLeafs( pdbvt, root, leafs, -1 );
}
public static void FetchLeafs( btDbvt pdbvt, btDbvtNode root, btList<btDbvtNode> leafs, int depth )
{
if( root.IsInternal() && depth != 0 )
{
FetchLeafs( pdbvt, root._children0, leafs, depth - 1 );
FetchLeafs( pdbvt, root._children1, leafs, depth - 1 );
DeleteNode( pdbvt, root );
}
else
{
leafs.Add( root );
}
}
//
public static void Bounds( btList<btDbvtNode> leafs, out btDbvtVolume volume )
{
volume = leafs[0].volume;
foreach( btDbvtNode node in leafs )
{
btDbvtVolume.Merge( ref volume, ref node.volume, out volume );
}
}
public static btDbvtNode TopDown( btDbvt pdbvt, btList<btDbvtNode> leaves, int bu_treshold )
{
if( leaves.Count > 1 )
{
if( leaves.Count > bu_treshold )
{
btDbvtVolume vol; Bounds( leaves, out vol );
btVector3 org = vol.Center();
btList<btDbvtNode> sets0 = new btList<btDbvtNode>();
btList<btDbvtNode> sets1 = new btList<btDbvtNode>();
int bestaxis = -1;
int bestmidp = leaves.Count;
int[] a1 = new int[] { 0, 0 };
int[] a2 = new int[] { 0, 0 };
int[] a3 = new int[] { 0, 0 };
int[][] splitcount = new int[][] { a1, a2, a3 };
int i;
for( i = 0; i < leaves.Count; ++i )
{
btVector3 x = leaves[i].volume.Center() - org;
for( int j = 0; j < 3; ++j )
{
++splitcount[j][btVector3.dot( ref x, ref axis[j] ) > 0 ? 1 : 0];
}
}
for( i = 0; i < 3; ++i )
{
if( ( splitcount[i][0] > 0 ) && ( splitcount[i][1] > 0 ) )
{
int midp = (int)Math.Abs( ( splitcount[i][0] - splitcount[i][1] ) );
if( midp < bestmidp )
{
bestaxis = i;
bestmidp = midp;
}
}
}
if( bestaxis >= 0 )
{
sets0.Capacity = ( splitcount[bestaxis][0] );
sets1.Capacity = ( splitcount[bestaxis][1] );
Split( leaves, sets0, sets1, ref org, ref axis[bestaxis] );
}
else
{
sets0.Capacity = ( leaves.Count / 2 + 1 );
sets1.Capacity = ( leaves.Count / 2 );
for( int i2 = 0, ni = leaves.Count; i2 < ni; ++i2 )
{
if( (i2 & 1)== 0 )
sets0.Add( leaves[i2] );
else
sets1.Add( leaves[i2] );
}
}
btDbvtNode node = CreateNode( pdbvt, null, ref vol, null );
node._children0 = TopDown( pdbvt, sets0, bu_treshold );
node._children1 = TopDown( pdbvt, sets1, bu_treshold );
node._children0.parent = node;
node._children1.parent = node;
return ( node );
}
else
{
BottomUp( pdbvt, leaves );
return ( leaves[0] );
}
}
return ( leaves[0] );
}
int calchullgen( btVector3[] verts, int verts_count, int vlimit )
{
if( verts_count < 4 ) return 0;
if( vlimit == 0 ) vlimit = 1000000000;
int j;
btVector3 bmin = verts[0], bmax= verts[0];
btList<int> isextreme = new btList<int>( verts_count );
btList<int> allow = new btList<int>( verts_count );
for( j = 0; j < verts_count; j++ )
{
allow.Add( 1 );
isextreme.Add( 0 );
bmin.setMin( ref verts[j] );
bmax.setMax( ref verts[j] );
}
btVector3 del;
bmax.Sub( ref bmin, out del );
double epsilon = ( del ).length() * 0.001;
Debug.Assert( epsilon != 0.0 );
int4 p = FindSimplex( verts, verts_count, allow.InternalArray );
if( p.x == -1 ) return 0; // simplex failed
btVector3 tmp;
btVector3 center; //= ( verts[p[0]] + verts[p[1]] + verts[p[2]] + verts[p[3]] ) / 4.0; // a valid interior point
verts[p[0]].Add( ref verts[p[1]], out tmp );
tmp.Add( ref verts[p[2]], out tmp );
tmp.Add( ref verts[p[3]], out tmp );
tmp.Div( 4, out center );
btHullTriangle t0 = allocateTriangle( p[2], p[3], p[1] ); t0.n = new int3( 2, 3, 1 );
btHullTriangle t1 = allocateTriangle( p[3], p[2], p[0] ); t1.n = new int3( 3, 2, 0 );
btHullTriangle t2 = allocateTriangle( p[0], p[1], p[3] ); t2.n = new int3( 0, 1, 3 );
btHullTriangle t3 = allocateTriangle( p[1], p[0], p[2] ); t3.n = new 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; j < m_tris.Count; j++ )
{
btHullTriangle t = m_tris[j];
Debug.Assert( t != null );
Debug.Assert( t.vmax < 0 );
btVector3 n; btPlane.TriNormal( ref verts[ t.v[0]], ref verts[ t.v[1]], ref verts[ t.v[2]], out n );
t.vmax = maxdirsterid( verts, verts_count, ref n, allow.InternalArray );
btVector3 tmp2;
verts[t.vmax].Sub( ref verts[( t ).v[0]], out tmp2 );
t.rise = btVector3.btDot( ref n, ref tmp );
}
btHullTriangle te;
vlimit -= 4;
while( vlimit > 0 && ( ( te = extrudable( epsilon ) ) != null ) )
{
//int3 ti=te;
int v = te.vmax;
Debug.Assert( v != -1 );
Debug.Assert( isextreme[v] == 0 ); // wtf we've already done this vertex
isextreme[v] = 1;
//if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
j = m_tris.Count;
while( j-- != 0 )
{
if( m_tris[j] == null ) continue;
int3 t = m_tris[j].v;
if( above( verts, t, ref verts[v], 0.01 * epsilon ) )
{
extrude( m_tris[j], v );
}
}
// now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
j = m_tris.Count;
while( j-- != 0 )
{
if( m_tris[j] == null ) continue;
if( !hasvert( m_tris[j].v, v ) ) break;
int3 nt = m_tris[j].v;
btVector3 tmp2;
btVector3 tmp3;
verts[nt[1]].Sub( ref verts[nt[0]], out tmp );
verts[nt[2]].Sub( ref verts[nt[1]], out tmp2 );
btVector3.btCross( ref tmp, ref tmp2, out tmp3 );
if( above( verts, nt, ref center, 0.01 * epsilon ) ||
tmp3.length() < epsilon * epsilon * 0.1 )
{
btHullTriangle nb = m_tris[m_tris[j].n[0]];
Debug.Assert( nb != null ); Debug.Assert( !hasvert( nb.v, v ) ); Debug.Assert( nb.id < j );
extrude( nb, v );
j = m_tris.Count;
}
}
j = m_tris.Count;
while( j-- != 0 )
{
btHullTriangle t = m_tris[j];
if( t == null ) continue;
if( t.vmax >= 0 ) break;
btVector3 n; btPlane.TriNormal( ref verts[( t ).v[0]], ref verts[( t ).v[1]], ref verts[( t ).v[2]], out n );
t.vmax = maxdirsterid( verts, verts_count, ref n, allow.InternalArray );
if( isextreme[t.vmax] != 0 )
{
t.vmax = -1; // already done that vertex - algorithm needs to be able to terminate.
}
else
{
verts[t.vmax].Sub( ref verts[( t ).v[0]], out tmp );
t.rise = btVector3.btDot( ref n, ref tmp );
}
}
vlimit--;
}
return 1;
}
//
public static void BottomUp( btDbvt pdbvt, btList<btDbvtNode> leaves )
{
while( leaves.Count > 1 )
{
double minsize = double.MaxValue;
int minidx0 = -1;
int minidx1 = -1;
for( int i = 0; i < leaves.Count; ++i )
{
for( int j = i + 1; j < leaves.Count; ++j )
{
btDbvtVolume mergeResults = btDbvtVolume.Merge( ref leaves[i].volume, ref leaves[j].volume );
double sz = Size( ref mergeResults );
if( sz < minsize )
{
minsize = sz;
minidx0 = i;
minidx1 = j;
}
}
}
btDbvtNode n0 = leaves[minidx0];
btDbvtNode n1 = leaves[minidx1];
btDbvtNode p = CreateNode( pdbvt, null, ref n0.volume, ref n1.volume, null );
p._children0 = n0;
p._children1 = n1;
n0.parent = p;
n1.parent = p;
leaves[minidx0] = p;
leaves.Swap( minidx1, leaves.Count - 1 );
leaves.Count--;// PopBack();
}
}
void gimpact_vs_shape_find_pairs( btTransform & trans0, btTransform & trans1, btGImpactShapeInterface * shape0, btCollisionShape * shape1, btList<int> & collided_primitives);
///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges)
///experimental/work-in-progress
public virtual bool initializePolyhedralFeatures( bool shiftVerticesByMargin = false )
{
if( m_polyhedron != null )
{
m_polyhedron = null;
}
m_polyhedron = new btConvexPolyhedron();
int count = getNumVertices();
btList<btVector3> orgVertices = new btList<btVector3>( count );
btVector3[] arrVertices = orgVertices.InternalArray;
for( int i = 0; i < count; i++ )
{
getVertex( i, out arrVertices[i] );
}
btConvexHullComputer conv = new btConvexHullComputer();
if( shiftVerticesByMargin )
{
btList<btVector3> planeEquations = new btList<btVector3>();
btGeometryUtil.getPlaneEquationsFromVertices( orgVertices, planeEquations );
btList<btVector3> shiftedPlaneEquations = new btList<btVector3>();
for( int p = 0; p < planeEquations.Count; p++ )
{
btVector3 plane = planeEquations[p];
// double margin = getMargin();
plane[3] -= getMargin();
shiftedPlaneEquations.Add( plane );
}
btList<btVector3> tmpVertices = new btList<btVector3>();
btGeometryUtil.getVerticesFromPlaneEquations( shiftedPlaneEquations, tmpVertices );
conv.compute( tmpVertices, tmpVertices.Count, 0, 0 );
}
else
{
conv.compute( orgVertices, orgVertices.Count, 0, 0 );
}
btList<btVector3> faceNormals = new btList<btVector3>( conv.faces.Count );
int numFaces = conv.faces.Count;
btConvexHullComputer convexUtil = conv;
btVector3[] arr_faceNormals = faceNormals.InternalArray;
btList<btConvexPolyhedron.btFace> tmpFaces = new btList<btConvexPolyhedron.btFace>( numFaces );
int numVertices = convexUtil.vertices.Count;
m_polyhedron.m_vertices.Count = m_polyhedron.m_vertices.Capacity = ( numVertices );
for( int p = 0; p < numVertices; p++ )
{
m_polyhedron.m_vertices[p] = convexUtil.vertices[p];
}
for( int i = 0; i < numFaces; i++ )
{
btConvexHullComputer.Edge face = convexUtil.faces[i];
//Console.WriteLine("face=%d\n",face);
//btConvexHullComputer::Edge* firstEdge = &convexUtil.edges[face];
btConvexHullComputer.Edge edge = face;
btVector3[] edges = new btVector3[3];
int numEdges = 0;
//compute face normals
do
{
int src = edge.getSourceVertex();
tmpFaces[i].m_indices.Add( (short)src );
int targ = edge.getTargetVertex();
btVector3 wa = convexUtil.vertices[src];
btVector3 wb = convexUtil.vertices[targ];
btVector3 newEdge; wb.Sub( ref wa, out newEdge );
newEdge.normalize();
if( numEdges < 2 )
edges[numEdges++] = newEdge;
edge = edge.getNextEdgeOfFace();
} while( edge != face );
double planeEq = btScalar.BT_LARGE_FLOAT;
if( numEdges == 2 )
{
//faceNormals[i]
edges[0].cross( ref edges[1], out faceNormals.InternalArray[i] );
faceNormals[i].normalize();
tmpFaces[i].m_plane[0] = faceNormals[i].x;
tmpFaces[i].m_plane[1] = faceNormals[i].y;
tmpFaces[i].m_plane[2] = faceNormals[i].z;
tmpFaces[i].m_plane[3] = planeEq;
}
else
{
Debug.Assert( false );//degenerate?
faceNormals[i].setZero();
}
for( int v = 0; v < tmpFaces[i].m_indices.Count; v++ )
{
double eq = m_polyhedron.m_vertices[tmpFaces[i].m_indices[v]].dot( ref arr_faceNormals[i] );
if( planeEq > eq )
{
planeEq = eq;
}
}
tmpFaces[i].m_plane[3] = -planeEq;
}
//merge coplanar faces and copy them to m_polyhedron
double faceWeldThreshold = 0.999f;
btList<int> todoFaces = new btList<int>();
for( int i = 0; i < tmpFaces.Count; i++ )
todoFaces.Add( i );
btList<int> coplanarFaceGroup = new btList<int>();
while( todoFaces.Count > 0 )
{
int refFace = todoFaces[todoFaces.Count - 1];
coplanarFaceGroup.Add( refFace );
btConvexPolyhedron.btFace faceA = tmpFaces[refFace];
todoFaces.Count--;
btVector3 faceNormalA = new btVector3( faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2] );
for( int j = todoFaces.Count - 1; j >= 0; j-- )
{
int i = todoFaces[j];
btConvexPolyhedron.btFace faceB = tmpFaces[i];
btVector3 faceNormalB = new btVector3( faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2] );
if( faceNormalA.dot( ref faceNormalB ) > faceWeldThreshold )
{
coplanarFaceGroup.Add( i );
todoFaces.RemoveAt( i );
}
}
bool did_merge = false;
if( coplanarFaceGroup.Count > 1 )
{
//do the merge: use Graham Scan 2d convex hull
btList<GrahamVector3> orgpoints = new btList<GrahamVector3>();
btVector3 averageFaceNormal = btVector3.Zero;
for( int i = 0; i < coplanarFaceGroup.Count; i++ )
{
// m_polyhedron.m_faces.Add(tmpFaces[coplanarFaceGroup[i]]);
btConvexPolyhedron.btFace face = tmpFaces[coplanarFaceGroup[i]];
btVector3 faceNormal = new btVector3( face.m_plane[0], face.m_plane[1], face.m_plane[2] );
averageFaceNormal.Add( ref faceNormal, out averageFaceNormal );
for( int f = 0; f < face.m_indices.Count; f++ )
{
int orgIndex = face.m_indices[f];
btVector3 pt = m_polyhedron.m_vertices[orgIndex];
bool found = false;
for( int j = 0; j < orgpoints.Count; j++ )
{
//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
if( orgpoints[j].m_orgIndex == orgIndex )
{
found = true;
break;
}
}
if( !found )
orgpoints.Add( new GrahamVector3( ref pt, orgIndex ) );
}
}
btConvexPolyhedron.btFace combinedFace = new btConvexPolyhedron.btFace();
for( int i = 0; i < 4; i++ )
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
btList<GrahamVector3> hull = new btList<GrahamVector3>();
averageFaceNormal.normalize();
GrahamVector3.GrahamScanConvexHull2D( orgpoints, hull, ref averageFaceNormal );
for( int i = 0; i < hull.Count; i++ )
{
combinedFace.m_indices.Add( hull[i].m_orgIndex );
for( int k = 0; k < orgpoints.Count; k++ )
{
if( orgpoints[k].m_orgIndex == hull[i].m_orgIndex )
{
orgpoints[k].m_orgIndex = -1; // invalidate...
break;
}
}
}
// are there rejected vertices?
bool reject_merge = false;
for( int i = 0; i < orgpoints.Count; i++ )
{
if( orgpoints[i].m_orgIndex == -1 )
continue; // this is in the hull...
// this vertex is rejected -- is anybody else using this vertex?
for( int j = 0; j < tmpFaces.Count; j++ )
{
btConvexPolyhedron.btFace face = tmpFaces[j];
// is this a face of the current coplanar group?
bool is_in_current_group = false;
for( int k = 0; k < coplanarFaceGroup.Count; k++ )
{
if( coplanarFaceGroup[k] == j )
{
is_in_current_group = true;
break;
}
}
if( is_in_current_group ) // ignore this face...
continue;
// does this face use this rejected vertex?
for( int v = 0; v < face.m_indices.Count; v++ )
{
if( face.m_indices[v] == orgpoints[i].m_orgIndex )
{
// this rejected vertex is used in another face -- reject merge
reject_merge = true;
break;
}
}
if( reject_merge )
break;
}
if( reject_merge )
break;
}
if( !reject_merge )
{
// do this merge!
did_merge = true;
m_polyhedron.m_faces.Add( combinedFace );
}
}
if( !did_merge )
{
for( int i = 0; i < coplanarFaceGroup.Count; i++ )
{
btConvexPolyhedron.btFace face = tmpFaces[coplanarFaceGroup[i]];
m_polyhedron.m_faces.Add( face );
}
}
}
m_polyhedron.initialize();
return true;
}
void BringOutYourDead( btList<btVector3> verts, int vcount, btList<btVector3> overts, out int ocount, TUIntArray indices, int indexcount )
{
btList<int> tmpIndices = new btList<int>( m_vertexIndexMapping.Count );
int i;
for( i = 0; i < m_vertexIndexMapping.Count; i++ )
{
tmpIndices[i] = m_vertexIndexMapping[i];
}
TUIntArray usedIndices = new TUIntArray( (int)vcount );
//usedIndices.Capacity( (int)vcount ) );
//memset( usedIndices, 0, sizeof( uint ) * vcount );
ocount = 0;
for( i = 0; i < (int)indexcount; i++ )
{
int v = (int)indices[i]; // original array index
Debug.Assert( v >= 0 && v < vcount );
if( usedIndices[(int) v] != 0 ) // if already remapped
{
indices[i] = usedIndices[v] - 1; // index to new array
}
else
{
indices[i] = (uint)ocount; // new index mapping
overts[ocount] = verts[v]; // copy old vert to new vert array
for( int k = 0; k < m_vertexIndexMapping.Count; k++ )
{
if( tmpIndices[k] == v )
m_vertexIndexMapping[k] = ocount;
}
ocount++; // increment output vert count
Debug.Assert( ocount >= 0 && ocount <= vcount );
usedIndices[v] = (uint)ocount; // assign new index remapping
}
}
}
