mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 21:30:09 +00:00
more work towards cpu pipeline, sharing OpenCL kernel code
This commit is contained in:
erwin coumans
parent
0ccb6922a8
commit
a6d9cf382f
@ -9,7 +9,6 @@
|
||||
#include "Bullet3Dynamics/shared/b3IntegrateTransforms.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.h"
|
||||
|
||||
static b3Vector4 colors[4] =
|
||||
{
|
||||
@ -25,17 +24,16 @@ void RigidBodyDemo::initPhysics(const ConstructionInfo& ci)
|
||||
{
|
||||
m_instancingRenderer = ci.m_instancingRenderer;
|
||||
|
||||
int x_dim=30;
|
||||
int y_dim=30;
|
||||
int z_dim=30;
|
||||
int x_dim=1;
|
||||
int y_dim=2;
|
||||
int z_dim=1;
|
||||
|
||||
int aabbCapacity = x_dim*y_dim*z_dim+1;
|
||||
int aabbCapacity = x_dim*y_dim*z_dim+10;
|
||||
|
||||
b3Config config;
|
||||
|
||||
m_bp = new b3DynamicBvhBroadphase(aabbCapacity);
|
||||
m_np = new b3CpuNarrowPhase(config);
|
||||
m_cd = new b3CpuCollisionWorld(m_bp,m_np);
|
||||
m_rb = new b3CpuRigidBodyPipeline(m_np,m_bp, config);
|
||||
|
||||
|
||||
@ -69,7 +67,6 @@ void RigidBodyDemo::initPhysics(const ConstructionInfo& ci)
|
||||
float mass=0.f;
|
||||
int collidableIndex = m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
|
||||
int bodyIndex = m_rb->getNumBodies();
|
||||
m_cd->addCollidable(bodyIndex,collidableIndex,position,orn);
|
||||
|
||||
|
||||
int userData=-1;
|
||||
@ -82,19 +79,19 @@ void RigidBodyDemo::initPhysics(const ConstructionInfo& ci)
|
||||
|
||||
b3Vector4 scaling=b3MakeVector4(1,1,1,1);
|
||||
int collidableIndex = m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
|
||||
for (int x=-x_dim/2;x<x_dim/2;x++)
|
||||
for (int x=0;x<x_dim;x++)
|
||||
{
|
||||
//for (int y=-y_dim/2;y<y_dim/2;y++)
|
||||
for (int y=1;y<y_dim;y++)
|
||||
for (int y=0;y<y_dim;y++)
|
||||
{
|
||||
for (int z=-z_dim/2;z<z_dim/2;z++)
|
||||
for (int z=0;z<z_dim;z++)
|
||||
{
|
||||
static int curColor=0;
|
||||
b3Vector4 color = colors[curColor];
|
||||
curColor++;
|
||||
curColor&=3;
|
||||
|
||||
b3Vector3 position = b3MakeVector3(x*2,y*2,z*2);
|
||||
b3Vector3 position = b3MakeVector3(x*2,2+y*2,z*2);
|
||||
b3Quaternion orn(0,0,0,1);
|
||||
|
||||
int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
|
||||
@ -102,7 +99,6 @@ void RigidBodyDemo::initPhysics(const ConstructionInfo& ci)
|
||||
|
||||
int userData=-1;
|
||||
int bodyIndex = m_rb->getNumBodies();
|
||||
m_cd->addCollidable(bodyIndex,collidableIndex,position,orn);
|
||||
int rbid = m_rb->registerPhysicsInstance(mass, position, orn, collidableIndex, userData);
|
||||
|
||||
}
|
||||
|
||||
@ -8,8 +8,7 @@ struct RigidBodyDemo : public CpuDemo
|
||||
|
||||
struct b3DynamicBvhBroadphase* m_bp;
|
||||
struct b3CpuNarrowPhase* m_np;
|
||||
class b3CpuCollisionWorld* m_cd;
|
||||
|
||||
|
||||
struct b3CpuRigidBodyPipeline* m_rb;
|
||||
|
||||
GLInstancingRenderer* m_instancingRenderer;
|
||||
|
||||
@ -101,7 +101,7 @@
|
||||
|
||||
if not _OPTIONS["ios"] then
|
||||
|
||||
include "../Demos3/CpuDemos"
|
||||
-- include "../Demos3/CpuDemos"
|
||||
include "../Demos3/GpuDemos"
|
||||
include "../Demos3/Wavefront"
|
||||
include "../btgui/MultiThreading"
|
||||
|
||||
@ -289,12 +289,13 @@ void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3&
|
||||
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::setAabb( b3BroadphaseProxy* absproxy,
|
||||
void b3DynamicBvhBroadphase::setAabb(int objectId,
|
||||
const b3Vector3& aabbMin,
|
||||
const b3Vector3& aabbMax,
|
||||
b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
|
||||
b3DbvtProxy* proxy=&m_proxies[objectId];
|
||||
// b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
|
||||
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax);
|
||||
#if B3_DBVT_BP_PREVENTFALSEUPDATE
|
||||
if(b3NotEqual(aabb,proxy->leaf->volume))
|
||||
@ -440,7 +441,7 @@ void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
|
||||
|
||||
int i;
|
||||
|
||||
b3BroadphasePair previousPair(-1,-1);
|
||||
b3BroadphasePair previousPair = b3MakeBroadphasePair(-1,-1);
|
||||
|
||||
|
||||
|
||||
|
||||
@ -167,7 +167,7 @@ struct b3DynamicBvhBroadphase
|
||||
/* b3BroadphaseInterface Implementation */
|
||||
b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int objectIndex,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask);
|
||||
virtual void destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher);
|
||||
virtual void setAabb(b3BroadphaseProxy* proxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher);
|
||||
virtual void setAabb(int objectId,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher);
|
||||
virtual void rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3MakeVector3(0,0,0), const b3Vector3& aabbMax = b3MakeVector3(0,0,0));
|
||||
virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
|
||||
|
||||
|
||||
@ -21,26 +21,33 @@ subject to the following restrictions:
|
||||
#define B3_NEW_PAIR_MARKER -1
|
||||
#define B3_REMOVED_PAIR_MARKER -2
|
||||
|
||||
//typedef b3Int2 b3BroadphasePair;
|
||||
struct b3BroadphasePair : public b3Int4
|
||||
typedef b3Int4 b3BroadphasePair;
|
||||
|
||||
inline b3Int4 b3MakeBroadphasePair(int xx,int yy)
|
||||
{
|
||||
b3Int4 pair;
|
||||
|
||||
if (xx < yy)
|
||||
{
|
||||
pair.x = xx;
|
||||
pair.y = yy;
|
||||
}
|
||||
else
|
||||
{
|
||||
pair.x = yy;
|
||||
pair.y = xx;
|
||||
}
|
||||
pair.z = B3_NEW_PAIR_MARKER;
|
||||
pair.w = B3_NEW_PAIR_MARKER;
|
||||
return pair;
|
||||
}
|
||||
|
||||
/*struct b3BroadphasePair : public b3Int4
|
||||
{
|
||||
explicit b3BroadphasePair(){}
|
||||
b3BroadphasePair(int xx,int yy)
|
||||
{
|
||||
if (xx < yy)
|
||||
{
|
||||
x = xx;
|
||||
y = yy;
|
||||
}
|
||||
else
|
||||
{
|
||||
x = yy;
|
||||
y = xx;
|
||||
}
|
||||
z = B3_NEW_PAIR_MARKER;
|
||||
w = B3_NEW_PAIR_MARKER;
|
||||
}
|
||||
|
||||
};
|
||||
*/
|
||||
|
||||
class b3BroadphasePairSortPredicate
|
||||
{
|
||||
|
||||
@ -240,7 +240,7 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int
|
||||
}*/
|
||||
int count = m_overlappingPairArray.size();
|
||||
int oldCapacity = m_overlappingPairArray.capacity();
|
||||
void* mem = &m_overlappingPairArray.expandNonInitializing();
|
||||
pair = &m_overlappingPairArray.expandNonInitializing();
|
||||
|
||||
//this is where we add an actual pair, so also call the 'ghost'
|
||||
// if (m_ghostPairCallback)
|
||||
@ -255,7 +255,7 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int
|
||||
hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
|
||||
}
|
||||
|
||||
pair = new (mem) b3BroadphasePair(proxy0,proxy1);
|
||||
*pair = b3MakeBroadphasePair(proxy0,proxy1);
|
||||
|
||||
// pair->m_pProxy0 = proxy0;
|
||||
// pair->m_pProxy1 = proxy1;
|
||||
@ -433,7 +433,7 @@ void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1,
|
||||
{
|
||||
if (!hasDeferredRemoval())
|
||||
{
|
||||
b3BroadphasePair findPair(proxy0,proxy1);
|
||||
b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0,proxy1);
|
||||
|
||||
|
||||
int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
|
||||
@ -470,8 +470,8 @@ b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,in
|
||||
if (!needsBroadphaseCollision(proxy0,proxy1))
|
||||
return 0;
|
||||
|
||||
void* mem = &m_overlappingPairArray.expandNonInitializing();
|
||||
b3BroadphasePair* pair = new (mem) b3BroadphasePair(proxy0,proxy1);
|
||||
b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing();
|
||||
*pair = b3MakeBroadphasePair(proxy0,proxy1);
|
||||
|
||||
|
||||
b3g_overlappingPairs++;
|
||||
@ -492,7 +492,7 @@ b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,in
|
||||
if (!needsBroadphaseCollision(proxy0,proxy1))
|
||||
return 0;
|
||||
|
||||
b3BroadphasePair tmpPair(proxy0,proxy1);
|
||||
b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0,proxy1);
|
||||
int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
|
||||
|
||||
if (findIndex < m_overlappingPairArray.size())
|
||||
|
||||
@ -1,46 +0,0 @@
|
||||
|
||||
#include "b3CpuCollisionWorld.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
|
||||
|
||||
|
||||
b3CpuCollisionWorld::b3CpuCollisionWorld(b3DynamicBvhBroadphase* bp, b3CpuNarrowPhase* np)
|
||||
:m_bp(bp),
|
||||
m_np(np)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
b3CpuCollisionWorld::~b3CpuCollisionWorld()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void b3CpuCollisionWorld::addCollidable(int bodyIndex, int collidableIndex,const b3Vector3& position, const b3Quaternion& orientation)
|
||||
{
|
||||
b3Vector3 aabbMinWorld, aabbMaxWorld;
|
||||
|
||||
if (collidableIndex>=0)
|
||||
{
|
||||
b3Aabb localAabb = m_np->getLocalSpaceAabb(collidableIndex);
|
||||
b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
|
||||
b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
|
||||
|
||||
b3Scalar margin = 0.01f;
|
||||
b3Transform t;
|
||||
t.setIdentity();
|
||||
t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
|
||||
t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
|
||||
b3TransformAabb(localAabbMin,localAabbMax, margin,t,aabbMinWorld,aabbMaxWorld);
|
||||
|
||||
m_bp->createProxy(aabbMinWorld,aabbMaxWorld,bodyIndex,0,1,1);
|
||||
|
||||
b3Vector3 aabbMin,aabbMax;
|
||||
m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
|
||||
}
|
||||
}
|
||||
@ -1,28 +0,0 @@
|
||||
|
||||
#ifndef B3_CPU2_COLLISION_WORLD_H
|
||||
#define B3_CPU2_COLLISION_WORLD_H
|
||||
|
||||
class b3CpuNarrowPhase;
|
||||
struct b3DynamicBvhBroadphase;
|
||||
#include "Bullet3Common/b3Quaternion.h"
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
|
||||
class b3CpuCollisionWorld
|
||||
{
|
||||
protected:
|
||||
|
||||
b3DynamicBvhBroadphase* m_bp;
|
||||
b3CpuNarrowPhase* m_np;
|
||||
|
||||
public:
|
||||
|
||||
b3CpuCollisionWorld(b3DynamicBvhBroadphase* bp, b3CpuNarrowPhase* np);
|
||||
|
||||
void addCollidable(int bodyIndex, int collidableIndex,const b3Vector3& position, const b3Quaternion& orientation);
|
||||
|
||||
virtual ~b3CpuCollisionWorld();
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //B3_CPU_COLLISION_WORLD_H
|
||||
@ -3,6 +3,8 @@
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
|
||||
|
||||
|
||||
struct b3CpuNarrowPhaseInternalData
|
||||
{
|
||||
@ -18,10 +20,21 @@ struct b3CpuNarrowPhaseInternalData
|
||||
b3AlignedObjectArray<int> m_convexIndices;
|
||||
b3AlignedObjectArray<b3GpuFace> m_convexFaces;
|
||||
|
||||
b3AlignedObjectArray<b3Contact4Data> m_contacts;
|
||||
|
||||
int m_numAcceleratedShapes;
|
||||
};
|
||||
|
||||
b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex)
|
||||
{
|
||||
return m_data->m_collidablesCPU[collidableIndex];
|
||||
}
|
||||
|
||||
const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) const
|
||||
{
|
||||
return m_data->m_collidablesCPU[collidableIndex];
|
||||
}
|
||||
|
||||
|
||||
b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
|
||||
{
|
||||
@ -35,9 +48,105 @@ b3CpuNarrowPhase::~b3CpuNarrowPhase()
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>* broadphasePairs, b3AlignedObjectArray<b3Aabb>* aabbsWorldSpace)
|
||||
void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies)
|
||||
{
|
||||
int nPairs = pairs.size();
|
||||
int numContacts = 0;
|
||||
int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
|
||||
m_data->m_contacts.resize(maxContactCapacity);
|
||||
|
||||
for (int i=0;i<nPairs;i++)
|
||||
{
|
||||
int bodyIndexA = pairs[i].x;
|
||||
int bodyIndexB = pairs[i].y;
|
||||
int collidableIndexA = bodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = bodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
//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");
|
||||
|
||||
}
|
||||
|
||||
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");
|
||||
|
||||
}
|
||||
|
||||
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");
|
||||
|
||||
}
|
||||
|
||||
|
||||
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");
|
||||
|
||||
}
|
||||
|
||||
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");
|
||||
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
//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);
|
||||
|
||||
|
||||
if (contactIndex>=0)
|
||||
{
|
||||
pairs[i].z = contactIndex;
|
||||
}
|
||||
// printf("plane-convex\n");
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
m_data->m_contacts.resize(numContacts);
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
class b3CpuNarrowPhase
|
||||
{
|
||||
@ -54,8 +55,8 @@ public:
|
||||
|
||||
|
||||
//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
|
||||
virtual void computeContacts(b3AlignedObjectArray<b3Int4>* broadphasePairs, b3AlignedObjectArray<b3Aabb>* aabbsWorldSpace);
|
||||
|
||||
virtual void computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies);
|
||||
|
||||
|
||||
|
||||
const struct b3RigidBodyCL* getBodiesCpu() const;
|
||||
|
||||
@ -0,0 +1,523 @@
|
||||
|
||||
#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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
|
||||
int ve;
|
||||
float ds, de;
|
||||
int numVertsOut = 0;
|
||||
if (numVertsIn < 2)
|
||||
return 0;
|
||||
|
||||
b3Float4 firstVertex=pVtxIn[numVertsIn-1];
|
||||
b3Float4 endVertex = pVtxIn[0];
|
||||
|
||||
ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS;
|
||||
|
||||
for (ve = 0; ve < numVertsIn; ve++)
|
||||
{
|
||||
endVertex=pVtxIn[ve];
|
||||
|
||||
de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS;
|
||||
|
||||
if (ds<0)
|
||||
{
|
||||
if (de<0)
|
||||
{
|
||||
// Start < 0, end < 0, so output endVertex
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Start < 0, end >= 0, so output intersection
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (de<0)
|
||||
{
|
||||
// Start >= 0, end < 0 so output intersection and end
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
}
|
||||
firstVertex = endVertex;
|
||||
ds = de;
|
||||
}
|
||||
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)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
|
||||
b3Float4* pVtxIn = worldVertsB1;
|
||||
b3Float4* pVtxOut = worldVertsB2;
|
||||
|
||||
int numVertsIn = numWorldVertsB1;
|
||||
int numVertsOut = 0;
|
||||
|
||||
int closestFaceA=-1;
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
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);
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
closestFaceA = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (closestFaceA<0)
|
||||
return numContactsOut;
|
||||
|
||||
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++)
|
||||
{
|
||||
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);
|
||||
|
||||
b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1);
|
||||
b3Float4 worldA1 = b3TransformPoint(a,posA,ornA);
|
||||
float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1);
|
||||
|
||||
b3Float4 planeNormalWS = planeNormalWS1;
|
||||
float planeEqWS=planeEqWS1;
|
||||
|
||||
//clip face
|
||||
//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
|
||||
numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut);
|
||||
|
||||
//btSwap(pVtxIn,pVtxOut);
|
||||
b3Float4* tmp = pVtxOut;
|
||||
pVtxOut = pVtxIn;
|
||||
pVtxIn = tmp;
|
||||
numVertsIn = numVertsOut;
|
||||
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);
|
||||
float localPlaneEq = polyA.m_plane.w;
|
||||
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)
|
||||
{
|
||||
depth = minDist;
|
||||
}
|
||||
if (numContactsOut<contactCapacity)
|
||||
{
|
||||
if (depth <=maxDist)
|
||||
{
|
||||
b3Float4 pointInWorld = pVtxIn[i];
|
||||
//resultOut.addContactPoint(separatingNormal,point,depth);
|
||||
contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
|
||||
//printf("depth=%f\n",depth);
|
||||
}
|
||||
} else
|
||||
{
|
||||
b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut,contactCapacity);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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,
|
||||
|
||||
b3Float4* contactsOut,
|
||||
int contactCapacity)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
int numWorldVertsB1= 0;
|
||||
|
||||
B3_PROFILE("clipHullAgainstHull");
|
||||
|
||||
float curMaxDist=maxDist;
|
||||
int closestFaceB=-1;
|
||||
float dmax = -FLT_MAX;
|
||||
|
||||
{
|
||||
//B3_PROFILE("closestFaceB");
|
||||
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++)
|
||||
{
|
||||
#ifdef BT_DEBUG_SAT_FACE
|
||||
if (once)
|
||||
printf("face %d\n",face);
|
||||
const b3GpuFace* faceB = &facesB[hullB.m_faceOffset+face];
|
||||
if (once)
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
#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 WorldNormal = b3QuatRotate(ornB, Normal);
|
||||
#ifdef BT_DEBUG_SAT_FACE
|
||||
if (once)
|
||||
printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z);
|
||||
#endif
|
||||
float d = b3Dot3F4(WorldNormal,separatingNormal);
|
||||
if (d > dmax)
|
||||
{
|
||||
dmax = d;
|
||||
closestFaceB = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
once = false;
|
||||
}
|
||||
|
||||
|
||||
b3Assert(closestFaceB>=0);
|
||||
{
|
||||
//B3_PROFILE("worldVertsB1");
|
||||
const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB];
|
||||
const int numVertices = polyB.m_numIndices;
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
if (closestFaceB>=0)
|
||||
{
|
||||
//B3_PROFILE("clipFaceAgainstHull");
|
||||
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 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<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);
|
||||
|
||||
|
||||
b3Float4 contactsOut[B3_MAX_VERTS];
|
||||
int localContactCapacity = B3_MAX_VERTS;
|
||||
|
||||
#ifdef _WIN32
|
||||
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);
|
||||
int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
|
||||
int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
|
||||
|
||||
b3Scalar minDist = -1;
|
||||
b3Scalar maxDist = 0.;
|
||||
|
||||
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
if (numContactsOut>0)
|
||||
{
|
||||
B3_PROFILE("overlap");
|
||||
|
||||
b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
|
||||
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);
|
||||
}
|
||||
|
||||
b3Assert(numPoints);
|
||||
|
||||
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_frictionCoeffCmp = 45874;
|
||||
contact.m_restituitionCoeffCmp = 0;
|
||||
|
||||
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;
|
||||
}
|
||||
//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
|
||||
{
|
||||
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 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);
|
||||
|
||||
|
||||
b3Float4 contactsOut[B3_MAX_VERTS];
|
||||
int contactCapacity = B3_MAX_VERTS;
|
||||
int numContactsOut=0;
|
||||
|
||||
|
||||
#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
|
||||
);
|
||||
|
||||
|
||||
if (foundSepAxis)
|
||||
{
|
||||
|
||||
|
||||
contactIndex = b3ClipHullHullSingle(
|
||||
bodyIndexA, bodyIndexB,
|
||||
posA,ornA,
|
||||
posB,ornB,
|
||||
collidableIndexA, collidableIndexB,
|
||||
&rigidBodies,
|
||||
&globalContactsOut,
|
||||
nGlobalContactsOut,
|
||||
|
||||
convexShapes,
|
||||
convexShapes,
|
||||
|
||||
convexVertices,
|
||||
uniqueEdges,
|
||||
faces,
|
||||
convexIndices,
|
||||
|
||||
convexVertices,
|
||||
uniqueEdges,
|
||||
faces,
|
||||
convexIndices,
|
||||
|
||||
collidables,
|
||||
collidables,
|
||||
sepNormalWorldSpace,
|
||||
maxContactCapacity);
|
||||
|
||||
}
|
||||
|
||||
return contactIndex;
|
||||
}
|
||||
|
||||
#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
|
||||
@ -0,0 +1,162 @@
|
||||
|
||||
#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 b3RigidBodyCL* rigidBodies,
|
||||
const b3Collidable* collidables,
|
||||
const b3ConvexPolyhedronCL* 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;
|
||||
|
||||
b3Transform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(pos);
|
||||
tr.setRotation(quat);
|
||||
b3Transform trInv = tr.inverse();
|
||||
|
||||
float4 spherePos = trInv(spherePos1);
|
||||
|
||||
int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
|
||||
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?
|
||||
bool bCollide = true;
|
||||
int region = -1;
|
||||
float4 localHitNormal;
|
||||
for ( int f = 0; f < numFaces; 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);
|
||||
planeEqn = n1;
|
||||
planeEqn[3] = face.m_plane.w;
|
||||
|
||||
float4 pntReturn;
|
||||
float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
|
||||
|
||||
if ( dist > radius)
|
||||
{
|
||||
bCollide = false;
|
||||
break;
|
||||
}
|
||||
|
||||
if ( dist > 0 )
|
||||
{
|
||||
//might hit an edge or vertex
|
||||
b3Vector3 out;
|
||||
|
||||
bool isInPoly = IsPointInPolygon(spherePos,
|
||||
&face,
|
||||
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
|
||||
convexIndices,
|
||||
&out);
|
||||
if (isInPoly)
|
||||
{
|
||||
if (dist>minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = pntReturn;
|
||||
localHitNormal = planeEqn;
|
||||
region=1;
|
||||
}
|
||||
} else
|
||||
{
|
||||
b3Vector3 tmp = spherePos-out;
|
||||
b3Scalar l2 = tmp.length2();
|
||||
if (l2<radius*radius)
|
||||
{
|
||||
dist = b3Sqrt(l2);
|
||||
if (dist>minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = out;
|
||||
localHitNormal = tmp/dist;
|
||||
region=2;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
bCollide = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( dist > minDist )
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = pntReturn;
|
||||
localHitNormal = planeEqn;
|
||||
region=3;
|
||||
}
|
||||
}
|
||||
}
|
||||
static int numChecks = 0;
|
||||
numChecks++;
|
||||
|
||||
if (bCollide && minDist > -10000)
|
||||
{
|
||||
|
||||
float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld;
|
||||
float4 pOnB1 = tr(closestPnt);
|
||||
//printf("dist ,%f,",minDist);
|
||||
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;
|
||||
|
||||
int dstIdx;
|
||||
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
|
||||
|
||||
if (nGlobalContactsOut < maxContactCapacity)
|
||||
{
|
||||
dstIdx=nGlobalContactsOut;
|
||||
nGlobalContactsOut++;
|
||||
|
||||
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)
|
||||
|
||||
}
|
||||
#endif //B3_CONTACT_SPHERE_SPHERE_H
|
||||
@ -0,0 +1,206 @@
|
||||
#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)
|
||||
{
|
||||
min = FLT_MAX;
|
||||
max = -FLT_MAX;
|
||||
int numVerts = hull.m_numVertices;
|
||||
|
||||
const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir);
|
||||
|
||||
b3Scalar offset = b3Dot3F4(pos,dir);
|
||||
|
||||
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);
|
||||
//b3Assert(dp==dpL);
|
||||
if(dp < min) min = dp;
|
||||
if(dp > max) max = dp;
|
||||
}
|
||||
if(min>max)
|
||||
{
|
||||
b3Scalar tmp = min;
|
||||
min = max;
|
||||
max = tmp;
|
||||
}
|
||||
min += offset;
|
||||
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)
|
||||
{
|
||||
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)
|
||||
return false;
|
||||
|
||||
b3Scalar d0 = Max0 - Min1;
|
||||
b3Assert(d0>=0.0f);
|
||||
b3Scalar d1 = Max1 - Min0;
|
||||
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,
|
||||
|
||||
b3Vector3& sep)
|
||||
{
|
||||
B3_PROFILE("findSeparatingAxis");
|
||||
|
||||
b3Float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
//#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);
|
||||
const b3Float4 deltaC2 = c0 - c1;
|
||||
//#endif
|
||||
|
||||
b3Scalar dmin = FLT_MAX;
|
||||
int curPlaneTests=0;
|
||||
|
||||
int numFacesA = hullA.m_numFaces;
|
||||
// Test normals from hullA
|
||||
for(int i=0;i<numFacesA;i++)
|
||||
{
|
||||
const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset+i].m_plane;
|
||||
b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
|
||||
|
||||
if (b3Dot3F4(deltaC2,faceANormalWS)<0)
|
||||
faceANormalWS*=-1.f;
|
||||
|
||||
curPlaneTests++;
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
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))
|
||||
return false;
|
||||
|
||||
if(d<dmin)
|
||||
{
|
||||
dmin = d;
|
||||
sep = (b3Vector3&)faceANormalWS;
|
||||
}
|
||||
}
|
||||
|
||||
int numFacesB = hullB.m_numFaces;
|
||||
// Test normals from hullB
|
||||
for(int i=0;i<numFacesB;i++)
|
||||
{
|
||||
b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset+i].m_plane;
|
||||
b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
|
||||
|
||||
if (b3Dot3F4(deltaC2,WorldNormal)<0)
|
||||
{
|
||||
WorldNormal*=-1.f;
|
||||
}
|
||||
curPlaneTests++;
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
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))
|
||||
return false;
|
||||
|
||||
if(d<dmin)
|
||||
{
|
||||
dmin = d;
|
||||
sep = (b3Vector3&)WorldNormal;
|
||||
}
|
||||
}
|
||||
|
||||
b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
|
||||
|
||||
int curEdgeEdge = 0;
|
||||
// Test edges
|
||||
for(int e0=0;e0<hullA.m_numUniqueEdges;e0++)
|
||||
{
|
||||
const b3Float4& edge0 = (b3Float4&) uniqueEdgesA[hullA.m_uniqueEdgesOffset+e0];
|
||||
b3Float4 edge0World = b3QuatRotate(ornA,(b3Float4&)edge0);
|
||||
|
||||
for(int e1=0;e1<hullB.m_numUniqueEdges;e1++)
|
||||
{
|
||||
const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset+e1];
|
||||
b3Float4 edge1World = b3QuatRotate(ornB,(b3Float4&)edge1);
|
||||
|
||||
|
||||
b3Float4 crossje = b3Cross3(edge0World,edge1World);
|
||||
|
||||
curEdgeEdge++;
|
||||
if(!b3IsAlmostZero((b3Vector3&)crossje))
|
||||
{
|
||||
crossje = b3FastNormalized3(crossje);
|
||||
if (b3Dot3F4(deltaC2,crossje)<0)
|
||||
crossje*=-1.f;
|
||||
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
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))
|
||||
return false;
|
||||
|
||||
if(dist<dmin)
|
||||
{
|
||||
dmin = dist;
|
||||
sep = (b3Vector3&)crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
if((b3Dot3F4(-deltaC2,(b3Float4&)sep))>0.0f)
|
||||
sep = -sep;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif //B3_FIND_SEPARATING_AXIS_H
|
||||
|
||||
@ -0,0 +1,97 @@
|
||||
#ifndef B3_REDUCE_CONTACTS_H
|
||||
#define B3_REDUCE_CONTACTS_H
|
||||
|
||||
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);
|
||||
{
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
#endif //B3_REDUCE_CONTACTS_H
|
||||
@ -0,0 +1,31 @@
|
||||
#ifndef B3_UPDATE_AABBS_H
|
||||
#define B3_UPDATE_AABBS_H
|
||||
|
||||
|
||||
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3CollidableData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
|
||||
|
||||
void b3ComputeWorldAabb( int bodyId, b3RigidBodyData* body, b3CollidableData* collidables, b3Aabb* localShapeAABB, b3Aabb* worldAabbs)
|
||||
{
|
||||
b3Float4 position = body->m_pos;
|
||||
b3Quat orientation = body->m_quat;
|
||||
|
||||
int collidableIndex = body->m_collidableIdx;
|
||||
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
|
||||
|
||||
if (shapeIndex>=0)
|
||||
{
|
||||
|
||||
b3Aabb localAabb = localShapeAABB[shapeIndex];
|
||||
b3Aabb worldAabb;
|
||||
|
||||
b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec);
|
||||
worldAabbs[bodyId] = worldAabb;
|
||||
}
|
||||
}
|
||||
|
||||
#endif //B3_UPDATE_AABBS_H
|
||||
@ -10,6 +10,13 @@
|
||||
#define b3Dot3F4 b3Dot
|
||||
#define b3Cross3 b3Cross
|
||||
#define b3MakeFloat4 b3MakeVector3
|
||||
|
||||
inline b3Float4 b3FastNormalized3(b3Float4ConstArg v)
|
||||
{
|
||||
return v.normalized();
|
||||
}
|
||||
|
||||
|
||||
#else
|
||||
typedef float4 b3Float4;
|
||||
#define b3Float4ConstArg const b3Float4
|
||||
@ -28,4 +35,15 @@
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
inline bool b3IsAlmostZero(b3Float4ConstArg v)
|
||||
{
|
||||
if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif //B3_FLOAT4_H
|
||||
|
||||
@ -5,11 +5,15 @@
|
||||
#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3CollidableData.h"
|
||||
|
||||
|
||||
struct b3CpuRigidBodyPipelineInternalData
|
||||
{
|
||||
b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
|
||||
b3AlignedObjectArray<b3Aabb> m_aabbWorldSpace;
|
||||
|
||||
b3DynamicBvhBroadphase* m_bp;
|
||||
b3CpuNarrowPhase* m_np;
|
||||
b3Config m_config;
|
||||
@ -32,6 +36,27 @@ b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
|
||||
void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
|
||||
{
|
||||
|
||||
for (int i=0;i<this->getNumBodies();i++)
|
||||
{
|
||||
b3RigidBodyData* body = &m_data->m_rigidBodies[i];
|
||||
b3Float4 position = body->m_pos;
|
||||
b3Quat orientation = body->m_quat;
|
||||
|
||||
int collidableIndex = body->m_collidableIdx;
|
||||
b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex);
|
||||
int shapeIndex = collidable.m_shapeIndex;
|
||||
|
||||
if (shapeIndex>=0)
|
||||
{
|
||||
|
||||
|
||||
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex);
|
||||
b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i];
|
||||
float margin=0.f;
|
||||
b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec);
|
||||
m_data->m_bp->setAabb(i,worldAabb.m_minVec,worldAabb.m_maxVec,0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3CpuRigidBodyPipeline::computeOverlappingPairs()
|
||||
@ -39,15 +64,15 @@ void b3CpuRigidBodyPipeline::computeOverlappingPairs()
|
||||
int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
|
||||
m_data->m_bp->calculateOverlappingPairs();
|
||||
numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
|
||||
//printf("numPairs=%d\n",numPairs);
|
||||
}
|
||||
|
||||
void b3CpuRigidBodyPipeline::computeContactPoints()
|
||||
{
|
||||
b3AlignedObjectArray<b3Aabb> aabbWorldSpace;
|
||||
b3AlignedObjectArray<b3Int4> pairs;
|
||||
|
||||
b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray();
|
||||
|
||||
m_data->m_np->computeContacts(&pairs,&aabbWorldSpace);
|
||||
m_data->m_np->computeContacts(pairs,m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
|
||||
|
||||
}
|
||||
void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
|
||||
@ -87,7 +112,7 @@ void b3CpuRigidBodyPipeline::integrate(float deltaTime)
|
||||
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
|
||||
{
|
||||
b3RigidBodyData body;
|
||||
int index = m_data->m_rigidBodies.size();
|
||||
int bodyIndex = m_data->m_rigidBodies.size();
|
||||
body.m_invMass = mass ? 1.f/mass : 0.f;
|
||||
body.m_angVel.setValue(0,0,0);
|
||||
body.m_collidableIdx = collidableIndex;
|
||||
@ -99,15 +124,32 @@ int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
|
||||
|
||||
m_data->m_rigidBodies.push_back(body);
|
||||
|
||||
|
||||
if (collidableIndex>=0)
|
||||
{
|
||||
b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
|
||||
|
||||
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
|
||||
b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
|
||||
b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
|
||||
|
||||
b3Scalar margin = 0.01f;
|
||||
b3Transform t;
|
||||
t.setIdentity();
|
||||
t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
|
||||
t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
|
||||
b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec);
|
||||
|
||||
m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1);
|
||||
// b3Vector3 aabbMin,aabbMax;
|
||||
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
|
||||
|
||||
} else
|
||||
{
|
||||
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
return index;
|
||||
return bodyIndex;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -227,10 +227,9 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
|
||||
m_data->m_allAabbsGPU->copyToHost(m_data->m_allAabbsCPU);
|
||||
for (int i=0;i<m_data->m_allAabbsCPU.size();i++)
|
||||
{
|
||||
b3BroadphaseProxy* proxy = &m_data->m_broadphaseDbvt->m_proxies[i];
|
||||
b3Vector3 aabbMin=b3MakeVector3(m_data->m_allAabbsCPU[i].m_min[0],m_data->m_allAabbsCPU[i].m_min[1],m_data->m_allAabbsCPU[i].m_min[2]);
|
||||
b3Vector3 aabbMax=b3MakeVector3(m_data->m_allAabbsCPU[i].m_max[0],m_data->m_allAabbsCPU[i].m_max[1],m_data->m_allAabbsCPU[i].m_max[2]);
|
||||
m_data->m_broadphaseDbvt->setAabb(proxy,aabbMin,aabbMax,0);
|
||||
m_data->m_broadphaseDbvt->setAabb(i,aabbMin,aabbMax,0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user