Code-style consistency improvement:
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
@@ -1,41 +1,39 @@
|
||||
#ifndef B3_CONFIG_H
|
||||
#define B3_CONFIG_H
|
||||
|
||||
struct b3Config
|
||||
struct b3Config
|
||||
{
|
||||
int m_maxConvexBodies;
|
||||
int m_maxConvexShapes;
|
||||
int m_maxBroadphasePairs;
|
||||
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_maxConvexVertices;
|
||||
int m_maxConvexIndices;
|
||||
int m_maxConvexUniqueEdges;
|
||||
|
||||
int m_maxCompoundChildShapes;
|
||||
|
||||
|
||||
int m_maxCompoundChildShapes;
|
||||
|
||||
int m_maxTriConvexPairCapacity;
|
||||
|
||||
b3Config()
|
||||
:m_maxConvexBodies(128*1024),
|
||||
m_maxVerticesPerFace(64),
|
||||
m_maxFacesPerShape(12),
|
||||
m_maxConvexVertices(8192),
|
||||
m_maxConvexIndices(81920),
|
||||
m_maxConvexUniqueEdges(8192),
|
||||
m_maxCompoundChildShapes(8192),
|
||||
m_maxTriConvexPairCapacity(256*1024)
|
||||
: m_maxConvexBodies(128 * 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_maxBroadphasePairs = 16 * m_maxConvexBodies;
|
||||
m_maxContactCapacity = m_maxBroadphasePairs;
|
||||
m_compoundPairCapacity = 1024*1024;
|
||||
m_compoundPairCapacity = 1024 * 1024;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif//B3_CONFIG_H
|
||||
|
||||
#endif //B3_CONFIG_H
|
||||
|
||||
@@ -19,28 +19,37 @@ subject to the following restrictions:
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3Contact4 : public b3Contact4Data
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
int getBodyA()const {return abs(m_bodyAPtrAndSignBit);}
|
||||
int getBodyB()const {return abs(m_bodyBPtrAndSignBit);}
|
||||
bool isBodyAFixed()const { return m_bodyAPtrAndSignBit<0;}
|
||||
bool isBodyBFixed()const { return m_bodyBPtrAndSignBit<0;}
|
||||
int getBodyA() const { return abs(m_bodyAPtrAndSignBit); }
|
||||
int getBodyB() const { return abs(m_bodyBPtrAndSignBit); }
|
||||
bool isBodyAFixed() const { return m_bodyAPtrAndSignBit < 0; }
|
||||
bool isBodyBFixed() const { return m_bodyBPtrAndSignBit < 0; }
|
||||
// todo. make it safer
|
||||
int& getBatchIdx() { return m_batchIdx; }
|
||||
const int& getBatchIdx() const { return m_batchIdx; }
|
||||
float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp/(float)0xffff); }
|
||||
void setRestituitionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_restituitionCoeffCmp = (unsigned short)(c*0xffff); }
|
||||
float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp/(float)0xffff); }
|
||||
void setFrictionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_frictionCoeffCmp = (unsigned short)(c*0xffff); }
|
||||
float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp / (float)0xffff); }
|
||||
void setRestituitionCoeff(float c)
|
||||
{
|
||||
b3Assert(c >= 0.f && c <= 1.f);
|
||||
m_restituitionCoeffCmp = (unsigned short)(c * 0xffff);
|
||||
}
|
||||
float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp / (float)0xffff); }
|
||||
void setFrictionCoeff(float c)
|
||||
{
|
||||
b3Assert(c >= 0.f && c <= 1.f);
|
||||
m_frictionCoeffCmp = (unsigned short)(c * 0xffff);
|
||||
}
|
||||
|
||||
//float& getNPoints() { return m_worldNormal[3]; }
|
||||
int getNPoints() const { return (int) m_worldNormalOnB.w; }
|
||||
int getNPoints() const { return (int)m_worldNormalOnB.w; }
|
||||
|
||||
float getPenetration(int idx) const { return m_worldPosB[idx].w; }
|
||||
|
||||
bool isInvalid() const { return (getBodyA()==0 || getBodyB()==0); }
|
||||
bool isInvalid() const { return (getBodyA() == 0 || getBodyB() == 0); }
|
||||
};
|
||||
|
||||
#endif //B3_CONTACT4_H
|
||||
#endif //B3_CONTACT4_H
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -20,43 +20,36 @@ subject to the following restrictions:
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Bullet3Common/b3Transform.h"
|
||||
|
||||
|
||||
|
||||
|
||||
struct b3MyFace
|
||||
{
|
||||
b3AlignedObjectArray<int> m_indices;
|
||||
b3Scalar m_plane[4];
|
||||
b3AlignedObjectArray<int> m_indices;
|
||||
b3Scalar m_plane[4];
|
||||
};
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility
|
||||
B3_ATTRIBUTE_ALIGNED16(class)
|
||||
b3ConvexUtility
|
||||
{
|
||||
public:
|
||||
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;
|
||||
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 initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles = true);
|
||||
|
||||
void initialize();
|
||||
bool testContainment() const;
|
||||
|
||||
|
||||
|
||||
};
|
||||
#endif
|
||||
|
||||
@@ -5,15 +5,13 @@
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
|
||||
|
||||
|
||||
struct b3CpuNarrowPhaseInternalData
|
||||
{
|
||||
b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
|
||||
b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
|
||||
b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
|
||||
b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
|
||||
b3Config m_config;
|
||||
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
|
||||
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
|
||||
b3AlignedObjectArray<b3Vector3> m_convexVertices;
|
||||
@@ -22,10 +20,9 @@ struct b3CpuNarrowPhaseInternalData
|
||||
|
||||
b3AlignedObjectArray<b3Contact4Data> m_contacts;
|
||||
|
||||
int m_numAcceleratedShapes;
|
||||
int m_numAcceleratedShapes;
|
||||
};
|
||||
|
||||
|
||||
const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
|
||||
{
|
||||
return m_data->m_contacts;
|
||||
@@ -41,7 +38,6 @@ const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) cons
|
||||
return m_data->m_collidablesCPU[collidableIndex];
|
||||
}
|
||||
|
||||
|
||||
b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
|
||||
{
|
||||
m_data = new b3CpuNarrowPhaseInternalData;
|
||||
@@ -61,7 +57,7 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
|
||||
int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
|
||||
m_data->m_contacts.resize(maxContactCapacity);
|
||||
|
||||
for (int i=0;i<nPairs;i++)
|
||||
for (int i = 0; i < nPairs; i++)
|
||||
{
|
||||
int bodyIndexA = pairs[i].x;
|
||||
int bodyIndexB = pairs[i].y;
|
||||
@@ -71,64 +67,57 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
// computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE)
|
||||
{
|
||||
// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
//printf("convex-sphere\n");
|
||||
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
|
||||
{
|
||||
// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("convex-plane\n");
|
||||
|
||||
// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("convex-plane\n");
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("plane-convex\n");
|
||||
|
||||
// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("plane-convex\n");
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
// computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
|
||||
// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
|
||||
// printf("convex-plane\n");
|
||||
|
||||
// computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
|
||||
// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
|
||||
// printf("convex-plane\n");
|
||||
}
|
||||
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
|
||||
{
|
||||
// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("convex-plane\n");
|
||||
|
||||
// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("convex-plane\n");
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("plane-convex\n");
|
||||
|
||||
// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("plane-convex\n");
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
||||
@@ -137,54 +126,48 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
|
||||
//printf("pairs[i].z=%d\n",pairs[i].z);
|
||||
//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
|
||||
// m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
|
||||
int contactIndex = b3ContactConvexConvexSAT(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
|
||||
m_data->m_collidablesCPU,m_data->m_convexPolyhedra,m_data->m_convexVertices,m_data->m_uniqueEdges,m_data->m_convexIndices,m_data->m_convexFaces,m_data->m_contacts,numContacts,maxContactCapacity);
|
||||
int contactIndex = b3ContactConvexConvexSAT(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, bodies,
|
||||
m_data->m_collidablesCPU, m_data->m_convexPolyhedra, m_data->m_convexVertices, m_data->m_uniqueEdges, m_data->m_convexIndices, m_data->m_convexFaces, m_data->m_contacts, numContacts, maxContactCapacity);
|
||||
|
||||
|
||||
if (contactIndex>=0)
|
||||
if (contactIndex >= 0)
|
||||
{
|
||||
pairs[i].z = contactIndex;
|
||||
}
|
||||
// printf("plane-convex\n");
|
||||
|
||||
// printf("plane-convex\n");
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
m_data->m_contacts.resize(numContacts);
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
|
||||
int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
|
||||
{
|
||||
int collidableIndex = allocateCollidable();
|
||||
if (collidableIndex<0)
|
||||
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());
|
||||
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);
|
||||
col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
|
||||
}
|
||||
|
||||
if (col.m_shapeIndex>=0)
|
||||
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++)
|
||||
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]);
|
||||
@@ -200,44 +183,42 @@ int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
|
||||
aabb.m_signedMaxIndices[3] = 0;
|
||||
|
||||
m_data->m_localShapeAABBCPU.push_back(aabb);
|
||||
|
||||
}
|
||||
|
||||
|
||||
return collidableIndex;
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::allocateCollidable()
|
||||
int b3CpuNarrowPhase::allocateCollidable()
|
||||
{
|
||||
int curSize = m_data->m_collidablesCPU.size();
|
||||
if (curSize<m_data->m_config.m_maxConvexShapes)
|
||||
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);
|
||||
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)
|
||||
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++)
|
||||
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]));
|
||||
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);
|
||||
utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
|
||||
}
|
||||
|
||||
int collidableIndex = registerConvexHullShape(utilPtr);
|
||||
@@ -246,74 +227,67 @@ int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideI
|
||||
return collidableIndex;
|
||||
}
|
||||
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col)
|
||||
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);
|
||||
|
||||
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);
|
||||
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_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);
|
||||
|
||||
|
||||
m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges);
|
||||
|
||||
//convex data here
|
||||
int i;
|
||||
for ( i=0;i<convexPtr->m_uniqueEdges.size();i++)
|
||||
for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++)
|
||||
{
|
||||
m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[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);
|
||||
|
||||
m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
|
||||
|
||||
for (i=0;i<convexPtr->m_faces.size();i++)
|
||||
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]);
|
||||
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_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];
|
||||
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++)
|
||||
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_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i];
|
||||
}
|
||||
|
||||
(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
|
||||
|
||||
|
||||
|
||||
|
||||
return m_data->m_numAcceleratedShapes++;
|
||||
}
|
||||
|
||||
|
||||
@@ -12,66 +12,55 @@
|
||||
class b3CpuNarrowPhase
|
||||
{
|
||||
protected:
|
||||
|
||||
struct b3CpuNarrowPhaseInternalData* m_data;
|
||||
struct b3CpuNarrowPhaseInternalData* m_data;
|
||||
int m_acceleratedCompanionShapeIndex;
|
||||
int m_planeBodyIndex;
|
||||
int m_static0Index;
|
||||
int m_static0Index;
|
||||
|
||||
int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr,b3Collidable& col);
|
||||
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 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);
|
||||
|
||||
|
||||
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 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 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 writeAllBodiesToGpu();
|
||||
void reset();
|
||||
void readbackAllBodiesToCpu();
|
||||
bool getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const;
|
||||
|
||||
void setObjectTransformCpu(float* position, float* orientation , int bodyIndex);
|
||||
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>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies);
|
||||
|
||||
|
||||
|
||||
const struct b3RigidBodyData* getBodiesCpu() const;
|
||||
//struct b3RigidBodyData* getBodiesCpu();
|
||||
|
||||
int getNumBodiesGpu() const;
|
||||
int getNumBodiesGpu() const;
|
||||
|
||||
|
||||
int getNumBodyInertiasGpu() const;
|
||||
int getNumBodyInertiasGpu() const;
|
||||
|
||||
|
||||
const struct b3Collidable* getCollidablesCpu() const;
|
||||
int getNumCollidablesGpu() const;
|
||||
|
||||
int getNumCollidablesGpu() const;
|
||||
|
||||
/*const struct b3Contact4* getContactsCPU() const;
|
||||
|
||||
@@ -80,8 +69,7 @@ public:
|
||||
*/
|
||||
|
||||
const b3AlignedObjectArray<b3Contact4Data>& getContacts() const;
|
||||
|
||||
|
||||
|
||||
int getNumRigidBodies() const;
|
||||
|
||||
int allocateCollidable();
|
||||
@@ -93,13 +81,12 @@ public:
|
||||
b3Collidable& getCollidableCpu(int collidableIndex);
|
||||
const b3Collidable& getCollidableCpu(int collidableIndex) const;
|
||||
|
||||
const b3CpuNarrowPhaseInternalData* getInternalData() const
|
||||
const b3CpuNarrowPhaseInternalData* getInternalData() const
|
||||
{
|
||||
return m_data;
|
||||
return m_data;
|
||||
}
|
||||
|
||||
const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const;
|
||||
};
|
||||
|
||||
#endif //B3_CPU_NARROWPHASE_H
|
||||
|
||||
#endif //B3_CPU_NARROWPHASE_H
|
||||
|
||||
@@ -4,21 +4,22 @@
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct) b3RayInfo
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3RayInfo
|
||||
{
|
||||
b3Vector3 m_from;
|
||||
b3Vector3 m_to;
|
||||
};
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3RayHit
|
||||
{
|
||||
b3Scalar m_hitFraction;
|
||||
int m_hitBody;
|
||||
int m_hitResult1;
|
||||
int m_hitResult2;
|
||||
b3Vector3 m_hitPoint;
|
||||
b3Vector3 m_hitNormal;
|
||||
b3Scalar m_hitFraction;
|
||||
int m_hitBody;
|
||||
int m_hitResult1;
|
||||
int m_hitResult2;
|
||||
b3Vector3 m_hitPoint;
|
||||
b3Vector3 m_hitNormal;
|
||||
};
|
||||
|
||||
#endif //B3_RAYCAST_INFO_H
|
||||
|
||||
#endif //B3_RAYCAST_INFO_H
|
||||
|
||||
@@ -20,11 +20,9 @@ subject to the following restrictions:
|
||||
#include "Bullet3Common/b3Matrix3x3.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
|
||||
inline float b3GetInvMass(const b3RigidBodyData& body)
|
||||
inline float b3GetInvMass(const b3RigidBodyData& body)
|
||||
{
|
||||
return body.m_invMass;
|
||||
return body.m_invMass;
|
||||
}
|
||||
|
||||
|
||||
#endif//B3_RIGID_BODY_CL
|
||||
#endif //B3_RIGID_BODY_CL
|
||||
|
||||
@@ -7,14 +7,13 @@ typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
|
||||
struct b3BvhSubtreeInfoData
|
||||
{
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes, points to the root of the subtree
|
||||
int m_rootNodeIndex;
|
||||
int m_rootNodeIndex;
|
||||
//4 bytes
|
||||
int m_subtreeSize;
|
||||
int m_padding[3];
|
||||
int m_subtreeSize;
|
||||
int m_padding[3];
|
||||
};
|
||||
|
||||
#endif //B3_BVH_SUBTREE_INFO_DATA_H
|
||||
|
||||
#endif //B3_BVH_SUBTREE_INFO_DATA_H
|
||||
|
||||
@@ -7,69 +7,64 @@
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
|
||||
|
||||
|
||||
// work-in-progress
|
||||
void b3BvhTraversal( __global const b3Int4* pairs,
|
||||
__global const b3RigidBodyData* rigidBodies,
|
||||
__global const b3Collidable* collidables,
|
||||
__global b3Aabb* aabbs,
|
||||
__global b3Int4* concavePairsOut,
|
||||
__global volatile int* numConcavePairsOut,
|
||||
__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
|
||||
__global const b3QuantizedBvhNode* quantizedNodesRoot,
|
||||
__global const b3BvhInfo* bvhInfos,
|
||||
int numPairs,
|
||||
int maxNumConcavePairsCapacity,
|
||||
int id)
|
||||
void b3BvhTraversal(__global const b3Int4* pairs,
|
||||
__global const b3RigidBodyData* rigidBodies,
|
||||
__global const b3Collidable* collidables,
|
||||
__global b3Aabb* aabbs,
|
||||
__global b3Int4* concavePairsOut,
|
||||
__global volatile int* numConcavePairsOut,
|
||||
__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
|
||||
__global const b3QuantizedBvhNode* quantizedNodesRoot,
|
||||
__global const b3BvhInfo* bvhInfos,
|
||||
int numPairs,
|
||||
int maxNumConcavePairsCapacity,
|
||||
int id)
|
||||
{
|
||||
|
||||
int bodyIndexA = pairs[id].x;
|
||||
int bodyIndexB = pairs[id].y;
|
||||
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
|
||||
//once the broadphase avoids static-static pairs, we can remove this test
|
||||
if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
|
||||
if ((rigidBodies[bodyIndexA].m_invMass == 0) && (rigidBodies[bodyIndexB].m_invMass == 0))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)
|
||||
|
||||
if (collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH)
|
||||
return;
|
||||
|
||||
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
|
||||
|
||||
if (shapeTypeB!=SHAPE_CONVEX_HULL &&
|
||||
shapeTypeB!=SHAPE_SPHERE &&
|
||||
shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS
|
||||
)
|
||||
|
||||
if (shapeTypeB != SHAPE_CONVEX_HULL &&
|
||||
shapeTypeB != SHAPE_SPHERE &&
|
||||
shapeTypeB != SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
return;
|
||||
|
||||
b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
|
||||
|
||||
b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
|
||||
b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
|
||||
b3Float4 bvhQuantization = bvhInfo.m_quantization;
|
||||
b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
|
||||
b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
|
||||
b3Float4 bvhQuantization = bvhInfo.m_quantization;
|
||||
int numSubtreeHeaders = bvhInfo.m_numSubTrees;
|
||||
__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
|
||||
__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
|
||||
|
||||
|
||||
unsigned short int quantizedQueryAabbMin[3];
|
||||
unsigned short int quantizedQueryAabbMax[3];
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization);
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);
|
||||
|
||||
for (int i=0;i<numSubtreeHeaders;i++)
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMin, aabbs[bodyIndexB].m_minVec, false, bvhAabbMin, bvhAabbMax, bvhQuantization);
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMax, aabbs[bodyIndexB].m_maxVec, true, bvhAabbMin, bvhAabbMax, bvhQuantization);
|
||||
|
||||
for (int i = 0; i < numSubtreeHeaders; i++)
|
||||
{
|
||||
b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
|
||||
|
||||
int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
|
||||
|
||||
int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax);
|
||||
if (overlap != 0)
|
||||
{
|
||||
int startNodeIndex = subtree.m_rootNodeIndex;
|
||||
int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize;
|
||||
int endNodeIndex = subtree.m_rootNodeIndex + subtree.m_subtreeSize;
|
||||
int curIndex = startNodeIndex;
|
||||
int escapeIndex;
|
||||
int isLeafNode;
|
||||
@@ -77,43 +72,46 @@ void b3BvhTraversal( __global const b3Int4* pairs,
|
||||
while (curIndex < endNodeIndex)
|
||||
{
|
||||
b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
|
||||
aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax);
|
||||
aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.m_quantizedAabbMin, rootNode.m_quantizedAabbMax);
|
||||
isLeafNode = b3IsLeaf(&rootNode);
|
||||
if (aabbOverlap)
|
||||
{
|
||||
if (isLeafNode)
|
||||
{
|
||||
int triangleIndex = b3GetTriangleIndex(&rootNode);
|
||||
if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
if (shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
|
||||
int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB);
|
||||
for (int b=0;b<numChildrenB;b++)
|
||||
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
|
||||
int pairIdx = b3AtomicAdd(numConcavePairsOut, numChildrenB);
|
||||
for (int b = 0; b < numChildrenB; b++)
|
||||
{
|
||||
if ((pairIdx + b) < maxNumConcavePairsCapacity)
|
||||
{
|
||||
if ((pairIdx+b)<maxNumConcavePairsCapacity)
|
||||
{
|
||||
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b;
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB);
|
||||
concavePairsOut[pairIdx+b] = newPair;
|
||||
}
|
||||
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, childShapeIndexB);
|
||||
concavePairsOut[pairIdx + b] = newPair;
|
||||
}
|
||||
} else
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int pairIdx = b3AtomicInc(numConcavePairsOut);
|
||||
if (pairIdx<maxNumConcavePairsCapacity)
|
||||
if (pairIdx < maxNumConcavePairsCapacity)
|
||||
{
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0);
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, 0);
|
||||
concavePairsOut[pairIdx] = newPair;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
curIndex++;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
if (isLeafNode)
|
||||
{
|
||||
curIndex++;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
escapeIndex = b3GetEscapeIndex(&rootNode);
|
||||
curIndex += escapeIndex;
|
||||
@@ -122,5 +120,4 @@ void b3BvhTraversal( __global const b3Int4* pairs,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,7 +1,6 @@
|
||||
#ifndef B3_CLIP_FACES_H
|
||||
#define B3_CLIP_FACES_H
|
||||
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
@@ -10,38 +9,36 @@
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
|
||||
|
||||
inline b3Float4 b3Lerp3(b3Float4ConstArg a,b3Float4ConstArg b, float t)
|
||||
inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
|
||||
{
|
||||
return b3MakeFloat4( a.x + (b.x - a.x) * t,
|
||||
return b3MakeFloat4(a.x + (b.x - a.x) * t,
|
||||
a.y + (b.y - a.y) * t,
|
||||
a.z + (b.z - a.z) * t,
|
||||
0.f);
|
||||
}
|
||||
|
||||
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
|
||||
int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS,float planeEqWS, __global b3Float4* ppVtxOut)
|
||||
int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut)
|
||||
{
|
||||
|
||||
int ve;
|
||||
float ds, de;
|
||||
int numVertsOut = 0;
|
||||
//double-check next test
|
||||
// if (numVertsIn < 2)
|
||||
// return 0;
|
||||
|
||||
b3Float4 firstVertex=pVtxIn[numVertsIn-1];
|
||||
//double-check next test
|
||||
// if (numVertsIn < 2)
|
||||
// return 0;
|
||||
|
||||
b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
|
||||
b3Float4 endVertex = pVtxIn[0];
|
||||
|
||||
ds = b3Dot(planeNormalWS,firstVertex)+planeEqWS;
|
||||
|
||||
|
||||
ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS;
|
||||
|
||||
for (ve = 0; ve < numVertsIn; ve++)
|
||||
{
|
||||
endVertex=pVtxIn[ve];
|
||||
de = b3Dot(planeNormalWS,endVertex)+planeEqWS;
|
||||
if (ds<0)
|
||||
endVertex = pVtxIn[ve];
|
||||
de = b3Dot(planeNormalWS, endVertex) + planeEqWS;
|
||||
if (ds < 0)
|
||||
{
|
||||
if (de<0)
|
||||
if (de < 0)
|
||||
{
|
||||
// Start < 0, end < 0, so output endVertex
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
@@ -49,15 +46,15 @@ int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4Cons
|
||||
else
|
||||
{
|
||||
// Start < 0, end >= 0, so output intersection
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (de<0)
|
||||
if (de < 0)
|
||||
{
|
||||
// Start >= 0, end < 0 so output intersection and end
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
}
|
||||
@@ -67,90 +64,81 @@ int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4Cons
|
||||
return numVertsOut;
|
||||
}
|
||||
|
||||
|
||||
__kernel void clipFacesAndFindContactsKernel( __global const b3Float4* separatingNormals,
|
||||
__global const int* hasSeparatingAxis,
|
||||
__global b3Int4* clippingFacesOut,
|
||||
__global b3Float4* worldVertsA1,
|
||||
__global b3Float4* worldNormalsA1,
|
||||
__global b3Float4* worldVertsB1,
|
||||
__global b3Float4* worldVertsB2,
|
||||
int vertexFaceCapacity,
|
||||
int pairIndex
|
||||
)
|
||||
__kernel void clipFacesAndFindContactsKernel(__global const b3Float4* separatingNormals,
|
||||
__global const int* hasSeparatingAxis,
|
||||
__global b3Int4* clippingFacesOut,
|
||||
__global b3Float4* worldVertsA1,
|
||||
__global b3Float4* worldNormalsA1,
|
||||
__global b3Float4* worldVertsB1,
|
||||
__global b3Float4* worldVertsB2,
|
||||
int vertexFaceCapacity,
|
||||
int pairIndex)
|
||||
{
|
||||
// int i = get_global_id(0);
|
||||
// int i = get_global_id(0);
|
||||
//int pairIndex = i;
|
||||
int i = pairIndex;
|
||||
|
||||
|
||||
float minDist = -1e30f;
|
||||
float maxDist = 0.02f;
|
||||
|
||||
// if (i<numPairs)
|
||||
|
||||
// if (i<numPairs)
|
||||
{
|
||||
|
||||
if (hasSeparatingAxis[i])
|
||||
{
|
||||
|
||||
// int bodyIndexA = pairs[i].x;
|
||||
// int bodyIndexB = pairs[i].y;
|
||||
|
||||
int numLocalContactsOut = 0;
|
||||
// int bodyIndexA = pairs[i].x;
|
||||
// int bodyIndexB = pairs[i].y;
|
||||
|
||||
int capacityWorldVertsB2 = vertexFaceCapacity;
|
||||
|
||||
__global b3Float4* pVtxIn = &worldVertsB1[pairIndex*capacityWorldVertsB2];
|
||||
__global b3Float4* pVtxOut = &worldVertsB2[pairIndex*capacityWorldVertsB2];
|
||||
|
||||
int numLocalContactsOut = 0;
|
||||
|
||||
{
|
||||
__global b3Int4* clippingFaces = clippingFacesOut;
|
||||
|
||||
|
||||
int closestFaceA = clippingFaces[pairIndex].x;
|
||||
// int closestFaceB = clippingFaces[pairIndex].y;
|
||||
int numVertsInA = clippingFaces[pairIndex].z;
|
||||
int numVertsInB = clippingFaces[pairIndex].w;
|
||||
|
||||
int numVertsOut = 0;
|
||||
|
||||
if (closestFaceA>=0)
|
||||
{
|
||||
|
||||
|
||||
|
||||
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
|
||||
|
||||
for(int e0=0;e0<numVertsInA;e0++)
|
||||
{
|
||||
const b3Float4 aw = worldVertsA1[pairIndex*capacityWorldVertsB2+e0];
|
||||
const b3Float4 bw = worldVertsA1[pairIndex*capacityWorldVertsB2+((e0+1)%numVertsInA)];
|
||||
const b3Float4 WorldEdge0 = aw - bw;
|
||||
b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
|
||||
b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0,worldPlaneAnormal1);
|
||||
b3Float4 worldA1 = aw;
|
||||
float planeEqWS1 = -b3Dot(worldA1,planeNormalWS1);
|
||||
b3Float4 planeNormalWS = planeNormalWS1;
|
||||
float planeEqWS=planeEqWS1;
|
||||
numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS,planeEqWS, pVtxOut);
|
||||
__global b3Float4* tmp = pVtxOut;
|
||||
pVtxOut = pVtxIn;
|
||||
pVtxIn = tmp;
|
||||
numVertsInB = numVertsOut;
|
||||
numVertsOut = 0;
|
||||
}
|
||||
|
||||
b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
|
||||
float planeEqWS=-b3Dot(planeNormalWS,worldVertsA1[pairIndex*capacityWorldVertsB2]);
|
||||
|
||||
for (int i=0;i<numVertsInB;i++)
|
||||
{
|
||||
float depth = b3Dot(planeNormalWS,pVtxIn[i])+planeEqWS;
|
||||
if (depth <=minDist)
|
||||
{
|
||||
depth = minDist;
|
||||
}
|
||||
/*
|
||||
int capacityWorldVertsB2 = vertexFaceCapacity;
|
||||
|
||||
__global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
|
||||
__global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];
|
||||
|
||||
{
|
||||
__global b3Int4* clippingFaces = clippingFacesOut;
|
||||
|
||||
int closestFaceA = clippingFaces[pairIndex].x;
|
||||
// int closestFaceB = clippingFaces[pairIndex].y;
|
||||
int numVertsInA = clippingFaces[pairIndex].z;
|
||||
int numVertsInB = clippingFaces[pairIndex].w;
|
||||
|
||||
int numVertsOut = 0;
|
||||
|
||||
if (closestFaceA >= 0)
|
||||
{
|
||||
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
|
||||
|
||||
for (int e0 = 0; e0 < numVertsInA; e0++)
|
||||
{
|
||||
const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0];
|
||||
const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)];
|
||||
const b3Float4 WorldEdge0 = aw - bw;
|
||||
b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
|
||||
b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1);
|
||||
b3Float4 worldA1 = aw;
|
||||
float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1);
|
||||
b3Float4 planeNormalWS = planeNormalWS1;
|
||||
float planeEqWS = planeEqWS1;
|
||||
numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut);
|
||||
__global b3Float4* tmp = pVtxOut;
|
||||
pVtxOut = pVtxIn;
|
||||
pVtxIn = tmp;
|
||||
numVertsInB = numVertsOut;
|
||||
numVertsOut = 0;
|
||||
}
|
||||
|
||||
b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
|
||||
float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]);
|
||||
|
||||
for (int i = 0; i < numVertsInB; i++)
|
||||
{
|
||||
float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
|
||||
if (depth <= minDist)
|
||||
{
|
||||
depth = minDist;
|
||||
}
|
||||
/*
|
||||
static float maxDepth = 0.f;
|
||||
if (depth < maxDepth)
|
||||
{
|
||||
@@ -163,26 +151,21 @@ __kernel void clipFacesAndFindContactsKernel( __global const b3Float4* sepa
|
||||
|
||||
}
|
||||
*/
|
||||
if (depth <=maxDist)
|
||||
{
|
||||
b3Float4 pointInWorld = pVtxIn[i];
|
||||
pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
clippingFaces[pairIndex].w =numLocalContactsOut;
|
||||
|
||||
if (depth <= maxDist)
|
||||
{
|
||||
b3Float4 pointInWorld = pVtxIn[i];
|
||||
pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
|
||||
}
|
||||
}
|
||||
}
|
||||
clippingFaces[pairIndex].w = numLocalContactsOut;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
for (int i=0;i<numLocalContactsOut;i++)
|
||||
pVtxIn[i] = pVtxOut[i];
|
||||
|
||||
}// if (hasSeparatingAxis[i])
|
||||
}// if (i<numPairs)
|
||||
|
||||
for (int i = 0; i < numLocalContactsOut; i++)
|
||||
pVtxIn[i] = pVtxOut[i];
|
||||
|
||||
} // if (hasSeparatingAxis[i])
|
||||
} // if (i<numPairs)
|
||||
}
|
||||
|
||||
#endif //B3_CLIP_FACES_H
|
||||
|
||||
#endif //B3_CLIP_FACES_H
|
||||
|
||||
@@ -2,40 +2,36 @@
|
||||
#ifndef B3_COLLIDABLE_H
|
||||
#define B3_COLLIDABLE_H
|
||||
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Common/shared/b3Quat.h"
|
||||
|
||||
enum b3ShapeTypes
|
||||
{
|
||||
SHAPE_HEIGHT_FIELD=1,
|
||||
SHAPE_HEIGHT_FIELD = 1,
|
||||
|
||||
SHAPE_CONVEX_HULL=3,
|
||||
SHAPE_PLANE=4,
|
||||
SHAPE_CONCAVE_TRIMESH=5,
|
||||
SHAPE_COMPOUND_OF_CONVEX_HULLS=6,
|
||||
SHAPE_SPHERE=7,
|
||||
SHAPE_CONVEX_HULL = 3,
|
||||
SHAPE_PLANE = 4,
|
||||
SHAPE_CONCAVE_TRIMESH = 5,
|
||||
SHAPE_COMPOUND_OF_CONVEX_HULLS = 6,
|
||||
SHAPE_SPHERE = 7,
|
||||
MAX_NUM_SHAPE_TYPES,
|
||||
};
|
||||
|
||||
typedef struct b3Collidable b3Collidable_t;
|
||||
|
||||
|
||||
struct b3Collidable
|
||||
{
|
||||
union {
|
||||
int m_numChildShapes;
|
||||
int m_bvhIndex;
|
||||
};
|
||||
union
|
||||
{
|
||||
union {
|
||||
float m_radius;
|
||||
int m_compoundBvhIndex;
|
||||
int m_compoundBvhIndex;
|
||||
};
|
||||
|
||||
int m_shapeType;
|
||||
union
|
||||
{
|
||||
union {
|
||||
int m_shapeIndex;
|
||||
float m_height;
|
||||
};
|
||||
@@ -44,33 +40,30 @@ struct b3Collidable
|
||||
typedef struct b3GpuChildShape b3GpuChildShape_t;
|
||||
struct b3GpuChildShape
|
||||
{
|
||||
b3Float4 m_childPosition;
|
||||
b3Quat m_childOrientation;
|
||||
union
|
||||
{
|
||||
int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS
|
||||
int m_capsuleAxis;
|
||||
b3Float4 m_childPosition;
|
||||
b3Quat m_childOrientation;
|
||||
union {
|
||||
int m_shapeIndex; //used for SHAPE_COMPOUND_OF_CONVEX_HULLS
|
||||
int m_capsuleAxis;
|
||||
};
|
||||
union
|
||||
{
|
||||
float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
|
||||
int m_numChildShapes;//used for compound shape
|
||||
union {
|
||||
float m_radius; //used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
|
||||
int m_numChildShapes; //used for compound shape
|
||||
};
|
||||
union
|
||||
{
|
||||
float m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES
|
||||
int m_collidableShapeIndex;
|
||||
union {
|
||||
float m_height; //used for childshape of SHAPE_COMPOUND_OF_CAPSULES
|
||||
int m_collidableShapeIndex;
|
||||
};
|
||||
int m_shapeType;
|
||||
int m_shapeType;
|
||||
};
|
||||
|
||||
struct b3CompoundOverlappingPair
|
||||
{
|
||||
int m_bodyIndexA;
|
||||
int m_bodyIndexB;
|
||||
// int m_pairType;
|
||||
// int m_pairType;
|
||||
int m_childShapeIndexA;
|
||||
int m_childShapeIndexB;
|
||||
};
|
||||
|
||||
#endif //B3_COLLIDABLE_H
|
||||
#endif //B3_COLLIDABLE_H
|
||||
|
||||
@@ -3,26 +3,24 @@
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
|
||||
typedef struct b3Contact4Data b3Contact4Data_t;
|
||||
typedef struct b3Contact4Data b3Contact4Data_t;
|
||||
|
||||
struct b3Contact4Data
|
||||
{
|
||||
b3Float4 m_worldPosB[4];
|
||||
// b3Float4 m_localPosA[4];
|
||||
// b3Float4 m_localPosB[4];
|
||||
b3Float4 m_worldNormalOnB; // w: m_nPoints
|
||||
unsigned short m_restituitionCoeffCmp;
|
||||
unsigned short m_frictionCoeffCmp;
|
||||
b3Float4 m_worldPosB[4];
|
||||
// b3Float4 m_localPosA[4];
|
||||
// b3Float4 m_localPosB[4];
|
||||
b3Float4 m_worldNormalOnB; // w: m_nPoints
|
||||
unsigned short m_restituitionCoeffCmp;
|
||||
unsigned short m_frictionCoeffCmp;
|
||||
int m_batchIdx;
|
||||
int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr
|
||||
int m_bodyAPtrAndSignBit; //x:m_bodyAPtr, y:m_bodyBPtr
|
||||
int m_bodyBPtrAndSignBit;
|
||||
|
||||
int m_childIndexA;
|
||||
int m_childIndexB;
|
||||
int m_childIndexA;
|
||||
int m_childIndexB;
|
||||
int m_unused1;
|
||||
int m_unused2;
|
||||
|
||||
|
||||
};
|
||||
|
||||
inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
|
||||
@@ -35,6 +33,4 @@ inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numP
|
||||
contact->m_worldNormalOnB.w = (float)numPoints;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //B3_CONTACT4DATA_H
|
||||
#endif //B3_CONTACT4DATA_H
|
||||
@@ -2,48 +2,43 @@
|
||||
#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
|
||||
#define B3_CONTACT_CONVEX_CONVEX_SAT_H
|
||||
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
|
||||
|
||||
#define B3_MAX_VERTS 1024
|
||||
|
||||
|
||||
|
||||
inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float t)
|
||||
inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float t)
|
||||
{
|
||||
return b3MakeVector3( a.x + (b.x - a.x) * t,
|
||||
a.y + (b.y - a.y) * t,
|
||||
a.z + (b.z - a.z) * t,
|
||||
0.f);
|
||||
return b3MakeVector3(a.x + (b.x - a.x) * t,
|
||||
a.y + (b.y - a.y) * t,
|
||||
a.z + (b.z - a.z) * t,
|
||||
0.f);
|
||||
}
|
||||
|
||||
|
||||
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
|
||||
inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS,float planeEqWS, b3Float4* ppVtxOut)
|
||||
inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
|
||||
{
|
||||
|
||||
int ve;
|
||||
float ds, de;
|
||||
int numVertsOut = 0;
|
||||
if (numVertsIn < 2)
|
||||
return 0;
|
||||
|
||||
b3Float4 firstVertex=pVtxIn[numVertsIn-1];
|
||||
b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
|
||||
b3Float4 endVertex = pVtxIn[0];
|
||||
|
||||
ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS;
|
||||
|
||||
ds = b3Dot3F4(planeNormalWS, firstVertex) + planeEqWS;
|
||||
|
||||
for (ve = 0; ve < numVertsIn; ve++)
|
||||
{
|
||||
endVertex=pVtxIn[ve];
|
||||
endVertex = pVtxIn[ve];
|
||||
|
||||
de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS;
|
||||
de = b3Dot3F4(planeNormalWS, endVertex) + planeEqWS;
|
||||
|
||||
if (ds<0)
|
||||
if (ds < 0)
|
||||
{
|
||||
if (de<0)
|
||||
if (de < 0)
|
||||
{
|
||||
// Start < 0, end < 0, so output endVertex
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
@@ -51,15 +46,15 @@ inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNor
|
||||
else
|
||||
{
|
||||
// Start < 0, end >= 0, so output intersection
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (de<0)
|
||||
if (de < 0)
|
||||
{
|
||||
// Start >= 0, end < 0 so output intersection and end
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
}
|
||||
@@ -69,36 +64,35 @@ inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNor
|
||||
return numVertsOut;
|
||||
}
|
||||
|
||||
|
||||
inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
|
||||
const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
|
||||
b3Float4* worldVertsB2, int capacityWorldVertsB2,
|
||||
const float minDist, float maxDist,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
|
||||
//const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
|
||||
b3Float4* contactsOut,
|
||||
int contactCapacity)
|
||||
inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
|
||||
const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
|
||||
b3Float4* worldVertsB2, int capacityWorldVertsB2,
|
||||
const float minDist, float maxDist,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
|
||||
//const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
|
||||
b3Float4* contactsOut,
|
||||
int contactCapacity)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
|
||||
b3Float4* pVtxIn = worldVertsB1;
|
||||
b3Float4* pVtxOut = worldVertsB2;
|
||||
|
||||
|
||||
int numVertsIn = numWorldVertsB1;
|
||||
int numVertsOut = 0;
|
||||
|
||||
int closestFaceA=-1;
|
||||
int closestFaceA = -1;
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
for(int face=0;face<hullA->m_numFaces;face++)
|
||||
for (int face = 0; face < hullA->m_numFaces; face++)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeVector3(
|
||||
facesA[hullA->m_faceOffset+face].m_plane.x,
|
||||
facesA[hullA->m_faceOffset+face].m_plane.y,
|
||||
facesA[hullA->m_faceOffset+face].m_plane.z,0.f);
|
||||
const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
|
||||
|
||||
float d = b3Dot3F4(faceANormalWS,separatingNormal);
|
||||
facesA[hullA->m_faceOffset + face].m_plane.x,
|
||||
facesA[hullA->m_faceOffset + face].m_plane.y,
|
||||
facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
|
||||
const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
|
||||
|
||||
float d = b3Dot3F4(faceANormalWS, separatingNormal);
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
@@ -106,33 +100,33 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
|
||||
}
|
||||
}
|
||||
}
|
||||
if (closestFaceA<0)
|
||||
if (closestFaceA < 0)
|
||||
return numContactsOut;
|
||||
|
||||
b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA];
|
||||
b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
|
||||
|
||||
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
|
||||
//int numContacts = numWorldVertsB1;
|
||||
int numVerticesA = polyA.m_numIndices;
|
||||
for(int e0=0;e0<numVerticesA;e0++)
|
||||
for (int e0 = 0; e0 < numVerticesA; e0++)
|
||||
{
|
||||
const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+e0]];
|
||||
const b3Float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]];
|
||||
const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
|
||||
const b3Float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
|
||||
const b3Float4 edge0 = a - b;
|
||||
const b3Float4 WorldEdge0 = b3QuatRotate(ornA,edge0);
|
||||
b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
|
||||
b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA);
|
||||
const b3Float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
|
||||
b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
|
||||
b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
|
||||
|
||||
b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0, worldPlaneAnormal1);
|
||||
b3Float4 worldA1 = b3TransformPoint(a, posA, ornA);
|
||||
float planeEqWS1 = -b3Dot3F4(worldA1, planeNormalWS1);
|
||||
|
||||
b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1);
|
||||
b3Float4 worldA1 = b3TransformPoint(a,posA,ornA);
|
||||
float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1);
|
||||
|
||||
b3Float4 planeNormalWS = planeNormalWS1;
|
||||
float planeEqWS=planeEqWS1;
|
||||
|
||||
float planeEqWS = planeEqWS1;
|
||||
|
||||
//clip face
|
||||
//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
|
||||
numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut);
|
||||
numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
|
||||
|
||||
//btSwap(pVtxIn,pVtxOut);
|
||||
b3Float4* tmp = pVtxOut;
|
||||
@@ -142,32 +136,32 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
|
||||
numVertsOut = 0;
|
||||
}
|
||||
|
||||
|
||||
// only keep points that are behind the witness face
|
||||
{
|
||||
b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
|
||||
b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
|
||||
float localPlaneEq = polyA.m_plane.w;
|
||||
b3Float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal);
|
||||
float planeEqWS=localPlaneEq-b3Dot3F4(planeNormalWS,posA);
|
||||
for (int i=0;i<numVertsIn;i++)
|
||||
b3Float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
|
||||
float planeEqWS = localPlaneEq - b3Dot3F4(planeNormalWS, posA);
|
||||
for (int i = 0; i < numVertsIn; i++)
|
||||
{
|
||||
float depth = b3Dot3F4(planeNormalWS,pVtxIn[i])+planeEqWS;
|
||||
if (depth <=minDist)
|
||||
float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
|
||||
if (depth <= minDist)
|
||||
{
|
||||
depth = minDist;
|
||||
}
|
||||
if (numContactsOut<contactCapacity)
|
||||
if (numContactsOut < contactCapacity)
|
||||
{
|
||||
if (depth <=maxDist)
|
||||
if (depth <= maxDist)
|
||||
{
|
||||
b3Float4 pointInWorld = pVtxIn[i];
|
||||
//resultOut.addContactPoint(separatingNormal,point,depth);
|
||||
contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
|
||||
contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
|
||||
//printf("depth=%f\n",depth);
|
||||
}
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut,contactCapacity);
|
||||
b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -175,62 +169,60 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
|
||||
return numContactsOut;
|
||||
}
|
||||
|
||||
inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
|
||||
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
|
||||
b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
|
||||
const float minDist, float maxDist,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
|
||||
inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
|
||||
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA, const b3Quaternion& ornA,const b3Float4& posB, const b3Quaternion& ornB,
|
||||
b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
|
||||
const float minDist, float maxDist,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
b3Float4* contactsOut,
|
||||
int contactCapacity)
|
||||
b3Float4* contactsOut,
|
||||
int contactCapacity)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
int numWorldVertsB1= 0;
|
||||
|
||||
int numWorldVertsB1 = 0;
|
||||
|
||||
B3_PROFILE("clipHullAgainstHull");
|
||||
|
||||
//float curMaxDist=maxDist;
|
||||
int closestFaceB=-1;
|
||||
int closestFaceB = -1;
|
||||
float dmax = -FLT_MAX;
|
||||
|
||||
{
|
||||
//B3_PROFILE("closestFaceB");
|
||||
if (hullB.m_numFaces!=1)
|
||||
if (hullB.m_numFaces != 1)
|
||||
{
|
||||
//printf("wtf\n");
|
||||
}
|
||||
static bool once = true;
|
||||
//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
|
||||
|
||||
for(int face=0;face<hullB.m_numFaces;face++)
|
||||
|
||||
for (int face = 0; face < hullB.m_numFaces; face++)
|
||||
{
|
||||
#ifdef BT_DEBUG_SAT_FACE
|
||||
if (once)
|
||||
printf("face %d\n",face);
|
||||
const b3GpuFace* faceB = &facesB[hullB.m_faceOffset+face];
|
||||
printf("face %d\n", face);
|
||||
const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
|
||||
if (once)
|
||||
{
|
||||
for (int i=0;i<faceB->m_numIndices;i++)
|
||||
for (int i = 0; i < faceB->m_numIndices; i++)
|
||||
{
|
||||
b3Float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]];
|
||||
printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z);
|
||||
b3Float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
|
||||
printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
|
||||
}
|
||||
}
|
||||
#endif //BT_DEBUG_SAT_FACE
|
||||
//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
|
||||
#endif //BT_DEBUG_SAT_FACE \
|
||||
//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x,
|
||||
facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f);
|
||||
const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
|
||||
facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
|
||||
const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
|
||||
#ifdef BT_DEBUG_SAT_FACE
|
||||
if (once)
|
||||
printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z);
|
||||
printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
|
||||
#endif
|
||||
float d = b3Dot3F4(WorldNormal,separatingNormal);
|
||||
float d = b3Dot3F4(WorldNormal, separatingNormal);
|
||||
if (d > dmax)
|
||||
{
|
||||
dmax = d;
|
||||
@@ -241,79 +233,73 @@ inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
|
||||
once = false;
|
||||
}
|
||||
|
||||
|
||||
b3Assert(closestFaceB>=0);
|
||||
b3Assert(closestFaceB >= 0);
|
||||
{
|
||||
//B3_PROFILE("worldVertsB1");
|
||||
const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB];
|
||||
const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
|
||||
const int numVertices = polyB.m_numIndices;
|
||||
for(int e0=0;e0<numVertices;e0++)
|
||||
for (int e0 = 0; e0 < numVertices; e0++)
|
||||
{
|
||||
const b3Float4& b = verticesB[hullB.m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
|
||||
worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
|
||||
const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
|
||||
worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
|
||||
}
|
||||
}
|
||||
|
||||
if (closestFaceB>=0)
|
||||
if (closestFaceB >= 0)
|
||||
{
|
||||
//B3_PROFILE("clipFaceAgainstHull");
|
||||
numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
|
||||
posA,ornA,
|
||||
worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist,
|
||||
verticesA, facesA, indicesA,
|
||||
contactsOut,contactCapacity);
|
||||
numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
|
||||
posA, ornA,
|
||||
worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
|
||||
verticesA, facesA, indicesA,
|
||||
contactsOut, contactCapacity);
|
||||
}
|
||||
|
||||
return numContactsOut;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
inline int b3ClipHullHullSingle(
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
const b3Float4& posA,
|
||||
const b3Quaternion& ornA,
|
||||
const b3Float4& posB,
|
||||
const b3Quaternion& ornB,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
const b3Float4& posA,
|
||||
const b3Quaternion& ornA,
|
||||
const b3Float4& posB,
|
||||
const b3Quaternion& ornB,
|
||||
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
|
||||
const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
|
||||
b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
|
||||
int& nContacts,
|
||||
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
|
||||
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesA,
|
||||
const b3AlignedObjectArray<int>& indicesA,
|
||||
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesB,
|
||||
const b3AlignedObjectArray<int>& indicesB,
|
||||
const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
|
||||
b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
|
||||
int& nContacts,
|
||||
|
||||
const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
|
||||
const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
|
||||
const b3Vector3& sepNormalWorldSpace,
|
||||
int maxContactCapacity )
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
|
||||
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesA,
|
||||
const b3AlignedObjectArray<int>& indicesA,
|
||||
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesB,
|
||||
const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
|
||||
const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
|
||||
const b3Vector3& sepNormalWorldSpace,
|
||||
int maxContactCapacity)
|
||||
{
|
||||
int contactIndex = -1;
|
||||
b3ConvexPolyhedronData hullA, hullB;
|
||||
|
||||
b3Collidable colA = hostCollidablesA[collidableIndexA];
|
||||
hullA = hostConvexDataA[colA.m_shapeIndex];
|
||||
//printf("numvertsA = %d\n",hullA.m_numVertices);
|
||||
|
||||
|
||||
b3Collidable colB = hostCollidablesB[collidableIndexB];
|
||||
hullB = hostConvexDataB[colB.m_shapeIndex];
|
||||
//printf("numvertsB = %d\n",hullB.m_numVertices);
|
||||
|
||||
|
||||
|
||||
b3Collidable colA = hostCollidablesA[collidableIndexA];
|
||||
hullA = hostConvexDataA[colA.m_shapeIndex];
|
||||
//printf("numvertsA = %d\n",hullA.m_numVertices);
|
||||
|
||||
b3Collidable colB = hostCollidablesB[collidableIndexB];
|
||||
hullB = hostConvexDataB[colB.m_shapeIndex];
|
||||
//printf("numvertsB = %d\n",hullB.m_numVertices);
|
||||
|
||||
b3Float4 contactsOut[B3_MAX_VERTS];
|
||||
int localContactCapacity = B3_MAX_VERTS;
|
||||
|
||||
@@ -321,187 +307,168 @@ inline int b3ClipHullHullSingle(
|
||||
b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
|
||||
b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
{
|
||||
|
||||
b3Float4 worldVertsB1[B3_MAX_VERTS];
|
||||
b3Float4 worldVertsB2[B3_MAX_VERTS];
|
||||
int capacityWorldVerts = B3_MAX_VERTS;
|
||||
|
||||
b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f);
|
||||
b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
|
||||
int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
|
||||
int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
|
||||
|
||||
b3Scalar minDist = -1;
|
||||
b3Scalar maxDist = 0.;
|
||||
|
||||
|
||||
|
||||
b3Transform trA,trB;
|
||||
b3Transform trA, trB;
|
||||
{
|
||||
//B3_PROFILE("b3TransformPoint computation");
|
||||
//trA.setIdentity();
|
||||
trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z));
|
||||
trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w));
|
||||
|
||||
//trB.setIdentity();
|
||||
trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z));
|
||||
trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w));
|
||||
//B3_PROFILE("b3TransformPoint computation");
|
||||
//trA.setIdentity();
|
||||
trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
|
||||
trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
|
||||
|
||||
//trB.setIdentity();
|
||||
trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
|
||||
trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
|
||||
}
|
||||
|
||||
b3Quaternion trAorn = trA.getRotation();
|
||||
b3Quaternion trBorn = trB.getRotation();
|
||||
|
||||
int numContactsOut = b3ClipHullAgainstHull(hostNormal,
|
||||
hostConvexDataA.at(shapeA),
|
||||
hostConvexDataB.at(shapeB),
|
||||
(b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
|
||||
(b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
|
||||
worldVertsB1,worldVertsB2,capacityWorldVerts,
|
||||
minDist, maxDist,
|
||||
verticesA, facesA,indicesA,
|
||||
verticesB, facesB,indicesB,
|
||||
|
||||
contactsOut,localContactCapacity);
|
||||
b3Quaternion trBorn = trB.getRotation();
|
||||
|
||||
if (numContactsOut>0)
|
||||
int numContactsOut = b3ClipHullAgainstHull(hostNormal,
|
||||
hostConvexDataA.at(shapeA),
|
||||
hostConvexDataB.at(shapeB),
|
||||
(b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
|
||||
(b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
|
||||
worldVertsB1, worldVertsB2, capacityWorldVerts,
|
||||
minDist, maxDist,
|
||||
verticesA, facesA, indicesA,
|
||||
verticesB, facesB, indicesB,
|
||||
|
||||
contactsOut, localContactCapacity);
|
||||
|
||||
if (numContactsOut > 0)
|
||||
{
|
||||
B3_PROFILE("overlap");
|
||||
|
||||
b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
|
||||
// b3Float4 centerOut;
|
||||
|
||||
// b3Float4 centerOut;
|
||||
|
||||
b3Int4 contactIdx;
|
||||
contactIdx.x = 0;
|
||||
contactIdx.y = 1;
|
||||
contactIdx.z = 2;
|
||||
contactIdx.w = 3;
|
||||
|
||||
|
||||
int numPoints = 0;
|
||||
|
||||
|
||||
{
|
||||
B3_PROFILE("extractManifold");
|
||||
numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
|
||||
numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
|
||||
}
|
||||
|
||||
|
||||
b3Assert(numPoints);
|
||||
|
||||
if (nContacts<maxContactCapacity)
|
||||
|
||||
if (nContacts < maxContactCapacity)
|
||||
{
|
||||
contactIndex = nContacts;
|
||||
globalContactOut->expand();
|
||||
b3Contact4Data& contact = globalContactOut->at(nContacts);
|
||||
contact.m_batchIdx = 0;//i;
|
||||
contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA;
|
||||
contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB;
|
||||
contact.m_batchIdx = 0; //i;
|
||||
contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
|
||||
contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
|
||||
|
||||
contact.m_frictionCoeffCmp = 45874;
|
||||
contact.m_restituitionCoeffCmp = 0;
|
||||
|
||||
// float distance = 0.f;
|
||||
for (int p=0;p<numPoints;p++)
|
||||
|
||||
// float distance = 0.f;
|
||||
for (int p = 0; p < numPoints; p++)
|
||||
{
|
||||
contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];//check if it is actually on B
|
||||
contact.m_worldNormalOnB = normalOnSurfaceB;
|
||||
contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]]; //check if it is actually on B
|
||||
contact.m_worldNormalOnB = normalOnSurfaceB;
|
||||
}
|
||||
//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
|
||||
contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
|
||||
nContacts++;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts,maxContactCapacity);
|
||||
b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
|
||||
}
|
||||
}
|
||||
}
|
||||
return contactIndex;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
inline int b3ContactConvexConvexSAT(
|
||||
int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
|
||||
const b3AlignedObjectArray<b3Collidable>& collidables,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
|
||||
const b3AlignedObjectArray<b3Float4>& convexVertices,
|
||||
const b3AlignedObjectArray<b3Float4>& uniqueEdges,
|
||||
const b3AlignedObjectArray<int>& convexIndices,
|
||||
const b3AlignedObjectArray<b3GpuFace>& faces,
|
||||
b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
|
||||
int& nGlobalContactsOut,
|
||||
int maxContactCapacity)
|
||||
int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
|
||||
const b3AlignedObjectArray<b3Collidable>& collidables,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
|
||||
const b3AlignedObjectArray<b3Float4>& convexVertices,
|
||||
const b3AlignedObjectArray<b3Float4>& uniqueEdges,
|
||||
const b3AlignedObjectArray<int>& convexIndices,
|
||||
const b3AlignedObjectArray<b3GpuFace>& faces,
|
||||
b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
|
||||
int& nGlobalContactsOut,
|
||||
int maxContactCapacity)
|
||||
{
|
||||
int contactIndex = -1;
|
||||
|
||||
|
||||
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
|
||||
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
||||
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
|
||||
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
|
||||
|
||||
|
||||
b3ConvexPolyhedronData hullA, hullB;
|
||||
|
||||
|
||||
b3Float4 sepNormalWorldSpace;
|
||||
|
||||
|
||||
|
||||
b3Collidable colA = collidables[collidableIndexA];
|
||||
hullA = convexShapes[colA.m_shapeIndex];
|
||||
//printf("numvertsA = %d\n",hullA.m_numVertices);
|
||||
|
||||
|
||||
b3Collidable colB = collidables[collidableIndexB];
|
||||
hullB = convexShapes[colB.m_shapeIndex];
|
||||
//printf("numvertsB = %d\n",hullB.m_numVertices);
|
||||
|
||||
|
||||
b3Collidable colA = collidables[collidableIndexA];
|
||||
hullA = convexShapes[colA.m_shapeIndex];
|
||||
//printf("numvertsA = %d\n",hullA.m_numVertices);
|
||||
|
||||
b3Collidable colB = collidables[collidableIndexB];
|
||||
hullB = convexShapes[colB.m_shapeIndex];
|
||||
//printf("numvertsB = %d\n",hullB.m_numVertices);
|
||||
|
||||
#ifdef _WIN32
|
||||
b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
|
||||
b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
|
||||
#endif
|
||||
|
||||
bool foundSepAxis = b3FindSeparatingAxis(hullA,hullB,
|
||||
posA,
|
||||
ornA,
|
||||
posB,
|
||||
ornB,
|
||||
|
||||
convexVertices,uniqueEdges,faces,convexIndices,
|
||||
convexVertices,uniqueEdges,faces,convexIndices,
|
||||
|
||||
sepNormalWorldSpace
|
||||
);
|
||||
bool foundSepAxis = b3FindSeparatingAxis(hullA, hullB,
|
||||
posA,
|
||||
ornA,
|
||||
posB,
|
||||
ornB,
|
||||
|
||||
convexVertices, uniqueEdges, faces, convexIndices,
|
||||
convexVertices, uniqueEdges, faces, convexIndices,
|
||||
|
||||
sepNormalWorldSpace);
|
||||
|
||||
|
||||
if (foundSepAxis)
|
||||
{
|
||||
|
||||
|
||||
contactIndex = b3ClipHullHullSingle(
|
||||
bodyIndexA, bodyIndexB,
|
||||
posA,ornA,
|
||||
posB,ornB,
|
||||
posA, ornA,
|
||||
posB, ornB,
|
||||
collidableIndexA, collidableIndexB,
|
||||
&rigidBodies,
|
||||
&rigidBodies,
|
||||
&globalContactsOut,
|
||||
nGlobalContactsOut,
|
||||
|
||||
|
||||
convexShapes,
|
||||
convexShapes,
|
||||
|
||||
convexVertices,
|
||||
uniqueEdges,
|
||||
|
||||
convexVertices,
|
||||
uniqueEdges,
|
||||
faces,
|
||||
convexIndices,
|
||||
|
||||
|
||||
convexVertices,
|
||||
uniqueEdges,
|
||||
faces,
|
||||
@@ -511,10 +478,9 @@ inline int b3ContactConvexConvexSAT(
|
||||
collidables,
|
||||
sepNormalWorldSpace,
|
||||
maxContactCapacity);
|
||||
|
||||
}
|
||||
|
||||
return contactIndex;
|
||||
}
|
||||
|
||||
#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
|
||||
#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
|
||||
|
||||
@@ -2,32 +2,24 @@
|
||||
#ifndef B3_CONTACT_SPHERE_SPHERE_H
|
||||
#define B3_CONTACT_SPHERE_SPHERE_H
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void computeContactSphereConvex(int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
const b3RigidBodyData* rigidBodies,
|
||||
const b3Collidable* collidables,
|
||||
const b3ConvexPolyhedronData* convexShapes,
|
||||
const b3Vector3* convexVertices,
|
||||
const int* convexIndices,
|
||||
const b3GpuFace* faces,
|
||||
b3Contact4* globalContactsOut,
|
||||
int& nGlobalContactsOut,
|
||||
int maxContactCapacity)
|
||||
void computeContactSphereConvex(int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
const b3RigidBodyData* rigidBodies,
|
||||
const b3Collidable* collidables,
|
||||
const b3ConvexPolyhedronData* convexShapes,
|
||||
const b3Vector3* convexVertices,
|
||||
const int* convexIndices,
|
||||
const b3GpuFace* faces,
|
||||
b3Contact4* globalContactsOut,
|
||||
int& nGlobalContactsOut,
|
||||
int maxContactCapacity)
|
||||
{
|
||||
|
||||
float radius = collidables[collidableIndexA].m_radius;
|
||||
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
|
||||
b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
|
||||
|
||||
|
||||
|
||||
float4 pos = rigidBodies[bodyIndexB].m_pos;
|
||||
|
||||
|
||||
b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
|
||||
|
||||
@@ -44,63 +36,64 @@ void computeContactSphereConvex(int pairIndex,
|
||||
int numFaces = convexShapes[shapeIndex].m_numFaces;
|
||||
float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
|
||||
float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
|
||||
float minDist = -1000000.f; // TODO: What is the largest/smallest float?
|
||||
float minDist = -1000000.f; // TODO: What is the largest/smallest float?
|
||||
bool bCollide = true;
|
||||
int region = -1;
|
||||
float4 localHitNormal;
|
||||
for ( int f = 0; f < numFaces; f++ )
|
||||
for (int f = 0; f < numFaces; f++)
|
||||
{
|
||||
b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
|
||||
b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
|
||||
float4 planeEqn;
|
||||
float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
|
||||
float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
|
||||
float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
|
||||
float4 n1 = localPlaneNormal; //quatRotate(quat,localPlaneNormal);
|
||||
planeEqn = n1;
|
||||
planeEqn[3] = face.m_plane.w;
|
||||
|
||||
float4 pntReturn;
|
||||
float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
|
||||
|
||||
if ( dist > radius)
|
||||
if (dist > radius)
|
||||
{
|
||||
bCollide = false;
|
||||
break;
|
||||
}
|
||||
|
||||
if ( dist > 0 )
|
||||
if (dist > 0)
|
||||
{
|
||||
//might hit an edge or vertex
|
||||
b3Vector3 out;
|
||||
|
||||
bool isInPoly = IsPointInPolygon(spherePos,
|
||||
&face,
|
||||
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
|
||||
convexIndices,
|
||||
&out);
|
||||
&face,
|
||||
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
|
||||
convexIndices,
|
||||
&out);
|
||||
if (isInPoly)
|
||||
{
|
||||
if (dist>minDist)
|
||||
if (dist > minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = pntReturn;
|
||||
localHitNormal = planeEqn;
|
||||
region=1;
|
||||
region = 1;
|
||||
}
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Vector3 tmp = spherePos-out;
|
||||
b3Vector3 tmp = spherePos - out;
|
||||
b3Scalar l2 = tmp.length2();
|
||||
if (l2<radius*radius)
|
||||
if (l2 < radius * radius)
|
||||
{
|
||||
dist = b3Sqrt(l2);
|
||||
if (dist>minDist)
|
||||
dist = b3Sqrt(l2);
|
||||
if (dist > minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = out;
|
||||
localHitNormal = tmp/dist;
|
||||
region=2;
|
||||
localHitNormal = tmp / dist;
|
||||
region = 2;
|
||||
}
|
||||
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
bCollide = false;
|
||||
break;
|
||||
@@ -109,12 +102,12 @@ void computeContactSphereConvex(int pairIndex,
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( dist > minDist )
|
||||
if (dist > minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = pntReturn;
|
||||
localHitNormal = planeEqn;
|
||||
region=3;
|
||||
region = 3;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -123,40 +116,38 @@ void computeContactSphereConvex(int pairIndex,
|
||||
|
||||
if (bCollide && minDist > -10000)
|
||||
{
|
||||
|
||||
float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld;
|
||||
float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal; //-hitNormalWorld;
|
||||
float4 pOnB1 = tr(closestPnt);
|
||||
//printf("dist ,%f,",minDist);
|
||||
float actualDepth = minDist-radius;
|
||||
if (actualDepth<0)
|
||||
float actualDepth = minDist - radius;
|
||||
if (actualDepth < 0)
|
||||
{
|
||||
//printf("actualDepth = ,%f,", actualDepth);
|
||||
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
|
||||
//printf("region=,%d,\n", region);
|
||||
pOnB1[3] = actualDepth;
|
||||
//printf("actualDepth = ,%f,", actualDepth);
|
||||
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
|
||||
//printf("region=,%d,\n", region);
|
||||
pOnB1[3] = actualDepth;
|
||||
|
||||
int dstIdx;
|
||||
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
|
||||
|
||||
if (nGlobalContactsOut < maxContactCapacity)
|
||||
{
|
||||
dstIdx=nGlobalContactsOut;
|
||||
nGlobalContactsOut++;
|
||||
int dstIdx;
|
||||
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
|
||||
|
||||
b3Contact4* c = &globalContactsOut[dstIdx];
|
||||
c->m_worldNormalOnB = normalOnSurfaceB1;
|
||||
c->setFrictionCoeff(0.7);
|
||||
c->setRestituitionCoeff(0.f);
|
||||
if (nGlobalContactsOut < maxContactCapacity)
|
||||
{
|
||||
dstIdx = nGlobalContactsOut;
|
||||
nGlobalContactsOut++;
|
||||
|
||||
c->m_batchIdx = pairIndex;
|
||||
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
|
||||
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
|
||||
c->m_worldPosB[0] = pOnB1;
|
||||
int numPoints = 1;
|
||||
c->m_worldNormalOnB.w = (b3Scalar)numPoints;
|
||||
}//if (dstIdx < numPairs)
|
||||
b3Contact4* c = &globalContactsOut[dstIdx];
|
||||
c->m_worldNormalOnB = normalOnSurfaceB1;
|
||||
c->setFrictionCoeff(0.7);
|
||||
c->setRestituitionCoeff(0.f);
|
||||
|
||||
c->m_batchIdx = pairIndex;
|
||||
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
|
||||
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
|
||||
c->m_worldPosB[0] = pOnB1;
|
||||
int numPoints = 1;
|
||||
c->m_worldNormalOnB.w = (b3Scalar)numPoints;
|
||||
} //if (dstIdx < numPairs)
|
||||
}
|
||||
}//if (hasCollision)
|
||||
|
||||
} //if (hasCollision)
|
||||
}
|
||||
#endif //B3_CONTACT_SPHERE_SPHERE_H
|
||||
#endif //B3_CONTACT_SPHERE_SPHERE_H
|
||||
|
||||
@@ -2,8 +2,6 @@
|
||||
#ifndef B3_CONVEX_POLYHEDRON_DATA_H
|
||||
#define B3_CONVEX_POLYHEDRON_DATA_H
|
||||
|
||||
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Common/shared/b3Quat.h"
|
||||
|
||||
@@ -21,20 +19,20 @@ typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;
|
||||
|
||||
struct b3ConvexPolyhedronData
|
||||
{
|
||||
b3Float4 m_localCenter;
|
||||
b3Float4 m_extents;
|
||||
b3Float4 mC;
|
||||
b3Float4 mE;
|
||||
b3Float4 m_localCenter;
|
||||
b3Float4 m_extents;
|
||||
b3Float4 mC;
|
||||
b3Float4 mE;
|
||||
|
||||
float m_radius;
|
||||
int m_faceOffset;
|
||||
float m_radius;
|
||||
int m_faceOffset;
|
||||
int m_numFaces;
|
||||
int m_numVertices;
|
||||
int m_numVertices;
|
||||
|
||||
int m_vertexOffset;
|
||||
int m_uniqueEdgesOffset;
|
||||
int m_numUniqueEdges;
|
||||
int m_uniqueEdgesOffset;
|
||||
int m_numUniqueEdges;
|
||||
int m_unused;
|
||||
};
|
||||
|
||||
#endif //B3_CONVEX_POLYHEDRON_DATA_H
|
||||
#endif //B3_CONVEX_POLYHEDRON_DATA_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,28 +1,27 @@
|
||||
#ifndef B3_FIND_SEPARATING_AXIS_H
|
||||
#define B3_FIND_SEPARATING_AXIS_H
|
||||
|
||||
|
||||
inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
|
||||
inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
|
||||
{
|
||||
min = FLT_MAX;
|
||||
max = -FLT_MAX;
|
||||
int numVerts = hull.m_numVertices;
|
||||
|
||||
const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir);
|
||||
const b3Float4 localDir = b3QuatRotate(orn.inverse(), dir);
|
||||
|
||||
b3Scalar offset = b3Dot3F4(pos,dir);
|
||||
b3Scalar offset = b3Dot3F4(pos, dir);
|
||||
|
||||
for(int i=0;i<numVerts;i++)
|
||||
for (int i = 0; i < numVerts; i++)
|
||||
{
|
||||
//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
|
||||
//b3Scalar dp = pt.dot(dir);
|
||||
//b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
|
||||
b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset+i],localDir);
|
||||
b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset + i], localDir);
|
||||
//b3Assert(dp==dpL);
|
||||
if(dp < min) min = dp;
|
||||
if(dp > max) max = dp;
|
||||
if (dp < min) min = dp;
|
||||
if (dp > max) max = dp;
|
||||
}
|
||||
if(min>max)
|
||||
if (min > max)
|
||||
{
|
||||
b3Scalar tmp = min;
|
||||
min = max;
|
||||
@@ -32,44 +31,42 @@ inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& p
|
||||
max += offset;
|
||||
}
|
||||
|
||||
|
||||
inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA,const b3Quaternion& ornA,
|
||||
const b3Float4& posB,const b3Quaternion& ornB,
|
||||
const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB,b3Scalar& depth)
|
||||
inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA, const b3Quaternion& ornA,
|
||||
const b3Float4& posB, const b3Quaternion& ornB,
|
||||
const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
|
||||
{
|
||||
b3Scalar Min0,Max0;
|
||||
b3Scalar Min1,Max1;
|
||||
b3ProjectAxis(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0);
|
||||
b3ProjectAxis(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1);
|
||||
b3Scalar Min0, Max0;
|
||||
b3Scalar Min1, Max1;
|
||||
b3ProjectAxis(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
|
||||
b3ProjectAxis(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
|
||||
|
||||
if(Max0<Min1 || Max1<Min0)
|
||||
if (Max0 < Min1 || Max1 < Min0)
|
||||
return false;
|
||||
|
||||
b3Scalar d0 = Max0 - Min1;
|
||||
b3Assert(d0>=0.0f);
|
||||
b3Assert(d0 >= 0.0f);
|
||||
b3Scalar d1 = Max1 - Min0;
|
||||
b3Assert(d1>=0.0f);
|
||||
depth = d0<d1 ? d0:d1;
|
||||
b3Assert(d1 >= 0.0f);
|
||||
depth = d0 < d1 ? d0 : d1;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA1,
|
||||
const b3Quaternion& ornA,
|
||||
const b3Float4& posB1,
|
||||
const b3Quaternion& ornB,
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesA,
|
||||
const b3AlignedObjectArray<int>& indicesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesB,
|
||||
const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA1,
|
||||
const b3Quaternion& ornA,
|
||||
const b3Float4& posB1,
|
||||
const b3Quaternion& ornB,
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesA,
|
||||
const b3AlignedObjectArray<int>& indicesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesB,
|
||||
const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
b3Vector3& sep)
|
||||
b3Vector3& sep)
|
||||
{
|
||||
B3_PROFILE("findSeparatingAxis");
|
||||
|
||||
@@ -77,42 +74,41 @@ inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3C
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
//#ifdef TEST_INTERNAL_OBJECTS
|
||||
//#ifdef TEST_INTERNAL_OBJECTS
|
||||
b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
|
||||
|
||||
b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
|
||||
b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
|
||||
b3Float4 c1 = b3TransformPoint(c1local,posB,ornB);
|
||||
b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
|
||||
const b3Float4 deltaC2 = c0 - c1;
|
||||
//#endif
|
||||
//#endif
|
||||
|
||||
b3Scalar dmin = FLT_MAX;
|
||||
int curPlaneTests=0;
|
||||
int curPlaneTests = 0;
|
||||
|
||||
int numFacesA = hullA.m_numFaces;
|
||||
// Test normals from hullA
|
||||
for(int i=0;i<numFacesA;i++)
|
||||
for (int i = 0; i < numFacesA; i++)
|
||||
{
|
||||
const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset+i].m_plane;
|
||||
b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
|
||||
const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset + i].m_plane;
|
||||
b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
|
||||
|
||||
if (b3Dot3F4(deltaC2,faceANormalWS)<0)
|
||||
faceANormalWS*=-1.f;
|
||||
if (b3Dot3F4(deltaC2, faceANormalWS) < 0)
|
||||
faceANormalWS *= -1.f;
|
||||
|
||||
curPlaneTests++;
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
|
||||
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
|
||||
b3Scalar d;
|
||||
if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,faceANormalWS, verticesA, verticesB,d))
|
||||
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
|
||||
return false;
|
||||
|
||||
if(d<dmin)
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
sep = (b3Vector3&)faceANormalWS;
|
||||
@@ -121,86 +117,81 @@ inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3C
|
||||
|
||||
int numFacesB = hullB.m_numFaces;
|
||||
// Test normals from hullB
|
||||
for(int i=0;i<numFacesB;i++)
|
||||
for (int i = 0; i < numFacesB; i++)
|
||||
{
|
||||
b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset+i].m_plane;
|
||||
b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset + i].m_plane;
|
||||
b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
|
||||
|
||||
if (b3Dot3F4(deltaC2,WorldNormal)<0)
|
||||
if (b3Dot3F4(deltaC2, WorldNormal) < 0)
|
||||
{
|
||||
WorldNormal*=-1.f;
|
||||
WorldNormal *= -1.f;
|
||||
}
|
||||
curPlaneTests++;
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
|
||||
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
b3Scalar d;
|
||||
if(!b3TestSepAxis(hullA, hullB,posA,ornA,posB,ornB,WorldNormal,verticesA,verticesB,d))
|
||||
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
|
||||
return false;
|
||||
|
||||
if(d<dmin)
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
sep = (b3Vector3&)WorldNormal;
|
||||
}
|
||||
}
|
||||
|
||||
// b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
|
||||
// b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
|
||||
|
||||
int curEdgeEdge = 0;
|
||||
// Test edges
|
||||
for(int e0=0;e0<hullA.m_numUniqueEdges;e0++)
|
||||
for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
|
||||
{
|
||||
const b3Float4& edge0 = (b3Float4&) uniqueEdgesA[hullA.m_uniqueEdgesOffset+e0];
|
||||
b3Float4 edge0World = b3QuatRotate(ornA,(b3Float4&)edge0);
|
||||
const b3Float4& edge0 = (b3Float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
|
||||
b3Float4 edge0World = b3QuatRotate(ornA, (b3Float4&)edge0);
|
||||
|
||||
for(int e1=0;e1<hullB.m_numUniqueEdges;e1++)
|
||||
for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
|
||||
{
|
||||
const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset+e1];
|
||||
b3Float4 edge1World = b3QuatRotate(ornB,(b3Float4&)edge1);
|
||||
const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
|
||||
b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1);
|
||||
|
||||
|
||||
b3Float4 crossje = b3Cross3(edge0World,edge1World);
|
||||
b3Float4 crossje = b3Cross3(edge0World, edge1World);
|
||||
|
||||
curEdgeEdge++;
|
||||
if(!b3IsAlmostZero((b3Vector3&)crossje))
|
||||
if (!b3IsAlmostZero((b3Vector3&)crossje))
|
||||
{
|
||||
crossje = b3FastNormalized3(crossje);
|
||||
if (b3Dot3F4(deltaC2,crossje)<0)
|
||||
crossje*=-1.f;
|
||||
|
||||
if (b3Dot3F4(deltaC2, crossje) < 0)
|
||||
crossje *= -1.f;
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
|
||||
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
b3Scalar dist;
|
||||
if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,crossje, verticesA,verticesB,dist))
|
||||
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
|
||||
return false;
|
||||
|
||||
if(dist<dmin)
|
||||
if (dist < dmin)
|
||||
{
|
||||
dmin = dist;
|
||||
sep = (b3Vector3&)crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
if((b3Dot3F4(-deltaC2,(b3Float4&)sep))>0.0f)
|
||||
if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f)
|
||||
sep = -sep;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif //B3_FIND_SEPARATING_AXIS_H
|
||||
|
||||
#endif //B3_FIND_SEPARATING_AXIS_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -8,189 +8,168 @@
|
||||
|
||||
#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
|
||||
|
||||
|
||||
int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
|
||||
{
|
||||
if( nPoints == 0 )
|
||||
return 0;
|
||||
|
||||
if (nPoints <=4)
|
||||
return nPoints;
|
||||
|
||||
|
||||
if (nPoints >64)
|
||||
nPoints = 64;
|
||||
|
||||
b3Float4 center = b3MakeFloat4(0,0,0,0);
|
||||
if (nPoints == 0)
|
||||
return 0;
|
||||
|
||||
if (nPoints <= 4)
|
||||
return nPoints;
|
||||
|
||||
if (nPoints > 64)
|
||||
nPoints = 64;
|
||||
|
||||
b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
|
||||
{
|
||||
|
||||
for (int i=0;i<nPoints;i++)
|
||||
for (int i = 0; i < nPoints; i++)
|
||||
center += p[i];
|
||||
center /= (float)nPoints;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// sample 4 directions
|
||||
|
||||
b3Float4 aVector = p[0] - center;
|
||||
b3Float4 u = b3Cross( nearNormal, aVector );
|
||||
b3Float4 v = b3Cross( nearNormal, u );
|
||||
u = b3Normalized( u );
|
||||
v = b3Normalized( v );
|
||||
|
||||
|
||||
//keep point with deepest penetration
|
||||
float minW= FLT_MAX;
|
||||
|
||||
int minIndex=-1;
|
||||
|
||||
b3Float4 maxDots;
|
||||
maxDots.x = FLT_MIN;
|
||||
maxDots.y = FLT_MIN;
|
||||
maxDots.z = FLT_MIN;
|
||||
maxDots.w = FLT_MIN;
|
||||
|
||||
// idx, distance
|
||||
for(int ie = 0; ie<nPoints; ie++ )
|
||||
{
|
||||
if (p[ie].w<minW)
|
||||
{
|
||||
minW = p[ie].w;
|
||||
minIndex=ie;
|
||||
}
|
||||
float f;
|
||||
b3Float4 r = p[ie]-center;
|
||||
f = b3Dot( u, r );
|
||||
if (f<maxDots.x)
|
||||
{
|
||||
maxDots.x = f;
|
||||
contactIdx[0].x = ie;
|
||||
}
|
||||
|
||||
f = b3Dot( -u, r );
|
||||
if (f<maxDots.y)
|
||||
{
|
||||
maxDots.y = f;
|
||||
contactIdx[0].y = ie;
|
||||
}
|
||||
|
||||
|
||||
f = b3Dot( v, r );
|
||||
if (f<maxDots.z)
|
||||
{
|
||||
maxDots.z = f;
|
||||
contactIdx[0].z = ie;
|
||||
}
|
||||
|
||||
f = b3Dot( -v, r );
|
||||
if (f<maxDots.w)
|
||||
{
|
||||
maxDots.w = f;
|
||||
contactIdx[0].w = ie;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
|
||||
{
|
||||
//replace the first contact with minimum (todo: replace contact with least penetration)
|
||||
contactIdx[0].x = minIndex;
|
||||
}
|
||||
|
||||
return 4;
|
||||
|
||||
|
||||
b3Float4 aVector = p[0] - center;
|
||||
b3Float4 u = b3Cross(nearNormal, aVector);
|
||||
b3Float4 v = b3Cross(nearNormal, u);
|
||||
u = b3Normalized(u);
|
||||
v = b3Normalized(v);
|
||||
|
||||
//keep point with deepest penetration
|
||||
float minW = FLT_MAX;
|
||||
|
||||
int minIndex = -1;
|
||||
|
||||
b3Float4 maxDots;
|
||||
maxDots.x = FLT_MIN;
|
||||
maxDots.y = FLT_MIN;
|
||||
maxDots.z = FLT_MIN;
|
||||
maxDots.w = FLT_MIN;
|
||||
|
||||
// idx, distance
|
||||
for (int ie = 0; ie < nPoints; ie++)
|
||||
{
|
||||
if (p[ie].w < minW)
|
||||
{
|
||||
minW = p[ie].w;
|
||||
minIndex = ie;
|
||||
}
|
||||
float f;
|
||||
b3Float4 r = p[ie] - center;
|
||||
f = b3Dot(u, r);
|
||||
if (f < maxDots.x)
|
||||
{
|
||||
maxDots.x = f;
|
||||
contactIdx[0].x = ie;
|
||||
}
|
||||
|
||||
f = b3Dot(-u, r);
|
||||
if (f < maxDots.y)
|
||||
{
|
||||
maxDots.y = f;
|
||||
contactIdx[0].y = ie;
|
||||
}
|
||||
|
||||
f = b3Dot(v, r);
|
||||
if (f < maxDots.z)
|
||||
{
|
||||
maxDots.z = f;
|
||||
contactIdx[0].z = ie;
|
||||
}
|
||||
|
||||
f = b3Dot(-v, r);
|
||||
if (f < maxDots.w)
|
||||
{
|
||||
maxDots.w = f;
|
||||
contactIdx[0].w = ie;
|
||||
}
|
||||
}
|
||||
|
||||
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
|
||||
{
|
||||
//replace the first contact with minimum (todo: replace contact with least penetration)
|
||||
contactIdx[0].x = minIndex;
|
||||
}
|
||||
|
||||
return 4;
|
||||
}
|
||||
|
||||
__kernel void b3NewContactReductionKernel( __global b3Int4* pairs,
|
||||
__global const b3RigidBodyData_t* rigidBodies,
|
||||
__global const b3Float4* separatingNormals,
|
||||
__global const int* hasSeparatingAxis,
|
||||
__global struct b3Contact4Data* globalContactsOut,
|
||||
__global b3Int4* clippingFaces,
|
||||
__global b3Float4* worldVertsB2,
|
||||
volatile __global int* nGlobalContactsOut,
|
||||
int vertexFaceCapacity,
|
||||
int contactCapacity,
|
||||
int numPairs,
|
||||
int pairIndex
|
||||
)
|
||||
__kernel void b3NewContactReductionKernel(__global b3Int4* pairs,
|
||||
__global const b3RigidBodyData_t* rigidBodies,
|
||||
__global const b3Float4* separatingNormals,
|
||||
__global const int* hasSeparatingAxis,
|
||||
__global struct b3Contact4Data* globalContactsOut,
|
||||
__global b3Int4* clippingFaces,
|
||||
__global b3Float4* worldVertsB2,
|
||||
volatile __global int* nGlobalContactsOut,
|
||||
int vertexFaceCapacity,
|
||||
int contactCapacity,
|
||||
int numPairs,
|
||||
int pairIndex)
|
||||
{
|
||||
// int i = get_global_id(0);
|
||||
// int i = get_global_id(0);
|
||||
//int pairIndex = i;
|
||||
int i = pairIndex;
|
||||
|
||||
b3Int4 contactIdx;
|
||||
contactIdx=b3MakeInt4(0,1,2,3);
|
||||
|
||||
if (i<numPairs)
|
||||
b3Int4 contactIdx;
|
||||
contactIdx = b3MakeInt4(0, 1, 2, 3);
|
||||
|
||||
if (i < numPairs)
|
||||
{
|
||||
|
||||
if (hasSeparatingAxis[i])
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
int nPoints = clippingFaces[pairIndex].w;
|
||||
|
||||
if (nPoints>0)
|
||||
{
|
||||
|
||||
__global b3Float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity];
|
||||
b3Float4 normal = -separatingNormals[i];
|
||||
|
||||
int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
|
||||
|
||||
int dstIdx;
|
||||
dstIdx = b3AtomicInc( nGlobalContactsOut);
|
||||
|
||||
//#if 0
|
||||
b3Assert(dstIdx < contactCapacity);
|
||||
if (nPoints > 0)
|
||||
{
|
||||
__global b3Float4* pointsIn = &worldVertsB2[pairIndex * vertexFaceCapacity];
|
||||
b3Float4 normal = -separatingNormals[i];
|
||||
|
||||
int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
|
||||
|
||||
int dstIdx;
|
||||
dstIdx = b3AtomicInc(nGlobalContactsOut);
|
||||
|
||||
//#if 0
|
||||
b3Assert(dstIdx < contactCapacity);
|
||||
if (dstIdx < contactCapacity)
|
||||
{
|
||||
|
||||
__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
|
||||
c->m_worldNormalOnB = -normal;
|
||||
c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
|
||||
c->m_restituitionCoeffCmp = (0.f * 0xffff);
|
||||
c->m_frictionCoeffCmp = (0.7f * 0xffff);
|
||||
c->m_batchIdx = pairIndex;
|
||||
int bodyA = pairs[pairIndex].x;
|
||||
int bodyB = pairs[pairIndex].y;
|
||||
|
||||
pairs[pairIndex].w = dstIdx;
|
||||
|
||||
c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;
|
||||
c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;
|
||||
c->m_childIndexA =-1;
|
||||
c->m_childIndexB =-1;
|
||||
c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
|
||||
c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
|
||||
c->m_childIndexA = -1;
|
||||
c->m_childIndexB = -1;
|
||||
|
||||
switch (nReducedContacts)
|
||||
{
|
||||
case 4:
|
||||
c->m_worldPosB[3] = pointsIn[contactIdx.w];
|
||||
case 3:
|
||||
c->m_worldPosB[2] = pointsIn[contactIdx.z];
|
||||
case 2:
|
||||
c->m_worldPosB[1] = pointsIn[contactIdx.y];
|
||||
case 1:
|
||||
c->m_worldPosB[0] = pointsIn[contactIdx.x];
|
||||
default:
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
switch (nReducedContacts)
|
||||
{
|
||||
case 4:
|
||||
c->m_worldPosB[3] = pointsIn[contactIdx.w];
|
||||
case 3:
|
||||
c->m_worldPosB[2] = pointsIn[contactIdx.z];
|
||||
case 2:
|
||||
c->m_worldPosB[1] = pointsIn[contactIdx.y];
|
||||
case 1:
|
||||
c->m_worldPosB[0] = pointsIn[contactIdx.x];
|
||||
default:
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
GET_NPOINTS(*c) = nReducedContacts;
|
||||
|
||||
}
|
||||
|
||||
|
||||
//#endif
|
||||
|
||||
}// if (numContactsOut>0)
|
||||
}// if (hasSeparatingAxis[i])
|
||||
}// if (i<numPairs)
|
||||
}
|
||||
|
||||
|
||||
|
||||
//#endif
|
||||
|
||||
} // if (numContactsOut>0)
|
||||
} // if (hasSeparatingAxis[i])
|
||||
} // if (i<numPairs)
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -14,57 +14,56 @@ typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
|
||||
struct b3QuantizedBvhNodeData
|
||||
{
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes
|
||||
int m_escapeIndexOrTriangleIndex;
|
||||
int m_escapeIndexOrTriangleIndex;
|
||||
};
|
||||
|
||||
inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
unsigned int x=0;
|
||||
unsigned int y = (~(x&0))<<(31-B3_MAX_NUM_PARTS_IN_BITS);
|
||||
unsigned int x = 0;
|
||||
unsigned int y = (~(x & 0)) << (31 - B3_MAX_NUM_PARTS_IN_BITS);
|
||||
// Get only the lower bits where the triangle index is stored
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex & ~(y));
|
||||
}
|
||||
|
||||
inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex >= 0) ? 1 : 0;
|
||||
}
|
||||
|
||||
|
||||
inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
return -rootNode->m_escapeIndexOrTriangleIndex;
|
||||
}
|
||||
|
||||
inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
|
||||
inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2, int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
|
||||
{
|
||||
b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin);
|
||||
clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax);
|
||||
b3Float4 clampedPoint = b3MaxFloat4(point2, bvhAabbMin);
|
||||
clampedPoint = b3MinFloat4(clampedPoint, bvhAabbMax);
|
||||
|
||||
b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
|
||||
if (isMax)
|
||||
{
|
||||
out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));
|
||||
out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));
|
||||
out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));
|
||||
} else
|
||||
{
|
||||
out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));
|
||||
out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));
|
||||
out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));
|
||||
out[0] = (unsigned short)(((unsigned short)(v.x + 1.f) | 1));
|
||||
out[1] = (unsigned short)(((unsigned short)(v.y + 1.f) | 1));
|
||||
out[2] = (unsigned short)(((unsigned short)(v.z + 1.f) | 1));
|
||||
}
|
||||
else
|
||||
{
|
||||
out[0] = (unsigned short)(((unsigned short)(v.x) & 0xfffe));
|
||||
out[1] = (unsigned short)(((unsigned short)(v.y) & 0xfffe));
|
||||
out[2] = (unsigned short)(((unsigned short)(v.z) & 0xfffe));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
|
||||
const unsigned short int* aabbMin1,
|
||||
const unsigned short int* aabbMax1,
|
||||
const unsigned short int* aabbMin2,
|
||||
const unsigned short int* aabbMax2)
|
||||
const unsigned short int* aabbMin1,
|
||||
const unsigned short int* aabbMax1,
|
||||
const unsigned short int* aabbMin2,
|
||||
const unsigned short int* aabbMax2)
|
||||
{
|
||||
//int overlap = 1;
|
||||
if (aabbMin1[0] > aabbMax2[0])
|
||||
@@ -86,5 +85,4 @@ inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
|
||||
//return overlap;
|
||||
}
|
||||
|
||||
|
||||
#endif //B3_QUANTIZED_BVH_NODE_H
|
||||
#endif //B3_QUANTIZED_BVH_NODE_H
|
||||
|
||||
@@ -3,95 +3,87 @@
|
||||
|
||||
inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
|
||||
{
|
||||
if( nPoints == 0 )
|
||||
return 0;
|
||||
|
||||
if (nPoints <=4)
|
||||
return nPoints;
|
||||
|
||||
|
||||
if (nPoints >64)
|
||||
nPoints = 64;
|
||||
|
||||
b3Float4 center = b3MakeFloat4(0,0,0,0);
|
||||
if (nPoints == 0)
|
||||
return 0;
|
||||
|
||||
if (nPoints <= 4)
|
||||
return nPoints;
|
||||
|
||||
if (nPoints > 64)
|
||||
nPoints = 64;
|
||||
|
||||
b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
|
||||
{
|
||||
|
||||
for (int i=0;i<nPoints;i++)
|
||||
for (int i = 0; i < nPoints; i++)
|
||||
center += p[i];
|
||||
center /= (float)nPoints;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// sample 4 directions
|
||||
|
||||
b3Float4 aVector = p[0] - center;
|
||||
b3Float4 u = b3Cross3( nearNormal, aVector );
|
||||
b3Float4 v = b3Cross3( nearNormal, u );
|
||||
u = b3FastNormalized3( u );
|
||||
v = b3FastNormalized3( v );
|
||||
|
||||
|
||||
//keep point with deepest penetration
|
||||
float minW= FLT_MAX;
|
||||
|
||||
int minIndex=-1;
|
||||
|
||||
b3Float4 maxDots;
|
||||
maxDots.x = FLT_MIN;
|
||||
maxDots.y = FLT_MIN;
|
||||
maxDots.z = FLT_MIN;
|
||||
maxDots.w = FLT_MIN;
|
||||
|
||||
// idx, distance
|
||||
for(int ie = 0; ie<nPoints; ie++ )
|
||||
{
|
||||
if (p[ie].w<minW)
|
||||
{
|
||||
minW = p[ie].w;
|
||||
minIndex=ie;
|
||||
}
|
||||
float f;
|
||||
b3Float4 r = p[ie]-center;
|
||||
f = b3Dot3F4( u, r );
|
||||
if (f<maxDots.x)
|
||||
{
|
||||
maxDots.x = f;
|
||||
contactIdx[0].x = ie;
|
||||
}
|
||||
|
||||
f = b3Dot3F4( -u, r );
|
||||
if (f<maxDots.y)
|
||||
{
|
||||
maxDots.y = f;
|
||||
contactIdx[0].y = ie;
|
||||
}
|
||||
|
||||
|
||||
f = b3Dot3F4( v, r );
|
||||
if (f<maxDots.z)
|
||||
{
|
||||
maxDots.z = f;
|
||||
contactIdx[0].z = ie;
|
||||
}
|
||||
|
||||
f = b3Dot3F4( -v, r );
|
||||
if (f<maxDots.w)
|
||||
{
|
||||
maxDots.w = f;
|
||||
contactIdx[0].w = ie;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
|
||||
{
|
||||
//replace the first contact with minimum (todo: replace contact with least penetration)
|
||||
contactIdx[0].x = minIndex;
|
||||
}
|
||||
|
||||
return 4;
|
||||
|
||||
|
||||
b3Float4 aVector = p[0] - center;
|
||||
b3Float4 u = b3Cross3(nearNormal, aVector);
|
||||
b3Float4 v = b3Cross3(nearNormal, u);
|
||||
u = b3FastNormalized3(u);
|
||||
v = b3FastNormalized3(v);
|
||||
|
||||
//keep point with deepest penetration
|
||||
float minW = FLT_MAX;
|
||||
|
||||
int minIndex = -1;
|
||||
|
||||
b3Float4 maxDots;
|
||||
maxDots.x = FLT_MIN;
|
||||
maxDots.y = FLT_MIN;
|
||||
maxDots.z = FLT_MIN;
|
||||
maxDots.w = FLT_MIN;
|
||||
|
||||
// idx, distance
|
||||
for (int ie = 0; ie < nPoints; ie++)
|
||||
{
|
||||
if (p[ie].w < minW)
|
||||
{
|
||||
minW = p[ie].w;
|
||||
minIndex = ie;
|
||||
}
|
||||
float f;
|
||||
b3Float4 r = p[ie] - center;
|
||||
f = b3Dot3F4(u, r);
|
||||
if (f < maxDots.x)
|
||||
{
|
||||
maxDots.x = f;
|
||||
contactIdx[0].x = ie;
|
||||
}
|
||||
|
||||
f = b3Dot3F4(-u, r);
|
||||
if (f < maxDots.y)
|
||||
{
|
||||
maxDots.y = f;
|
||||
contactIdx[0].y = ie;
|
||||
}
|
||||
|
||||
f = b3Dot3F4(v, r);
|
||||
if (f < maxDots.z)
|
||||
{
|
||||
maxDots.z = f;
|
||||
contactIdx[0].z = ie;
|
||||
}
|
||||
|
||||
f = b3Dot3F4(-v, r);
|
||||
if (f < maxDots.w)
|
||||
{
|
||||
maxDots.w = f;
|
||||
contactIdx[0].w = ie;
|
||||
}
|
||||
}
|
||||
|
||||
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
|
||||
{
|
||||
//replace the first contact with minimum (todo: replace contact with least penetration)
|
||||
contactIdx[0].x = minIndex;
|
||||
}
|
||||
|
||||
return 4;
|
||||
}
|
||||
|
||||
#endif //B3_REDUCE_CONTACTS_H
|
||||
#endif //B3_REDUCE_CONTACTS_H
|
||||
|
||||
@@ -7,18 +7,17 @@
|
||||
|
||||
typedef struct b3RigidBodyData b3RigidBodyData_t;
|
||||
|
||||
|
||||
struct b3RigidBodyData
|
||||
{
|
||||
b3Float4 m_pos;
|
||||
b3Quat m_quat;
|
||||
b3Float4 m_linVel;
|
||||
b3Float4 m_angVel;
|
||||
b3Float4 m_pos;
|
||||
b3Quat m_quat;
|
||||
b3Float4 m_linVel;
|
||||
b3Float4 m_angVel;
|
||||
|
||||
int m_collidableIdx;
|
||||
float m_invMass;
|
||||
float m_restituitionCoeff;
|
||||
float m_frictionCoeff;
|
||||
int m_collidableIdx;
|
||||
float m_invMass;
|
||||
float m_restituitionCoeff;
|
||||
float m_frictionCoeff;
|
||||
};
|
||||
|
||||
typedef struct b3InertiaData b3InertiaData_t;
|
||||
@@ -29,6 +28,4 @@ struct b3InertiaData
|
||||
b3Mat3x3 m_initInvInertia;
|
||||
};
|
||||
|
||||
|
||||
#endif //B3_RIGIDBODY_DATA_H
|
||||
|
||||
#endif //B3_RIGIDBODY_DATA_H
|
||||
|
||||
@@ -1,40 +1,35 @@
|
||||
#ifndef B3_UPDATE_AABBS_H
|
||||
#define B3_UPDATE_AABBS_H
|
||||
|
||||
|
||||
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
|
||||
|
||||
void b3ComputeWorldAabb( int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
|
||||
void b3ComputeWorldAabb(int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
|
||||
{
|
||||
__global const b3RigidBodyData_t* body = &bodies[bodyId];
|
||||
|
||||
b3Float4 position = body->m_pos;
|
||||
b3Quat orientation = body->m_quat;
|
||||
|
||||
b3Quat orientation = body->m_quat;
|
||||
|
||||
int collidableIndex = body->m_collidableIdx;
|
||||
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
|
||||
|
||||
if (shapeIndex>=0)
|
||||
|
||||
if (shapeIndex >= 0)
|
||||
{
|
||||
|
||||
b3Aabb_t localAabb = localShapeAABB[collidableIndex];
|
||||
b3Aabb_t worldAabb;
|
||||
|
||||
b3Float4 aabbAMinOut,aabbAMaxOut;
|
||||
|
||||
b3Float4 aabbAMinOut, aabbAMaxOut;
|
||||
float margin = 0.f;
|
||||
b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut);
|
||||
|
||||
worldAabb.m_minVec =aabbAMinOut;
|
||||
b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &aabbAMinOut, &aabbAMaxOut);
|
||||
|
||||
worldAabb.m_minVec = aabbAMinOut;
|
||||
worldAabb.m_minIndices[3] = bodyId;
|
||||
worldAabb.m_maxVec = aabbAMaxOut;
|
||||
worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1;
|
||||
worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass == 0.f ? 0 : 1;
|
||||
worldAabbs[bodyId] = worldAabb;
|
||||
}
|
||||
}
|
||||
|
||||
#endif //B3_UPDATE_AABBS_H
|
||||
#endif //B3_UPDATE_AABBS_H
|
||||
|
||||
Reference in New Issue
Block a user