more work towards CPU version
This commit is contained in:
erwincoumans
@@ -204,12 +204,21 @@ void b3DynamicBvhBroadphase::destroyProxy( b3BroadphaseProxy* absproxy,
|
||||
m_needcleanup=true;
|
||||
}
|
||||
|
||||
void b3DynamicBvhBroadphase::getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
|
||||
{
|
||||
const b3DbvtProxy* proxy=&m_proxies[objectId];
|
||||
aabbMin = proxy->m_aabbMin;
|
||||
aabbMax = proxy->m_aabbMax;
|
||||
}
|
||||
/*
|
||||
void b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
|
||||
{
|
||||
b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
|
||||
aabbMin = proxy->m_aabbMin;
|
||||
aabbMax = proxy->m_aabbMax;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
struct BroadphaseRayTester : b3DynamicBvh::ICollide
|
||||
{
|
||||
|
||||
@@ -165,13 +165,14 @@ struct b3DynamicBvhBroadphase
|
||||
void optimize();
|
||||
|
||||
/* b3BroadphaseInterface Implementation */
|
||||
b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask);
|
||||
b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int objectIndex,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask);
|
||||
virtual void destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher);
|
||||
virtual void setAabb(b3BroadphaseProxy* proxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher);
|
||||
virtual void rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3MakeVector3(0,0,0), const b3Vector3& aabbMax = b3MakeVector3(0,0,0));
|
||||
virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
|
||||
|
||||
virtual void getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
|
||||
//virtual void getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
|
||||
virtual void getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
|
||||
virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher=0);
|
||||
virtual b3OverlappingPairCache* getOverlappingPairCache();
|
||||
virtual const b3OverlappingPairCache* getOverlappingPairCache() const;
|
||||
|
||||
41
src/Bullet3Collision/NarrowPhaseCollision/b3Config.h
Normal file
41
src/Bullet3Collision/NarrowPhaseCollision/b3Config.h
Normal file
@@ -0,0 +1,41 @@
|
||||
#ifndef B3_CONFIG_H
|
||||
#define B3_CONFIG_H
|
||||
|
||||
struct b3Config
|
||||
{
|
||||
int m_maxConvexBodies;
|
||||
int m_maxConvexShapes;
|
||||
int m_maxBroadphasePairs;
|
||||
int m_maxContactCapacity;
|
||||
int m_compoundPairCapacity;
|
||||
|
||||
int m_maxVerticesPerFace;
|
||||
int m_maxFacesPerShape;
|
||||
int m_maxConvexVertices;
|
||||
int m_maxConvexIndices;
|
||||
int m_maxConvexUniqueEdges;
|
||||
|
||||
int m_maxCompoundChildShapes;
|
||||
|
||||
int m_maxTriConvexPairCapacity;
|
||||
|
||||
b3Config()
|
||||
:m_maxConvexBodies(32*1024),
|
||||
m_maxVerticesPerFace(64),
|
||||
m_maxFacesPerShape(12),
|
||||
m_maxConvexVertices(8192),
|
||||
m_maxConvexIndices(81920),
|
||||
m_maxConvexUniqueEdges(8192),
|
||||
m_maxCompoundChildShapes(8192),
|
||||
m_maxTriConvexPairCapacity(256*1024)
|
||||
{
|
||||
m_maxConvexShapes = m_maxConvexBodies;
|
||||
m_maxBroadphasePairs = 16*m_maxConvexBodies;
|
||||
m_maxContactCapacity = m_maxBroadphasePairs;
|
||||
m_compoundPairCapacity = 1024*1024;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif//B3_CONFIG_H
|
||||
|
||||
520
src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp
Normal file
520
src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp
Normal file
@@ -0,0 +1,520 @@
|
||||
/*
|
||||
Copyright (c) 2012 Advanced Micro Devices, Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
//Originally written by Erwin Coumans
|
||||
|
||||
|
||||
#include "b3ConvexUtility.h"
|
||||
#include "Bullet3Geometry/b3ConvexHullComputer.h"
|
||||
#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
|
||||
#include "Bullet3Common/b3Quaternion.h"
|
||||
#include "Bullet3Common/b3HashMap.h"
|
||||
|
||||
//#include "b3ConvexPolyhedronCL.h"
|
||||
|
||||
|
||||
|
||||
b3ConvexUtility::~b3ConvexUtility()
|
||||
{
|
||||
}
|
||||
|
||||
bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
|
||||
{
|
||||
|
||||
|
||||
|
||||
b3ConvexHullComputer conv;
|
||||
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;
|
||||
tmpFaces.resize(numFaces);
|
||||
|
||||
int numVertices = convexUtil->vertices.size();
|
||||
m_vertices.resize(numVertices);
|
||||
for (int p=0;p<numVertices;p++)
|
||||
{
|
||||
m_vertices[p] = convexUtil->vertices[p];
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
|
||||
b3Vector3 edges[3];
|
||||
int numEdges = 0;
|
||||
//compute face normals
|
||||
|
||||
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;
|
||||
newEdge.normalize();
|
||||
if (numEdges<2)
|
||||
edges[numEdges++] = newEdge;
|
||||
|
||||
edge = edge->getNextEdgeOfFace();
|
||||
} while (edge!=firstEdge);
|
||||
|
||||
b3Scalar 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
|
||||
{
|
||||
b3Assert(0);//degenerate?
|
||||
faceNormals[i].setZero();
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
planeEq=eq;
|
||||
}
|
||||
}
|
||||
tmpFaces[i].m_plane[3] = -planeEq;
|
||||
}
|
||||
|
||||
//merge coplanar faces and copy them to m_polyhedron
|
||||
|
||||
b3Scalar faceWeldThreshold= 0.999f;
|
||||
b3AlignedObjectArray<int> todoFaces;
|
||||
for (int i=0;i<tmpFaces.size();i++)
|
||||
todoFaces.push_back(i);
|
||||
|
||||
while (todoFaces.size())
|
||||
{
|
||||
b3AlignedObjectArray<int> coplanarFaceGroup;
|
||||
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--)
|
||||
{
|
||||
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)
|
||||
{
|
||||
coplanarFaceGroup.push_back(i);
|
||||
todoFaces.remove(i);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool did_merge = false;
|
||||
if (coplanarFaceGroup.size()>1)
|
||||
{
|
||||
//do the merge: use Graham Scan 2d convex hull
|
||||
|
||||
b3AlignedObjectArray<b3GrahamVector3> orgpoints;
|
||||
b3Vector3 averageFaceNormal = b3MakeVector3(0,0,0);
|
||||
|
||||
for (int i=0;i<coplanarFaceGroup.size();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++)
|
||||
{
|
||||
int orgIndex = face.m_indices[f];
|
||||
b3Vector3 pt = 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(b3GrahamVector3(pt,orgIndex));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
b3MyFace combinedFace;
|
||||
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);
|
||||
|
||||
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++) {
|
||||
|
||||
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) {
|
||||
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_faces.push_back(combinedFace);
|
||||
}
|
||||
}
|
||||
if(!did_merge)
|
||||
{
|
||||
for (int i=0;i<coplanarFaceGroup.size();i++)
|
||||
{
|
||||
b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
|
||||
m_faces.push_back(face);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
initialize();
|
||||
|
||||
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;
|
||||
return true;
|
||||
}
|
||||
|
||||
struct b3InternalVertexPair
|
||||
{
|
||||
b3InternalVertexPair(short int v0,short int v1)
|
||||
:m_v0(v0),
|
||||
m_v1(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);
|
||||
}
|
||||
bool equals(const b3InternalVertexPair& other) const
|
||||
{
|
||||
return m_v0==other.m_v0 && m_v1==other.m_v1;
|
||||
}
|
||||
};
|
||||
|
||||
struct b3InternalEdge
|
||||
{
|
||||
b3InternalEdge()
|
||||
:m_face0(-1),
|
||||
m_face1(-1)
|
||||
{
|
||||
}
|
||||
short int m_face0;
|
||||
short int m_face1;
|
||||
};
|
||||
|
||||
//
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
bool b3ConvexUtility::testContainment() const
|
||||
{
|
||||
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]);
|
||||
|
||||
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)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
void b3ConvexUtility::initialize()
|
||||
{
|
||||
|
||||
b3HashMap<b3InternalVertexPair,b3InternalEdge> edges;
|
||||
|
||||
b3Scalar TotalArea = 0.0f;
|
||||
|
||||
m_localCenter.setValue(0, 0, 0);
|
||||
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++)
|
||||
{
|
||||
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];
|
||||
edge.normalize();
|
||||
|
||||
bool found = false;
|
||||
b3Vector3 diff,diff2;
|
||||
|
||||
for (int p=0;p<m_uniqueEdges.size();p++)
|
||||
{
|
||||
diff = m_uniqueEdges[p]-edge;
|
||||
diff2 = m_uniqueEdges[p]+edge;
|
||||
|
||||
// if ((diff.length2()==0.f) ||
|
||||
// (diff2.length2()==0.f))
|
||||
|
||||
if (IsAlmostZero(diff) ||
|
||||
IsAlmostZero(diff2))
|
||||
{
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found)
|
||||
{
|
||||
m_uniqueEdges.push_back(edge);
|
||||
}
|
||||
|
||||
if (edptr)
|
||||
{
|
||||
//TBD: figure out why I added this assert
|
||||
// b3Assert(edptr->m_face0>=0);
|
||||
// b3Assert(edptr->m_face1<0);
|
||||
edptr->m_face1 = i;
|
||||
} else
|
||||
{
|
||||
b3InternalEdge ed;
|
||||
ed.m_face0 = i;
|
||||
edges.insert(vp,ed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_CONNECTED_FACES
|
||||
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++)
|
||||
{
|
||||
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);
|
||||
|
||||
int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
|
||||
m_faces[i].m_connectedFaces[j] = connectedFace;
|
||||
}
|
||||
}
|
||||
#endif//USE_CONNECTED_FACES
|
||||
|
||||
for(int i=0;i<m_faces.size();i++)
|
||||
{
|
||||
int numVertices = m_faces[i].m_indices.size();
|
||||
int NbTris = numVertices-2;
|
||||
|
||||
const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]];
|
||||
for(int j=1;j<=NbTris;j++)
|
||||
{
|
||||
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;
|
||||
m_localCenter += Area * Center;
|
||||
TotalArea += Area;
|
||||
}
|
||||
}
|
||||
m_localCenter /= TotalArea;
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
if(1)
|
||||
{
|
||||
m_radius = FLT_MAX;
|
||||
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)
|
||||
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++)
|
||||
{
|
||||
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();
|
||||
}
|
||||
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(3.0f);
|
||||
const int LargestExtent = mE.maxAxis();
|
||||
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;
|
||||
bool FoundBox = false;
|
||||
for(int j=0;j<1024;j++)
|
||||
{
|
||||
if(testContainment())
|
||||
{
|
||||
FoundBox = true;
|
||||
break;
|
||||
}
|
||||
|
||||
m_extents[LargestExtent] -= Step;
|
||||
}
|
||||
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;
|
||||
|
||||
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())
|
||||
{
|
||||
m_extents[e0] = Saved0;
|
||||
m_extents[e1] = Saved1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
62
src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h
Normal file
62
src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h
Normal file
@@ -0,0 +1,62 @@
|
||||
|
||||
/*
|
||||
Copyright (c) 2012 Advanced Micro Devices, Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
//Originally written by Erwin Coumans
|
||||
|
||||
#ifndef _BT_CONVEX_UTILITY_H
|
||||
#define _BT_CONVEX_UTILITY_H
|
||||
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Bullet3Common/b3Transform.h"
|
||||
|
||||
//#include "b3ConvexPolyhedronCL.h"
|
||||
|
||||
|
||||
struct b3MyFace
|
||||
{
|
||||
b3AlignedObjectArray<int> m_indices;
|
||||
b3Scalar m_plane[4];
|
||||
};
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility
|
||||
{
|
||||
public:
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
b3Vector3 m_localCenter;
|
||||
b3Vector3 m_extents;
|
||||
b3Vector3 mC;
|
||||
b3Vector3 mE;
|
||||
b3Scalar m_radius;
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> m_vertices;
|
||||
b3AlignedObjectArray<b3MyFace> m_faces;
|
||||
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
|
||||
|
||||
|
||||
b3ConvexUtility()
|
||||
{
|
||||
}
|
||||
virtual ~b3ConvexUtility();
|
||||
|
||||
bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles=true);
|
||||
|
||||
void initialize();
|
||||
bool testContainment() const;
|
||||
|
||||
|
||||
|
||||
};
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
|
||||
#include "b3CpuCollisionWorld.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
|
||||
|
||||
|
||||
b3CpuCollisionWorld::b3CpuCollisionWorld(b3DynamicBvhBroadphase* bp, b3CpuNarrowPhase* np)
|
||||
:m_bp(bp),
|
||||
m_np(np)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
b3CpuCollisionWorld::~b3CpuCollisionWorld()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void b3CpuCollisionWorld::addCollidable(int bodyIndex, int collidableIndex,const b3Vector3& position, const b3Quaternion& orientation)
|
||||
{
|
||||
b3Vector3 aabbMinWorld, aabbMaxWorld;
|
||||
|
||||
if (collidableIndex>=0)
|
||||
{
|
||||
b3Aabb localAabb = m_np->getLocalSpaceAabb(collidableIndex);
|
||||
b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
|
||||
b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
|
||||
|
||||
b3Scalar margin = 0.01f;
|
||||
b3Transform t;
|
||||
t.setIdentity();
|
||||
t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
|
||||
t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
|
||||
b3TransformAabb(localAabbMin,localAabbMax, margin,t,aabbMinWorld,aabbMaxWorld);
|
||||
|
||||
m_bp->createProxy(aabbMinWorld,aabbMaxWorld,bodyIndex,0,1,1);
|
||||
|
||||
b3Vector3 aabbMin,aabbMax;
|
||||
m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
|
||||
#ifndef B3_CPU2_COLLISION_WORLD_H
|
||||
#define B3_CPU2_COLLISION_WORLD_H
|
||||
|
||||
class b3CpuNarrowPhase;
|
||||
struct b3DynamicBvhBroadphase;
|
||||
#include "Bullet3Common/b3Quaternion.h"
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
|
||||
class b3CpuCollisionWorld
|
||||
{
|
||||
protected:
|
||||
|
||||
b3DynamicBvhBroadphase* m_bp;
|
||||
b3CpuNarrowPhase* m_np;
|
||||
|
||||
public:
|
||||
|
||||
b3CpuCollisionWorld(b3DynamicBvhBroadphase* bp, b3CpuNarrowPhase* np);
|
||||
|
||||
void addCollidable(int bodyIndex, int collidableIndex,const b3Vector3& position, const b3Quaternion& orientation);
|
||||
|
||||
virtual ~b3CpuCollisionWorld();
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //B3_CPU_COLLISION_WORLD_H
|
||||
208
src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
Normal file
208
src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
Normal file
@@ -0,0 +1,208 @@
|
||||
#include "b3CpuNarrowPhase.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
|
||||
struct b3CpuNarrowPhaseInternalData
|
||||
{
|
||||
b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
|
||||
b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
|
||||
b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
|
||||
b3Config m_config;
|
||||
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
|
||||
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
|
||||
b3AlignedObjectArray<b3Vector3> m_convexVertices;
|
||||
b3AlignedObjectArray<int> m_convexIndices;
|
||||
b3AlignedObjectArray<b3GpuFace> m_convexFaces;
|
||||
|
||||
|
||||
int m_numAcceleratedShapes;
|
||||
};
|
||||
|
||||
|
||||
b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
|
||||
{
|
||||
m_data = new b3CpuNarrowPhaseInternalData;
|
||||
m_data->m_config = config;
|
||||
m_data->m_numAcceleratedShapes = 0;
|
||||
}
|
||||
|
||||
b3CpuNarrowPhase::~b3CpuNarrowPhase()
|
||||
{
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>* broadphasePairs, b3AlignedObjectArray<b3Aabb>* aabbsWorldSpace)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
|
||||
{
|
||||
int collidableIndex = allocateCollidable();
|
||||
if (collidableIndex<0)
|
||||
return collidableIndex;
|
||||
|
||||
|
||||
b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
|
||||
col.m_shapeType = SHAPE_CONVEX_HULL;
|
||||
col.m_shapeIndex = -1;
|
||||
|
||||
|
||||
{
|
||||
b3Vector3 localCenter=b3MakeVector3(0,0,0);
|
||||
for (int i=0;i<utilPtr->m_vertices.size();i++)
|
||||
localCenter+=utilPtr->m_vertices[i];
|
||||
localCenter*= (1.f/utilPtr->m_vertices.size());
|
||||
utilPtr->m_localCenter = localCenter;
|
||||
|
||||
col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col);
|
||||
}
|
||||
|
||||
if (col.m_shapeIndex>=0)
|
||||
{
|
||||
b3Aabb aabb;
|
||||
|
||||
b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f);
|
||||
b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f);
|
||||
|
||||
for (int i=0;i<utilPtr->m_vertices.size();i++)
|
||||
{
|
||||
myAabbMin.setMin(utilPtr->m_vertices[i]);
|
||||
myAabbMax.setMax(utilPtr->m_vertices[i]);
|
||||
}
|
||||
aabb.m_min[0] = myAabbMin[0];
|
||||
aabb.m_min[1] = myAabbMin[1];
|
||||
aabb.m_min[2] = myAabbMin[2];
|
||||
aabb.m_minIndices[3] = 0;
|
||||
|
||||
aabb.m_max[0] = myAabbMax[0];
|
||||
aabb.m_max[1] = myAabbMax[1];
|
||||
aabb.m_max[2] = myAabbMax[2];
|
||||
aabb.m_signedMaxIndices[3] = 0;
|
||||
|
||||
m_data->m_localShapeAABBCPU.push_back(aabb);
|
||||
|
||||
}
|
||||
|
||||
return collidableIndex;
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::allocateCollidable()
|
||||
{
|
||||
int curSize = m_data->m_collidablesCPU.size();
|
||||
if (curSize<m_data->m_config.m_maxConvexShapes)
|
||||
{
|
||||
m_data->m_collidablesCPU.expand();
|
||||
return curSize;
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes);
|
||||
}
|
||||
return -1;
|
||||
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
|
||||
{
|
||||
b3AlignedObjectArray<b3Vector3> verts;
|
||||
|
||||
unsigned char* vts = (unsigned char*) vertices;
|
||||
for (int i=0;i<numVertices;i++)
|
||||
{
|
||||
float* vertex = (float*) &vts[i*strideInBytes];
|
||||
verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2]));
|
||||
}
|
||||
|
||||
b3ConvexUtility* utilPtr = new b3ConvexUtility();
|
||||
bool merge = true;
|
||||
if (numVertices)
|
||||
{
|
||||
utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge);
|
||||
}
|
||||
|
||||
int collidableIndex = registerConvexHullShape(utilPtr);
|
||||
|
||||
delete utilPtr;
|
||||
return collidableIndex;
|
||||
}
|
||||
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col)
|
||||
{
|
||||
|
||||
m_data->m_convexData.resize(m_data->m_numAcceleratedShapes+1);
|
||||
m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1);
|
||||
|
||||
|
||||
b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1);
|
||||
convex.mC = convexPtr->mC;
|
||||
convex.mE = convexPtr->mE;
|
||||
convex.m_extents= convexPtr->m_extents;
|
||||
convex.m_localCenter = convexPtr->m_localCenter;
|
||||
convex.m_radius = convexPtr->m_radius;
|
||||
|
||||
convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
|
||||
int edgeOffset = m_data->m_uniqueEdges.size();
|
||||
convex.m_uniqueEdgesOffset = edgeOffset;
|
||||
|
||||
m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges);
|
||||
|
||||
//convex data here
|
||||
int i;
|
||||
for ( i=0;i<convexPtr->m_uniqueEdges.size();i++)
|
||||
{
|
||||
m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i];
|
||||
}
|
||||
|
||||
int faceOffset = m_data->m_convexFaces.size();
|
||||
convex.m_faceOffset = faceOffset;
|
||||
convex.m_numFaces = convexPtr->m_faces.size();
|
||||
|
||||
m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces);
|
||||
|
||||
|
||||
for (i=0;i<convexPtr->m_faces.size();i++)
|
||||
{
|
||||
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
|
||||
convexPtr->m_faces[i].m_plane[1],
|
||||
convexPtr->m_faces[i].m_plane[2],
|
||||
convexPtr->m_faces[i].m_plane[3]);
|
||||
|
||||
|
||||
int indexOffset = m_data->m_convexIndices.size();
|
||||
int numIndices = convexPtr->m_faces[i].m_indices.size();
|
||||
m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices;
|
||||
m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset;
|
||||
m_data->m_convexIndices.resize(indexOffset+numIndices);
|
||||
for (int p=0;p<numIndices;p++)
|
||||
{
|
||||
m_data->m_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p];
|
||||
}
|
||||
}
|
||||
|
||||
convex.m_numVertices = convexPtr->m_vertices.size();
|
||||
int vertexOffset = m_data->m_convexVertices.size();
|
||||
convex.m_vertexOffset =vertexOffset;
|
||||
|
||||
m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices);
|
||||
for (int i=0;i<convexPtr->m_vertices.size();i++)
|
||||
{
|
||||
m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i];
|
||||
}
|
||||
|
||||
(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
|
||||
|
||||
|
||||
|
||||
return m_data->m_numAcceleratedShapes++;
|
||||
}
|
||||
|
||||
const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
|
||||
{
|
||||
return m_data->m_localShapeAABBCPU[collidableIndex];
|
||||
}
|
||||
101
src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h
Normal file
101
src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h
Normal file
@@ -0,0 +1,101 @@
|
||||
#ifndef B3_CPU_NARROWPHASE_H
|
||||
#define B3_CPU_NARROWPHASE_H
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
|
||||
class b3CpuNarrowPhase
|
||||
{
|
||||
protected:
|
||||
|
||||
struct b3CpuNarrowPhaseInternalData* m_data;
|
||||
int m_acceleratedCompanionShapeIndex;
|
||||
int m_planeBodyIndex;
|
||||
int m_static0Index;
|
||||
|
||||
int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr,b3Collidable& col);
|
||||
int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
|
||||
|
||||
public:
|
||||
|
||||
|
||||
|
||||
|
||||
b3CpuNarrowPhase(const struct b3Config& config);
|
||||
|
||||
virtual ~b3CpuNarrowPhase(void);
|
||||
|
||||
int registerSphereShape(float radius);
|
||||
int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
|
||||
|
||||
int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
|
||||
int registerFace(const b3Vector3& faceNormal, float faceConstant);
|
||||
|
||||
int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling);
|
||||
|
||||
//do they need to be merged?
|
||||
|
||||
int registerConvexHullShape(b3ConvexUtility* utilPtr);
|
||||
int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
|
||||
|
||||
//int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu);
|
||||
void setObjectTransform(const float* position, const float* orientation , int bodyIndex);
|
||||
|
||||
void writeAllBodiesToGpu();
|
||||
void reset();
|
||||
void readbackAllBodiesToCpu();
|
||||
bool getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const;
|
||||
|
||||
void setObjectTransformCpu(float* position, float* orientation , int bodyIndex);
|
||||
void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex);
|
||||
|
||||
|
||||
//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
|
||||
virtual void computeContacts(b3AlignedObjectArray<b3Int4>* broadphasePairs, b3AlignedObjectArray<b3Aabb>* aabbsWorldSpace);
|
||||
|
||||
|
||||
|
||||
const struct b3RigidBodyCL* getBodiesCpu() const;
|
||||
//struct b3RigidBodyCL* getBodiesCpu();
|
||||
|
||||
int getNumBodiesGpu() const;
|
||||
|
||||
|
||||
int getNumBodyInertiasGpu() const;
|
||||
|
||||
|
||||
const struct b3Collidable* getCollidablesCpu() const;
|
||||
int getNumCollidablesGpu() const;
|
||||
|
||||
|
||||
const struct b3Contact4* getContactsCPU() const;
|
||||
|
||||
|
||||
int getNumContactsGpu() const;
|
||||
|
||||
|
||||
|
||||
int getNumRigidBodies() const;
|
||||
|
||||
int allocateCollidable();
|
||||
|
||||
int getStatic0Index() const
|
||||
{
|
||||
return m_static0Index;
|
||||
}
|
||||
b3Collidable& getCollidableCpu(int collidableIndex);
|
||||
const b3Collidable& getCollidableCpu(int collidableIndex) const;
|
||||
|
||||
const b3CpuNarrowPhaseInternalData* getInternalData() const
|
||||
{
|
||||
return m_data;
|
||||
}
|
||||
|
||||
const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const;
|
||||
};
|
||||
|
||||
#endif //B3_CPU_NARROWPHASE_H
|
||||
|
||||
Reference in New Issue
Block a user