bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
{
if (m_polyhedron)
{
m_polyhedron->~btConvexPolyhedron();
btAlignedFree(m_polyhedron);
}
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
m_polyhedron = new (mem) btConvexPolyhedron;
btAlignedObjectArray<btVector3> orgVertices;
for (int i=0;i<getNumVertices();i++)
{
btVector3& newVertex = orgVertices.expand();
getVertex(i,newVertex);
}
btConvexHullComputer conv;
if (shiftVerticesByMargin)
{
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
{
btVector3 plane = planeEquations[p];
// btScalar margin = getMargin();
plane[3] -= getMargin();
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> tmpVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
} else
{
conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
}
#ifndef BT_RECONSTRUCT_FACES
int numVertices = conv.vertices.size();
m_polyhedron->m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
{
m_polyhedron->m_vertices[p] = conv.vertices[p];
}
int v0, v1;
for (int j = 0; j < conv.faces.size(); j++)
{
btVector3 edges[3];
int numEdges = 0;
btFace combinedFace;
const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]];
v0 = edge->getSourceVertex();
int prevVertex=v0;
combinedFace.m_indices.push_back(v0);
v1 = edge->getTargetVertex();
while (v1 != v0)
{
btVector3 wa = conv.vertices[prevVertex];
btVector3 wb = conv.vertices[v1];
btVector3 newEdge = wb-wa;
newEdge.normalize();
if (numEdges<2)
edges[numEdges++] = newEdge;
//face->addIndex(v1);
combinedFace.m_indices.push_back(v1);
edge = edge->getNextEdgeOfFace();
prevVertex = v1;
int v01 = edge->getSourceVertex();
v1 = edge->getTargetVertex();
}
btAssert(combinedFace.m_indices.size() > 2);
btVector3 faceNormal = edges[0].cross(edges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFace.m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFace.m_plane[0] = faceNormal.getX();
combinedFace.m_plane[1] = faceNormal.getY();
combinedFace.m_plane[2] = faceNormal.getZ();
combinedFace.m_plane[3] = -planeEq;
m_polyhedron->m_faces.push_back(combinedFace);
}
#else//BT_RECONSTRUCT_FACES
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
faceNormals.resize(numFaces);
btConvexHullComputer* convexUtil = &conv;
btAlignedObjectArray<btFace> tmpFaces;
tmpFaces.resize(numFaces);
int numVertices = convexUtil->vertices.size();
m_polyhedron->m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
{
m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
}
for (int i=0;i<numFaces;i++)
{
int face = convexUtil->faces[i];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const btConvexHullComputer::Edge* edge = firstEdge;
btVector3 edges[3];
int numEdges = 0;
//compute face normals
do
{
int src = edge->getSourceVertex();
tmpFaces[i].m_indices.push_back(src);
int targ = edge->getTargetVertex();
btVector3 wa = convexUtil->vertices[src];
btVector3 wb = convexUtil->vertices[targ];
btVector3 newEdge = wb-wa;
newEdge.normalize();
if (numEdges<2)
edges[numEdges++] = newEdge;
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
btScalar planeEq = 1e30f;
if (numEdges==2)
{
faceNormals[i] = edges[0].cross(edges[1]);
faceNormals[i].normalize();
tmpFaces[i].m_plane[0] = faceNormals[i].getX();
tmpFaces[i].m_plane[1] = faceNormals[i].getY();
tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
tmpFaces[i].m_plane[3] = planeEq;
}
else
{
btAssert(0);//degenerate?
faceNormals[i].setZero();
}
for (int v=0;v<tmpFaces[i].m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
if (planeEq>eq)
{
planeEq=eq;
}
}
tmpFaces[i].m_plane[3] = -planeEq;
}
//merge coplanar faces and copy them to m_polyhedron
btScalar faceWeldThreshold= 0.999f;
btAlignedObjectArray<int> todoFaces;
for (int i=0;i<tmpFaces.size();i++)
todoFaces.push_back(i);
while (todoFaces.size())
{
btAlignedObjectArray<int> coplanarFaceGroup;
int refFace = todoFaces[todoFaces.size()-1];
coplanarFaceGroup.push_back(refFace);
btFace& faceA = tmpFaces[refFace];
todoFaces.pop_back();
btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
for (int j=todoFaces.size()-1;j>=0;j--)
{
int i = todoFaces[j];
btFace& faceB = tmpFaces[i];
btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
{
coplanarFaceGroup.push_back(i);
todoFaces.remove(i);
}
}
bool did_merge = false;
if (coplanarFaceGroup.size()>1)
{
//do the merge: use Graham Scan 2d convex hull
btAlignedObjectArray<GrahamVector3> orgpoints;
btVector3 averageFaceNormal(0,0,0);
for (int i=0;i<coplanarFaceGroup.size();i++)
{
// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
btFace& face = tmpFaces[coplanarFaceGroup[i]];
btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
averageFaceNormal+=faceNormal;
for (int f=0;f<face.m_indices.size();f++)
{
int orgIndex = face.m_indices[f];
btVector3 pt = m_polyhedron->m_vertices[orgIndex];
bool found = false;
for (int i=0;i<orgpoints.size();i++)
{
//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
if (orgpoints[i].m_orgIndex == orgIndex)
{
found=true;
break;
}
}
if (!found)
orgpoints.push_back(GrahamVector3(pt,orgIndex));
}
}
btFace combinedFace;
for (int i=0;i<4;i++)
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
btAlignedObjectArray<GrahamVector3> hull;
averageFaceNormal.normalize();
GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
for (int i=0;i<hull.size();i++)
{
combinedFace.m_indices.push_back(hull[i].m_orgIndex);
for(int k = 0; k < orgpoints.size(); 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.size(); 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.size(); j++) {
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.size(); 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.size(); 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.push_back(combinedFace);
}
}
if(!did_merge)
{
for (int i=0;i<coplanarFaceGroup.size();i++)
{
btFace face = tmpFaces[coplanarFaceGroup[i]];
m_polyhedron->m_faces.push_back(face);
}
}
}
#endif //BT_RECONSTRUCT_FACES
m_polyhedron->initialize();
return true;
}
bool westArcTaken(int y, int x) const {
VertexNumber v1 = getVertex(y, x, rot);
VertexNumber v2 = getVertex(y + 1, x, rot);
return arcTaken(getArc(v1, v2));
}
const Position& Polygon::operator [](const size_t index) const
{
return getVertex(index);
}
TPoint MNPolyLine::getStartPoint()
{
if(countOfVertexes() > 0)
return TPoint(getVertex(0)->getX(),getVertex(0)->getY());
return TPoint(0,0);
}
void cMesh::setGraph(int img_idx, RGraph &aGraph, vector <int> &aTriInGraph, vector <unsigned int> const &aVisTriIdx)
{
int id1, id2, pos1, pos2;
float E0, E1, E2;
//parcours des aretes du graphe d'adjacence
for (unsigned int aK=0; aK < mEdges.size(); aK++)
{
id1 = mEdges[aK].n1();
id2 = mEdges[aK].n2();
//on recherche id1 et id2 parmi les triangles visibles
if ((find(aVisTriIdx.begin(), aVisTriIdx.end(), id1)!=aVisTriIdx.end()) &&
(find(aVisTriIdx.begin(), aVisTriIdx.end(), id2)!=aVisTriIdx.end()))
{
//on ajoute seulement les triangles qui ne sont pas encore presents dans le graphe
if (find(aTriInGraph.begin(), aTriInGraph.end(), id1) == aTriInGraph.end())
{
aTriInGraph.push_back(id1);
aGraph.add_node();
}
if (find(aTriInGraph.begin(), aTriInGraph.end(), id2) == aTriInGraph.end())
{
aTriInGraph.push_back(id2);
aGraph.add_node();
}
}
}
cEdge elEdge;
//creation des aretes et calcul de leur energie
for (unsigned int aK=0; aK < mEdges.size(); aK++)
{
elEdge = mEdges[aK];
id1 = elEdge.n1();
id2 = elEdge.n2();
vector<int>::iterator it1 = find(aTriInGraph.begin(), aTriInGraph.end(), id1);
vector<int>::iterator it2 = find(aTriInGraph.begin(), aTriInGraph.end(), id2);
if ( (it1 != aTriInGraph.end()) && (it2 != aTriInGraph.end()) )
{
//pos1 = (int) (it1 - aTriInGraph.begin());
//pos2 = (int) (it2 - aTriInGraph.begin());
pos1 = (int) distance(aTriInGraph.begin(), it1);
pos2 = (int) distance(aTriInGraph.begin(), it2);
//energies de l'arete triangle-source et de l'arete triangle-puit
cTriangle *Tri1 = getTriangle(id1);
cTriangle *Tri2 = getTriangle(id2);
E1 = (float)Tri1->computeEnergy(img_idx);
E2 = (float)Tri2->computeEnergy(img_idx);
//if (E1 == 0.f)
// aGraph.add_tweights( pos1, 0.f, 1.f );
//else
//{
aGraph.add_tweights( pos1, (float)(mLambda*E1), 0.f );
//}
//if (E2 == 0.f)
// aGraph.add_tweights( pos2, 0.f, 1.f );
//else
//{
aGraph.add_tweights( pos2, (float)(mLambda*E2), 0.f );
//}
//energie de l'arete inter-triangles
//longueur^2 de l'arete coupee par elEdge
E0 = (float)square_euclid( getVertex( elEdge.v1() ), getVertex( elEdge.v2() ) );
aGraph.add_edge(pos1, pos2, E0, E0);
//aGraph.add_edge(pos1, pos2, 1, 1);
}
}
#ifdef oldoldold
for (unsigned int aK=0; aK < aTriInGraph.size(); aK++)
{
cTriangle *Tri = getTriangle(aTriInGraph[aK]);
E = Tri->computeEnergy(img_idx);
if (E == 0.f)
aGraph.add_tweights( aK, 1.f, 0.f );
else
{
aGraph.add_tweights( aK, mLambda*E, 0.f );
//aGraph.add_tweights( aK, 1.f, 0.f );
/*if (Tri->isInside())
aGraph.add_tweights( aK, 0.f, 1.f );
else
aGraph.add_tweights( aK, 1.f, 0.f );*/
}
}
#endif
}
u32 NFABuilderImpl::getAssertFlag(Position pos) {
NFAVertex v = getVertex(pos);
return (*graph)[v].assert_flags;
}
bool NFABuilderImpl::hasEdge(Position startPos, Position endPos) const {
return edge(getVertex(startPos), getVertex(endPos), *graph).second;
}
void NFABuilderImpl::addCharReach(Position pos, const CharReach &cr) {
NFAVertex v = getVertex(pos);
(*graph)[v].char_reach |= cr;
}
void NFABuilderImpl::setAssertFlag(Position pos, u32 flag) {
NFAVertex v = getVertex(pos);
(*graph)[v].assert_flags |= flag;
}
//-----------------------------------------------------------------------
void ConvexBody::extend(const Vector3& pt)
{
// Erase all polygons facing towards the point. For all edges that
// are not removed twice (once in AB and once BA direction) build a
// convex polygon (triangle) with the point.
Polygon::EdgeMap edgeMap;
for ( size_t i = 0; i < getPolygonCount(); ++i )
{
const Vector3& normal = getNormal( i );
// direction of the point in regard to the polygon
// the polygon is planar so we can take an arbitrary vertex
Vector3 ptDir = pt - getVertex( i, 0 );
ptDir.normalise();
// remove polygon if dot product is greater or equals null.
if ( normal.dotProduct( ptDir ) >= 0 )
{
// store edges (copy them because if the polygon is deleted
// its vertices are also deleted)
storeEdgesOfPolygon( i, &edgeMap );
// remove polygon
deletePolygon( i );
// decrement iterator because of deleted polygon
--i;
}
}
// point is already a part of the hull (point lies inside)
if ( edgeMap.empty() )
return;
// remove the edges that are twice in the list (once from each side: AB,BA)
Polygon::EdgeMap::iterator it;
// iterate from first to the element before the last one
for (Polygon::EdgeMap::iterator itStart = edgeMap.begin();
itStart != edgeMap.end(); )
{
// compare with iterator + 1 to end
// don't need to skip last entry in itStart since omitted in inner loop
it = itStart;
++it;
bool erased = false;
// iterate from itStart+1 to the element before the last one
for ( ; it != edgeMap.end(); ++it )
{
if (itStart->first.positionEquals(it->second) &&
itStart->second.positionEquals(it->first))
{
edgeMap.erase(it);
// increment itStart before deletion (iterator invalidation)
Polygon::EdgeMap::iterator delistart = itStart++;
edgeMap.erase(delistart);
erased = true;
break; // found and erased
}
}
// increment itStart if we didn't do it when erasing
if (!erased)
++itStart;
}
// use the remaining edges to build triangles with the point
// the vertices of the edges are in ccw order (edgePtA-edgePtB-point
// to form a ccw polygon)
while ( !edgeMap.empty() )
{
Polygon::EdgeMap::iterator mapIt = edgeMap.begin();
// build polygon it.first, it.second, point
Polygon *p = allocatePolygon();
p->insertVertex(mapIt->first);
p->insertVertex(mapIt->second);
p->insertVertex( pt );
// attach polygon to body
insertPolygon( p );
// erase the vertices from the list
// pointers are now held by the polygon
edgeMap.erase( mapIt );
}
}
//-----------------------------------------------------------------------
void ConvexBody::mergePolygons( void )
{
// Merge all polygons that lay in the same plane as one big polygon.
// A convex body does not have two separate regions (separated by polygons
// with different normals) where the same normal occurs, so we can simply
// search all similar normals of a polygon. Two different options are
// possible when the normals fit:
// - the two polygons are neighbors
// - the two polygons aren't neighbors (but a third, fourth,.. polygon lays
// in between)
// Signals if the body holds polygons which aren't neighbors but have the same
// normal. That means another step has to be processed.
bool bDirty = false;
for ( size_t iPolyA = 0; iPolyA < getPolygonCount(); ++iPolyA )
{
// ??
OgreAssert( iPolyA >= 0, "strange..." );
for ( size_t iPolyB = iPolyA+1; iPolyB < getPolygonCount(); ++iPolyB )
{
const Vector3& n1 = getNormal( iPolyA );
const Vector3& n2 = getNormal( iPolyB );
// if the normals point into the same direction
if ( n1.directionEquals( n2, Radian( Degree( 0.00001 ) ) ) )
{
// indicates if a neighbor has been found and joined
bool bFound = false;
// search the two fitting vertices (if there are any) for the common edge
const size_t numVerticesA = getVertexCount( iPolyA );
for ( size_t iVertexA = 0; iVertexA < numVerticesA; ++iVertexA )
{
const size_t numVerticesB = getVertexCount( iPolyB );
for ( size_t iVertexB = 0; iVertexB < numVerticesB; ++iVertexB )
{
const Vector3& aCurrent = getVertex( iPolyA, iVertexA );
const Vector3& aNext = getVertex( iPolyA, (iVertexA + 1) % getVertexCount( iPolyA ) );
const Vector3& bCurrent = getVertex( iPolyB, iVertexB );
const Vector3& bNext = getVertex( iPolyB, (iVertexB + 1) % getVertexCount( iPolyB ) );
// if the edge is the same the current vertex of A has to be equal to the next of B and the other
// way round
if ( aCurrent.positionEquals(bNext) &&
bCurrent.positionEquals(aNext))
{
// polygons are neighbors, assemble new one
Polygon *pNew = allocatePolygon();
// insert all vertices of A up to the join (including the common vertex, ignoring
// whether the first vertex of A may be a shared vertex)
for ( size_t i = 0; i <= iVertexA; ++i )
{
pNew->insertVertex( getVertex( iPolyA, i%numVerticesA ) );
}
// insert all vertices of B _after_ the join to the end
for ( size_t i = iVertexB + 2; i < numVerticesB; ++i )
{
pNew->insertVertex( getVertex( iPolyB, i ) );
}
// insert all vertices of B from the beginning up to the join (including the common vertex
// and excluding the first vertex if the first is part of the shared edge)
for ( size_t i = 0; i <= iVertexB; ++i )
{
pNew->insertVertex( getVertex( iPolyB, i%numVerticesB ) );
}
// insert all vertices of A _after_ the join to the end
for ( size_t i = iVertexA + 2; i < numVerticesA; ++i )
{
pNew->insertVertex( getVertex( iPolyA, i ) );
}
// in case there are double vertices (in special cases), remove them
for ( size_t i = 0; i < pNew->getVertexCount(); ++i )
{
const Vector3& a = pNew->getVertex( i );
const Vector3& b = pNew->getVertex( (i + 1) % pNew->getVertexCount() );
// if the two vertices are the same...
if (a.positionEquals(b))
{
// remove a
pNew->deleteVertex( i );
// decrement counter
--i;
}
}
// delete the two old ones
OgreAssert( iPolyA != iPolyB, "PolyA and polyB are the same!" );
// polyB is always higher than polyA, so delete polyB first
deletePolygon( iPolyB );
deletePolygon( iPolyA );
// continue with next (current is deleted, so don't jump to the next after the next)
--iPolyA;
--iPolyB;
// insert new polygon
insertPolygon( pNew );
bFound = true;
break;
}
}
if ( bFound )
{
break;
}
}
if ( bFound == false )
{
// there are two polygons available with the same normal direction, but they
// could not be merged into one single because of no shared edge
bDirty = true;
break;
}
}
}
}
// recursion to merge the previous non-neighbors
if ( bDirty )
{
mergePolygons();
}
}
/**
basic algorithm:
- convert input aabb to local coordinates (scale down and shift for local origin)
- convert input aabb to a range of heightfield grid points (quantize)
- iterate over all triangles in that subset of the grid
*/
void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
{
// scale down the input aabb's so they are in local (non-scaled) coordinates
btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
// account for local origin
localAabbMin += m_localOrigin;
localAabbMax += m_localOrigin;
//quantize the aabbMin and aabbMax, and adjust the start/end ranges
int quantizedAabbMin[3];
int quantizedAabbMax[3];
quantizeWithClamp(quantizedAabbMin, localAabbMin,0);
quantizeWithClamp(quantizedAabbMax, localAabbMax,1);
// expand the min/max quantized values
// this is to catch the case where the input aabb falls between grid points!
for (int i = 0; i < 3; ++i) {
quantizedAabbMin[i]--;
quantizedAabbMax[i]++;
}
int startX=0;
int endX=m_heightStickWidth-1;
int startJ=0;
int endJ=m_heightStickLength-1;
switch (m_upAxis)
{
case 0:
{
if (quantizedAabbMin[1]>startX)
startX = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endX)
endX = quantizedAabbMax[1];
if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2];
break;
}
case 1:
{
if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2];
break;
};
case 2:
{
if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[1]>startJ)
startJ = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endJ)
endJ = quantizedAabbMax[1];
break;
}
default:
{
//need to get valid m_upAxis
btAssert(0);
}
}
for(int j=startJ; j<endJ; j++)
{
for(int x=startX; x<endX; x++)
{
btVector3 vertices[3];
if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1)))
{
//first triangle
getVertex(x,j,vertices[0]);
getVertex(x+1,j,vertices[1]);
getVertex(x+1,j+1,vertices[2]);
callback->processTriangle(vertices,x,j);
//second triangle
getVertex(x,j,vertices[0]);
getVertex(x+1,j+1,vertices[1]);
getVertex(x,j+1,vertices[2]);
callback->processTriangle(vertices,x,j);
} else
{
//first triangle
getVertex(x,j,vertices[0]);
getVertex(x,j+1,vertices[1]);
getVertex(x+1,j,vertices[2]);
callback->processTriangle(vertices,x,j);
//second triangle
getVertex(x+1,j,vertices[0]);
getVertex(x,j+1,vertices[1]);
getVertex(x+1,j+1,vertices[2]);
callback->processTriangle(vertices,x,j);
}
}
}
}
void Label::getVertices(vector<Vector3>& vertices) const
{
vertices.push_back(getVertex());
}
bool btPolyhedralConvexShape::initializePolyhedralFeatures()
{
if (m_polyhedron)
btAlignedFree(m_polyhedron);
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
m_polyhedron = new (mem) btConvexPolyhedron;
btAlignedObjectArray<btVector3> orgVertices;
for (int i=0;i<getNumVertices();i++)
{
btVector3& newVertex = orgVertices.expand();
getVertex(i,newVertex);
}
#if 0
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
{
btVector3 plane = planeEquations[p];
plane[3] -= getMargin();
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> tmpVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
btConvexHullComputer conv;
conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
#else
btConvexHullComputer conv;
conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
#endif
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
faceNormals.resize(numFaces);
btConvexHullComputer* convexUtil = &conv;
btAlignedObjectArray<btFace> tmpFaces;
tmpFaces.resize(numFaces);
int numVertices = convexUtil->vertices.size();
m_polyhedron->m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
{
m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
}
for (int i=0;i<numFaces;i++)
{
int face = convexUtil->faces[i];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const btConvexHullComputer::Edge* edge = firstEdge;
btVector3 edges[3];
int numEdges = 0;
//compute face normals
do
{
int src = edge->getSourceVertex();
tmpFaces[i].m_indices.push_back(src);
int targ = edge->getTargetVertex();
btVector3 wa = convexUtil->vertices[src];
btVector3 wb = convexUtil->vertices[targ];
btVector3 newEdge = wb-wa;
newEdge.normalize();
if (numEdges<2)
edges[numEdges++] = newEdge;
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
btScalar planeEq = 1e30f;
if (numEdges==2)
{
faceNormals[i] = edges[0].cross(edges[1]);
faceNormals[i].normalize();
tmpFaces[i].m_plane[0] = faceNormals[i].getX();
tmpFaces[i].m_plane[1] = faceNormals[i].getY();
tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
tmpFaces[i].m_plane[3] = planeEq;
}
else
{
btAssert(0);//degenerate?
faceNormals[i].setZero();
}
for (int v=0;v<tmpFaces[i].m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
if (planeEq>eq)
{
planeEq=eq;
}
}
tmpFaces[i].m_plane[3] = -planeEq;
}
//merge coplanar faces and copy them to m_polyhedron
btScalar faceWeldThreshold= 0.999f;
btAlignedObjectArray<int> todoFaces;
for (int i=0;i<tmpFaces.size();i++)
todoFaces.push_back(i);
while (todoFaces.size())
{
btAlignedObjectArray<int> coplanarFaceGroup;
int refFace = todoFaces[todoFaces.size()-1];
coplanarFaceGroup.push_back(refFace);
btFace& faceA = tmpFaces[refFace];
todoFaces.pop_back();
btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
for (int j=todoFaces.size()-1;j>=0;j--)
{
int i = todoFaces[j];
btFace& faceB = tmpFaces[i];
btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
{
coplanarFaceGroup.push_back(i);
todoFaces.remove(i);
}
}
if (coplanarFaceGroup.size()>1)
{
//do the merge: use Graham Scan 2d convex hull
btAlignedObjectArray<GrahamVector2> orgpoints;
for (int i=0;i<coplanarFaceGroup.size();i++)
{
// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
btFace& face = tmpFaces[coplanarFaceGroup[i]];
btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
btVector3 xyPlaneNormal(0,0,1);
btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal);
for (int f=0;f<face.m_indices.size();f++)
{
int orgIndex = face.m_indices[f];
btVector3 pt = m_polyhedron->m_vertices[orgIndex];
btVector3 rotatedPt = quatRotate(rotationArc,pt);
rotatedPt.setZ(0);
bool found = false;
for (int i=0;i<orgpoints.size();i++)
{
if ((rotatedPt-orgpoints[i]).length2()<0.001)
{
found=true;
break;
}
}
if (!found)
orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex));
}
}
btFace combinedFace;
for (int i=0;i<4;i++)
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
btAlignedObjectArray<GrahamVector2> hull;
GrahamScanConvexHull2D(orgpoints,hull);
for (int i=0;i<hull.size();i++)
{
combinedFace.m_indices.push_back(hull[i].m_orgIndex);
}
m_polyhedron->m_faces.push_back(combinedFace);
} else
{
for (int i=0;i<coplanarFaceGroup.size();i++)
{
m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
}
}
}
m_polyhedron->initialize();
return true;
}
/*
* March through the grid, creating the tesselation of an isosurface.
* The somewhat convoluted logic is necessary to prevent any single cube
* edge from being examined more than once. This allows for a compact
* mesh representation, with each vertex stored only once and each face
* stored simply as three integer indices into the vertex array.
*/
void MarchCube::march (IsoSurface& surface)
{
surface.clear();
ImpSurface* function = surface.getFunction();
/*
*
*/
initVertices(0, vtxGrid[0], function);
initVertices(1, vtxGrid[1], function);
setCubeFlags();
/*
* Fill in the back of the grid first (where "forward" is the +z
* direction. This is done by checking the left- and bottom-edges
* of each square at the very back of the grid, then all the edges
* along the top of the back, then all those along the right.
*/
for (int i = 0; i < resx; ++i)
{
for (int j = 0; j < resy; ++j)
{
if (edgeFlags[i][j] & LEFTBACK)
edgeGrid[0][i][j][0] =
surface.addVertex(vtxGrid[0][i][j].findSurface(
vtxGrid[0][i][j + 1], threshold));
if (edgeFlags[i][j] & BOTBACK)
edgeGrid[0][i][j][2] =
surface.addVertex(vtxGrid[0][i][j].findSurface(
vtxGrid[0][i + 1][j], threshold));
}
if (edgeFlags[i][resy - 1] & TOPBACK)
edgeGrid[0][i][resy][2] =
surface.addVertex(vtxGrid[0][i][resy].findSurface(
vtxGrid[0][i + 1][resy], threshold));
}
for (int i = 0; i < resy; ++i)
{
if (edgeFlags[resx - 1][i] & RIGHTBACK)
edgeGrid[0][resx][i][0] =
surface.addVertex(vtxGrid[0][resx][i].findSurface(
vtxGrid[0][resx][i + 1], threshold));
}
/*
* Step forward (in the +z direction) through the grid. We first
* consider the bottom-left, front-left, and bottom-front edges from
* each cube. The top edges from the top row of cubes are then
* examined, followed by those on the right edge of the grid.
*/
for (int layer = 1; layer <= resz; ++layer)
{
// cerr << "filling in layer " << layer << endl;
if (layer > 1)
{
initVertices(layer, vtxGrid[1], function);
setCubeFlags();
}
for (int i = 0; i < resx; ++i)
{
for (int j = 0; j < resy; ++j)
{
if (edgeFlags[i][j] & BOTLEFT)
edgeGrid[0][i][j][1] =
surface.addVertex(vtxGrid[0][i][j].findSurface(
vtxGrid[1][i][j],
threshold));
if (edgeFlags[i][j] & LEFTFRONT)
edgeGrid[1][i][j][0] =
surface.addVertex(vtxGrid[1][i][j].findSurface(
vtxGrid[1][i][j + 1],
threshold));
if (edgeFlags[i][j] & BOTFRONT)
edgeGrid[1][i][j][2] =
surface.addVertex(vtxGrid[1][i][j].findSurface(
vtxGrid[1][i + 1][j],
threshold));
}
if (edgeFlags[i][resy - 1] & TOPLEFT)
edgeGrid[0][i][resy][1] =
surface.addVertex(vtxGrid[0][i][resy].findSurface(
vtxGrid[1][i][resy],
threshold));
if (edgeFlags[i][resy - 1] & TOPFRONT)
edgeGrid[1][i][resy][2] =
surface.addVertex(vtxGrid[1][i][resy].findSurface(
vtxGrid[1][i + 1][resy],
threshold));
}
for (int i = 0; i < resy; ++i)
{
if (edgeFlags[resx - 1][i] & RIGHTFRONT)
edgeGrid[1][resx][i][0] =
surface.addVertex(vtxGrid[1][resx][i].findSurface(
vtxGrid[1][resx][i + 1],
threshold));
if (edgeFlags[resx - 1][i] & BOTRIGHT)
edgeGrid[0][resx][i][1] =
surface.addVertex(vtxGrid[0][resx][i].findSurface(
vtxGrid[1][resx][i],
threshold));
}
if (edgeFlags[resx - 1][resy - 1] & TOPRIGHT)
edgeGrid[0][resx][resy][1] =
surface.addVertex(vtxGrid[0][resx][resy].findSurface(
vtxGrid[1][resx][resy],
threshold));
/*
* Now that we've found all the vertices on the edges of the cubes
* in the layer, extract the set of triangles defined.
*/
int* indices;
int e0, e1, e2;
int v0, v1, v2;
static int badV = 0;
for (int i = 0; i < resx; ++i)
{
for (int j = 0; j < resy; ++j)
{
indices = triTable[vtxFlags[i][j]];
while(*indices != -1)
{
e0 = *indices++;
e1 = *indices++;
e2 = *indices++;
v0 = getVertex(i, j, e0);
v1 = getVertex(i, j, e1);
v2 = getVertex(i, j, e2);
surface.addFace(v0, v1, v2);
}
}
}
/*
* Move the vertex/edge-index grids one step forward
*/
CubeVtx** vTemp = vtxGrid[0];
vtxGrid[0] = vtxGrid[1];
vtxGrid[1] = vTemp;
int*** eTemp = edgeGrid[0];
edgeGrid[0] = edgeGrid[1];
edgeGrid[1] = eTemp;
}
surface.calcVNorms();
}
bool btPolyhedralConvexShape::initializePolyhedralFeatures()
{
if (m_polyhedron)
btAlignedFree(m_polyhedron);
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
m_polyhedron = new (mem) btConvexPolyhedron;
btAlignedObjectArray<btVector3> tmpVertices;
for (int i=0;i<getNumVertices();i++)
{
btVector3& newVertex = tmpVertices.expand();
getVertex(i,newVertex);
}
btConvexHullComputer conv;
conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
faceNormals.resize(numFaces);
btConvexHullComputer* convexUtil = &conv;
m_polyhedron->m_faces.resize(numFaces);
int numVertices = convexUtil->vertices.size();
m_polyhedron->m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
{
m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
}
for (int i=0;i<numFaces;i++)
{
int face = convexUtil->faces[i];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const btConvexHullComputer::Edge* edge = firstEdge;
btVector3 edges[3];
int numEdges = 0;
//compute face normals
//btScalar maxCross2 = 0.f;
//int chosenEdge = -1;
do
{
int src = edge->getSourceVertex();
m_polyhedron->m_faces[i].m_indices.push_back(src);
int targ = edge->getTargetVertex();
btVector3 wa = convexUtil->vertices[src];
btVector3 wb = convexUtil->vertices[targ];
btVector3 newEdge = wb-wa;
newEdge.normalize();
if (numEdges<2)
edges[numEdges++] = newEdge;
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
btScalar planeEq = 1e30f;
if (numEdges==2)
{
faceNormals[i] = edges[0].cross(edges[1]);
faceNormals[i].normalize();
m_polyhedron->m_faces[i].m_plane[0] = -faceNormals[i].getX();
m_polyhedron->m_faces[i].m_plane[1] = -faceNormals[i].getY();
m_polyhedron->m_faces[i].m_plane[2] = -faceNormals[i].getZ();
m_polyhedron->m_faces[i].m_plane[3] = planeEq;
}
else
{
btAssert(0);//degenerate?
faceNormals[i].setZero();
}
for (int v=0;v<m_polyhedron->m_faces[i].m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[m_polyhedron->m_faces[i].m_indices[v]].dot(faceNormals[i]);
if (planeEq>eq)
{
planeEq=eq;
}
}
m_polyhedron->m_faces[i].m_plane[3] = planeEq;
}
if (m_polyhedron->m_faces.size() && conv.vertices.size())
{
for (int f=0;f<m_polyhedron->m_faces.size();f++)
{
btVector3 planeNormal(m_polyhedron->m_faces[f].m_plane[0],m_polyhedron->m_faces[f].m_plane[1],m_polyhedron->m_faces[f].m_plane[2]);
btScalar planeEq = m_polyhedron->m_faces[f].m_plane[3];
btVector3 supVec = localGetSupportingVertex(-planeNormal);
if (supVec.dot(planeNormal)<planeEq)
{
m_polyhedron->m_faces[f].m_plane[0] *= -1;
m_polyhedron->m_faces[f].m_plane[1] *= -1;
m_polyhedron->m_faces[f].m_plane[2] *= -1;
m_polyhedron->m_faces[f].m_plane[3] *= -1;
int numVerts = m_polyhedron->m_faces[f].m_indices.size();
for (int v=0;v<numVerts/2;v++)
{
btSwap(m_polyhedron->m_faces[f].m_indices[v],m_polyhedron->m_faces[f].m_indices[numVerts-1-v]);
}
}
}
}
m_polyhedron->initialize();
return true;
}