Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

312
src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp
View File

@@ -13,52 +13,42 @@ subject to the following restrictions:
*/
//Originally written by Erwin Coumans
#include "b3ConvexUtility.h"
#include "Bullet3Geometry/b3ConvexHullComputer.h"
#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3HashMap.h"
b3ConvexUtility::~b3ConvexUtility()
{
}
bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
{
b3ConvexHullComputer conv;
conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3),numPoints,0.f,0.f);
conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3), numPoints, 0.f, 0.f);
b3AlignedObjectArray<b3Vector3> faceNormals;
int numFaces = conv.faces.size();
faceNormals.resize(numFaces);
b3ConvexHullComputer* convexUtil = &conv;
b3AlignedObjectArray<b3MyFace> tmpFaces;
b3AlignedObjectArray<b3MyFace> tmpFaces;
tmpFaces.resize(numFaces);
int numVertices = convexUtil->vertices.size();
m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
for (int p = 0; p < numVertices; p++)
{
m_vertices[p] = convexUtil->vertices[p];
}
for (int i=0;i<numFaces;i++)
for (int i = 0; i < numFaces; i++)
{
int face = convexUtil->faces[i];
//printf("face=%d\n",face);
const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const b3ConvexHullComputer::Edge* edge = firstEdge;
const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const b3ConvexHullComputer::Edge* edge = firstEdge;
b3Vector3 edges[3];
int numEdges = 0;
@@ -66,25 +56,23 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
do
{
int src = edge->getSourceVertex();
tmpFaces[i].m_indices.push_back(src);
int targ = edge->getTargetVertex();
b3Vector3 wa = convexUtil->vertices[src];
b3Vector3 wb = convexUtil->vertices[targ];
b3Vector3 newEdge = wb-wa;
b3Vector3 newEdge = wb - wa;
newEdge.normalize();
if (numEdges<2)
if (numEdges < 2)
edges[numEdges++] = newEdge;
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
} while (edge != firstEdge);
b3Scalar planeEq = 1e30f;
if (numEdges==2)
if (numEdges == 2)
{
faceNormals[i] = edges[0].cross(edges[1]);
faceNormals[i].normalize();
@@ -92,20 +80,19 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
tmpFaces[i].m_plane[1] = faceNormals[i].getY();
tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
tmpFaces[i].m_plane[3] = planeEq;
}
else
{
b3Assert(0);//degenerate?
b3Assert(0); //degenerate?
faceNormals[i].setZero();
}
for (int v=0;v<tmpFaces[i].m_indices.size();v++)
for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
{
b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
if (planeEq>eq)
if (planeEq > eq)
{
planeEq=eq;
planeEq = eq;
}
}
tmpFaces[i].m_plane[3] = -planeEq;
@@ -113,89 +100,86 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
//merge coplanar faces and copy them to m_polyhedron
b3Scalar faceWeldThreshold= 0.999f;
b3Scalar faceWeldThreshold = 0.999f;
b3AlignedObjectArray<int> todoFaces;
for (int i=0;i<tmpFaces.size();i++)
for (int i = 0; i < tmpFaces.size(); i++)
todoFaces.push_back(i);
while (todoFaces.size())
{
b3AlignedObjectArray<int> coplanarFaceGroup;
int refFace = todoFaces[todoFaces.size()-1];
int refFace = todoFaces[todoFaces.size() - 1];
coplanarFaceGroup.push_back(refFace);
b3MyFace& faceA = tmpFaces[refFace];
todoFaces.pop_back();
b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
for (int j=todoFaces.size()-1;j>=0;j--)
b3Vector3 faceNormalA = b3MakeVector3(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];
b3MyFace& faceB = tmpFaces[i];
b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
b3Vector3 faceNormalB = b3MakeVector3(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)
if (coplanarFaceGroup.size() > 1)
{
//do the merge: use Graham Scan 2d convex hull
b3AlignedObjectArray<b3GrahamVector3> orgpoints;
b3Vector3 averageFaceNormal = b3MakeVector3(0,0,0);
b3Vector3 averageFaceNormal = b3MakeVector3(0, 0, 0);
for (int i=0;i<coplanarFaceGroup.size();i++)
for (int i = 0; i < coplanarFaceGroup.size(); i++)
{
// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
b3MyFace& face = tmpFaces[coplanarFaceGroup[i]];
b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
averageFaceNormal+=faceNormal;
for (int f=0;f<face.m_indices.size();f++)
b3Vector3 faceNormal = b3MakeVector3(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];
b3Vector3 pt = m_vertices[orgIndex];
bool found = false;
for (int i=0;i<orgpoints.size();i++)
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;
found = true;
break;
}
}
if (!found)
orgpoints.push_back(b3GrahamVector3(pt,orgIndex));
orgpoints.push_back(b3GrahamVector3(pt, orgIndex));
}
}
b3MyFace combinedFace;
for (int i=0;i<4;i++)
for (int i = 0; i < 4; i++)
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
b3AlignedObjectArray<b3GrahamVector3> hull;
averageFaceNormal.normalize();
b3GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
b3GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal);
for (int i=0;i<hull.size();i++)
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++)
for (int k = 0; k < orgpoints.size(); k++)
{
if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
{
orgpoints[k].m_orgIndex = -1; // invalidate...
orgpoints[k].m_orgIndex = -1; // invalidate...
break;
}
}
@@ -203,38 +187,41 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
// 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...
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++) {
for (int j = 0; j < tmpFaces.size(); j++)
{
b3MyFace& 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) {
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...
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) {
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)
if (reject_merge)
break;
}
if(reject_merge)
if (reject_merge)
break;
}
@@ -245,18 +232,14 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
m_faces.push_back(combinedFace);
}
}
if(!did_merge)
if (!did_merge)
{
for (int i=0;i<coplanarFaceGroup.size();i++)
for (int i = 0; i < coplanarFaceGroup.size(); i++)
{
b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
m_faces.push_back(face);
}
}
}
}
initialize();
@@ -264,43 +247,38 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
return true;
}
inline bool IsAlmostZero(const b3Vector3& v)
{
if(fabsf(v.getX())>1e-6 || fabsf(v.getY())>1e-6 || fabsf(v.getZ())>1e-6) return false;
if (fabsf(v.getX()) > 1e-6 || fabsf(v.getY()) > 1e-6 || fabsf(v.getZ()) > 1e-6) return false;
return true;
}
struct b3InternalVertexPair
{
b3InternalVertexPair(short int v0,short int v1)
:m_v0(v0),
m_v1(v1)
b3InternalVertexPair(short int v0, short int v1)
: m_v0(v0),
m_v1(v1)
{
if (m_v1>m_v0)
b3Swap(m_v0,m_v1);
if (m_v1 > m_v0)
b3Swap(m_v0, m_v1);
}
short int m_v0;
short int m_v1;
int getHash() const
{
return m_v0+(m_v1<<16);
return m_v0 + (m_v1 << 16);
}
bool equals(const b3InternalVertexPair& other) const
{
return m_v0==other.m_v0 && m_v1==other.m_v1;
return m_v0 == other.m_v0 && m_v1 == other.m_v1;
}
};
struct b3InternalEdge
{
b3InternalEdge()
:m_face0(-1),
m_face1(-1)
: m_face0(-1),
m_face1(-1)
{
}
short int m_face0;
@@ -312,23 +290,31 @@ struct b3InternalEdge
#ifdef TEST_INTERNAL_OBJECTS
bool b3ConvexUtility::testContainment() const
{
for(int p=0;p<8;p++)
for (int p = 0; p < 8; p++)
{
b3Vector3 LocalPt;
if(p==0) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
else if(p==1) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
else if(p==2) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
else if(p==3) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
else if(p==4) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
else if(p==5) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
else if(p==6) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
else if(p==7) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
if (p == 0)
LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
else if (p == 1)
LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
else if (p == 2)
LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
else if (p == 3)
LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
else if (p == 4)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
else if (p == 5)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
else if (p == 6)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
else if (p == 7)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
for(int i=0;i<m_faces.size();i++)
for (int i = 0; i < m_faces.size(); i++)
{
const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
if(d>0.0f)
if (d > 0.0f)
return false;
}
}
@@ -336,39 +322,38 @@ bool b3ConvexUtility::testContainment() const
}
#endif
void b3ConvexUtility::initialize()
void b3ConvexUtility::initialize()
{
b3HashMap<b3InternalVertexPair,b3InternalEdge> edges;
b3HashMap<b3InternalVertexPair, b3InternalEdge> edges;
b3Scalar TotalArea = 0.0f;
m_localCenter.setValue(0, 0, 0);
for(int i=0;i<m_faces.size();i++)
for (int i = 0; i < m_faces.size(); i++)
{
int numVertices = m_faces[i].m_indices.size();
int NbTris = numVertices;
for(int j=0;j<NbTris;j++)
for (int j = 0; j < NbTris; j++)
{
int k = (j+1)%numVertices;
b3InternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
int k = (j + 1) % numVertices;
b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
b3InternalEdge* edptr = edges.find(vp);
b3Vector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
b3Vector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
edge.normalize();
bool found = false;
b3Vector3 diff,diff2;
b3Vector3 diff, diff2;
for (int p=0;p<m_uniqueEdges.size();p++)
for (int p = 0; p < m_uniqueEdges.size(); p++)
{
diff = m_uniqueEdges[p]-edge;
diff2 = m_uniqueEdges[p]+edge;
diff = m_uniqueEdges[p] - edge;
diff2 = m_uniqueEdges[p] + edge;
// if ((diff.length2()==0.f) ||
// if ((diff.length2()==0.f) ||
// (diff2.length2()==0.f))
if (IsAlmostZero(diff) ||
IsAlmostZero(diff2))
if (IsAlmostZero(diff) ||
IsAlmostZero(diff2))
{
found = true;
break;
@@ -382,106 +367,101 @@ void b3ConvexUtility::initialize()
if (edptr)
{
//TBD: figure out why I added this assert
// b3Assert(edptr->m_face0>=0);
// b3Assert(edptr->m_face1<0);
//TBD: figure out why I added this assert
// b3Assert(edptr->m_face0>=0);
// b3Assert(edptr->m_face1<0);
edptr->m_face1 = i;
} else
}
else
{
b3InternalEdge ed;
ed.m_face0 = i;
edges.insert(vp,ed);
edges.insert(vp, ed);
}
}
}
#ifdef USE_CONNECTED_FACES
for(int i=0;i<m_faces.size();i++)
for (int i = 0; i < m_faces.size(); i++)
{
int numVertices = m_faces[i].m_indices.size();
m_faces[i].m_connectedFaces.resize(numVertices);
for(int j=0;j<numVertices;j++)
for (int j = 0; j < numVertices; j++)
{
int k = (j+1)%numVertices;
b3InternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
int k = (j + 1) % numVertices;
b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
b3InternalEdge* edptr = edges.find(vp);
b3Assert(edptr);
b3Assert(edptr->m_face0>=0);
b3Assert(edptr->m_face1>=0);
b3Assert(edptr->m_face0 >= 0);
b3Assert(edptr->m_face1 >= 0);
int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
m_faces[i].m_connectedFaces[j] = connectedFace;
}
}
#endif//USE_CONNECTED_FACES
#endif //USE_CONNECTED_FACES
for(int i=0;i<m_faces.size();i++)
for (int i = 0; i < m_faces.size(); i++)
{
int numVertices = m_faces[i].m_indices.size();
int NbTris = numVertices-2;
int NbTris = numVertices - 2;
const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]];
for(int j=1;j<=NbTris;j++)
for (int j = 1; j <= NbTris; j++)
{
int k = (j+1)%numVertices;
int k = (j + 1) % numVertices;
const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]];
const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]];
b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
b3Vector3 Center = (p0+p1+p2)/3.0f;
b3Vector3 Center = (p0 + p1 + p2) / 3.0f;
m_localCenter += Area * Center;
TotalArea += Area;
}
}
m_localCenter /= TotalArea;
#ifdef TEST_INTERNAL_OBJECTS
if(1)
if (1)
{
m_radius = FLT_MAX;
for(int i=0;i<m_faces.size();i++)
for (int i = 0; i < m_faces.size(); i++)
{
const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
if(dist<m_radius)
if (dist < m_radius)
m_radius = dist;
}
b3Scalar MinX = FLT_MAX;
b3Scalar MinY = FLT_MAX;
b3Scalar MinZ = FLT_MAX;
b3Scalar MaxX = -FLT_MAX;
b3Scalar MaxY = -FLT_MAX;
b3Scalar MaxZ = -FLT_MAX;
for(int i=0; i<m_vertices.size(); i++)
for (int i = 0; i < m_vertices.size(); i++)
{
const b3Vector3& pt = m_vertices[i];
if(pt.getX()<MinX) MinX = pt.getX();
if(pt.getX()>MaxX) MaxX = pt.getX();
if(pt.getY()<MinY) MinY = pt.getY();
if(pt.getY()>MaxY) MaxY = pt.getY();
if(pt.getZ()<MinZ) MinZ = pt.getZ();
if(pt.getZ()>MaxZ) MaxZ = pt.getZ();
if (pt.getX() < MinX) MinX = pt.getX();
if (pt.getX() > MaxX) MaxX = pt.getX();
if (pt.getY() < MinY) MinY = pt.getY();
if (pt.getY() > MaxY) MaxY = pt.getY();
if (pt.getZ() < MinZ) MinZ = pt.getZ();
if (pt.getZ() > MaxZ) MaxZ = pt.getZ();
}
mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
// const b3Scalar r = m_radius / sqrtf(2.0f);
// const b3Scalar r = m_radius / sqrtf(2.0f);
const b3Scalar r = m_radius / sqrtf(3.0f);
const int LargestExtent = mE.maxAxis();
const b3Scalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
const b3Scalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
m_extents[0] = m_extents[1] = m_extents[2] = r;
m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
bool FoundBox = false;
for(int j=0;j<1024;j++)
for (int j = 0; j < 1024; j++)
{
if(testContainment())
if (testContainment())
{
FoundBox = true;
break;
@@ -489,25 +469,25 @@ void b3ConvexUtility::initialize()
m_extents[LargestExtent] -= Step;
}
if(!FoundBox)
if (!FoundBox)
{
m_extents[0] = m_extents[1] = m_extents[2] = r;
}
else
{
// Refine the box
const b3Scalar Step = (m_radius - r)/1024.0f;
const int e0 = (1<<LargestExtent) & 3;
const int e1 = (1<<e0) & 3;
const b3Scalar Step = (m_radius - r) / 1024.0f;
const int e0 = (1 << LargestExtent) & 3;
const int e1 = (1 << e0) & 3;
for(int j=0;j<1024;j++)
for (int j = 0; j < 1024; j++)
{
const b3Scalar Saved0 = m_extents[e0];
const b3Scalar Saved1 = m_extents[e1];
m_extents[e0] += Step;
m_extents[e1] += Step;
if(!testContainment())
if (!testContainment())
{
m_extents[e0] = Saved0;
m_extents[e1] = Saved1;