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

Merge pull request #1749 from erwincoumans/master

Browse Source 
remove getLinkState from API, it automatically calculated forwardKine…
This commit is contained in:
erwincoumans
2018-06-13 03:38:36 +00:00
committed by GitHub
parent ce47fcc7f2 e24da97140
commit 6376197e5e
20 changed files with 384 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

146
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -28,6 +28,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
@@ -442,10 +443,26 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
{
btVector3 m_normalOnBInWorld;
btVector3 m_pointInWorld;
btScalar m_depth;
bool m_hasContact;
btDummyResult()
: m_hasContact(false)
{
}
virtual void setShapeIdentifiersA(int partId0,int index0){}
virtual void setShapeIdentifiersB(int partId1,int index1){}
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{
m_hasContact = true;
m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld;
m_depth = depth;
}
};
@@ -560,15 +577,18 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
} else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
{
btVertexArray vertices;
btVertexArray worldSpaceVertices;
btTriangleShape* tri = (btTriangleShape*)polyhedronB;
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
//tri->initializePolyhedralFeatures();
@@ -579,17 +599,97 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
btScalar maxDist = threshold;
bool foundSepAxis = false;
if (0)
bool useSatSepNormal = true;
if (useSatSepNormal)
{
polyhedronB->initializePolyhedralFeatures();
if (0)
{
//initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle
polyhedronB->initializePolyhedralFeatures();
} else
{
btVector3 uniqueEdges[3] = {tri->m_vertices1[1]-tri->m_vertices1[0],
tri->m_vertices1[2]-tri->m_vertices1[1],
tri->m_vertices1[0]-tri->m_vertices1[2]};
uniqueEdges[0].normalize();
uniqueEdges[1].normalize();
uniqueEdges[2].normalize();
btConvexPolyhedron polyhedron;
polyhedron.m_vertices.push_back(tri->m_vertices1[2]);
polyhedron.m_vertices.push_back(tri->m_vertices1[0]);
polyhedron.m_vertices.push_back(tri->m_vertices1[1]);
{
btFace combinedFaceA;
combinedFaceA.m_indices.push_back(0);
combinedFaceA.m_indices.push_back(1);
combinedFaceA.m_indices.push_back(2);
btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFaceA.m_indices.size();v++)
{
btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFaceA.m_plane[0] = faceNormal[0];
combinedFaceA.m_plane[1] = faceNormal[1];
combinedFaceA.m_plane[2] = faceNormal[2];
combinedFaceA.m_plane[3] = -planeEq;
polyhedron.m_faces.push_back(combinedFaceA);
}
{
btFace combinedFaceB;
combinedFaceB.m_indices.push_back(0);
combinedFaceB.m_indices.push_back(2);
combinedFaceB.m_indices.push_back(1);
btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFaceB.m_indices.size();v++)
{
btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFaceB.m_plane[0] = faceNormal[0];
combinedFaceB.m_plane[1] = faceNormal[1];
combinedFaceB.m_plane[2] = faceNormal[2];
combinedFaceB.m_plane[3] = -planeEq;
polyhedron.m_faces.push_back(combinedFaceB);
}
polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]);
polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]);
polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]);
polyhedron.initialize2();
polyhedronB->setPolyhedralFeatures(polyhedron);
}
foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(),
sepNormalWorldSpace,*resultOut);
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(),
sepNormalWorldSpace,*resultOut);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
} else
}
else
{
#ifdef ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
@@ -598,6 +698,24 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
if (dummy.m_hasContact && dummy.m_depth<0)
{
if (foundSepAxis)
{
if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace)<0.99)
{
printf("?\n");
}
} else
{
printf("!\n");
}
sepNormalWorldSpace.setValue(0,0,1);// = dummy.m_normalOnBInWorld;
//minDist = dummy.m_depth;
foundSepAxis = true;
}
#if 0
btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if (l2>SIMD_EPSILON)
{
@@ -607,6 +725,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
foundSepAxis = true;
}
#endif
}
@@ -614,7 +733,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
{
worldVertsB2.resize(0);
btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
}
@@ -625,6 +744,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
return;
}
}

12
src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
View File

@@ -104,9 +104,9 @@ void btConvexPolyhedron::initialize()
btHashMap<btInternalVertexPair,btInternalEdge> edges;
btScalar TotalArea = 0.0f;
m_localCenter.setValue(0, 0, 0);
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
@@ -172,6 +172,13 @@ void btConvexPolyhedron::initialize()
}
#endif//USE_CONNECTED_FACES
initialize2();
}
void btConvexPolyhedron::initialize2()
{
m_localCenter.setValue(0, 0, 0);
btScalar TotalArea = 0.0f;
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
@@ -274,7 +281,6 @@ void btConvexPolyhedron::initialize()
}
#endif
}
void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
minProj = FLT_MAX;

1
src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
View File

@@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
btVector3 mE;
void initialize();
void initialize2();
bool testContainment() const;
void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;

79
src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
View File

@@ -39,6 +39,17 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
}
}
void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron)
{
if (m_polyhedron)
{
*m_polyhedron = polyhedron;
} else
{
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
m_polyhedron = new (mem) btConvexPolyhedron(polyhedron);
}
}
bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
{
@@ -87,7 +98,71 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
}
#ifndef BT_RECONSTRUCT_FACES
int numVertices = conv.vertices.size();
m_polyhedron->m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
{
m_polyhedron->m_vertices[p] = conv.vertices[p];
}
int v0, v1;
for (int j = 0; j < conv.faces.size(); j++)
{
btVector3 edges[3];
int numEdges = 0;
btFace combinedFace;
const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]];
v0 = edge->getSourceVertex();
int prevVertex=v0;
combinedFace.m_indices.push_back(v0);
v1 = edge->getTargetVertex();
while (v1 != v0)
{
btVector3 wa = conv.vertices[prevVertex];
btVector3 wb = conv.vertices[v1];
btVector3 newEdge = wb-wa;
newEdge.normalize();
if (numEdges<2)
edges[numEdges++] = newEdge;
//face->addIndex(v1);
combinedFace.m_indices.push_back(v1);
edge = edge->getNextEdgeOfFace();
prevVertex = v1;
int v01 = edge->getSourceVertex();
v1 = edge->getTargetVertex();
}
btAssert(combinedFace.m_indices.size() > 2);
btVector3 faceNormal = edges[0].cross(edges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFace.m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFace.m_plane[0] = faceNormal.getX();
combinedFace.m_plane[1] = faceNormal.getY();
combinedFace.m_plane[2] = faceNormal.getZ();
combinedFace.m_plane[3] = -planeEq;
m_polyhedron->m_faces.push_back(combinedFace);
}
#else//BT_RECONSTRUCT_FACES
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
@@ -311,7 +386,9 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
}
#endif //BT_RECONSTRUCT_FACES
m_polyhedron->initialize();
return true;

3
src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
View File

@@ -43,6 +43,9 @@ public:
///experimental/work-in-progress
virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0);
virtual void setPolyhedralFeatures(btConvexPolyhedron& polyhedron);
const btConvexPolyhedron* getConvexPolyhedron() const
{
return m_polyhedron;