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

x() -> x or getX() or [0]

Browse Source 
y() -> y or getY() or [1]
z() -> z or getZ() or [2]
w() -> w or getW() or [3]

make sphere-convex and sphere-compound collision work (still issues remaining)
This commit is contained in:
erwin coumans
2013-04-03 18:27:36 -07:00
parent 8cee2e9b23
commit 4a93c2e704
23 changed files with 692 additions and 315 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
btgui/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -959,15 +959,15 @@ void btCreateLookAt(const btVector3& eye, const btVector3& center,const btVec
btVector3 s = (f.cross(u)).normalized();
u = s.cross(f);
result[0*4+0] = s.x();
result[1*4+0] = s.y();
result[2*4+0] = s.z();
result[0*4+1] = u.x();
result[1*4+1] = u.y();
result[2*4+1] = u.z();
result[0*4+2] =-f.x();
result[1*4+2] =-f.y();
result[2*4+2] =-f.z();
result[0*4+0] = s.getX();
result[1*4+0] = s.getY();
result[2*4+0] = s.getZ();
result[0*4+1] = u.getX();
result[1*4+1] = u.getY();
result[2*4+1] = u.getZ();
result[0*4+2] =-f.getX();
result[1*4+2] =-f.getY();
result[2*4+2] =-f.getZ();
result[3*4+0] = -s.dot(eye);
result[3*4+1] = -u.dot(eye);
@@ -1063,9 +1063,9 @@ void GLInstancingRenderer::updateCamera()
void GLInstancingRenderer::getCameraPosition(float cameraPos[4])
{
cameraPos[0] = m_data->m_cameraPosition.x();
cameraPos[1] = m_data->m_cameraPosition.y();
cameraPos[2] = m_data->m_cameraPosition.z();
cameraPos[0] = m_data->m_cameraPosition[0];
cameraPos[1] = m_data->m_cameraPosition[1];
cameraPos[2] = m_data->m_cameraPosition[2];
cameraPos[3] = 1.f;
}
@@ -1090,9 +1090,9 @@ void GLInstancingRenderer::setCameraTargetPosition(float cameraPos[4])
void GLInstancingRenderer::getCameraTargetPosition(float cameraPos[4]) const
{
cameraPos[0] = m_data->m_cameraTargetPosition.x();
cameraPos[1] = m_data->m_cameraTargetPosition.y();
cameraPos[2] = m_data->m_cameraTargetPosition.z();
cameraPos[0] = m_data->m_cameraTargetPosition.getX();
cameraPos[1] = m_data->m_cameraTargetPosition.getY();
cameraPos[2] = m_data->m_cameraTargetPosition.getZ();
}

12
demo/donttouch/OpenGL3CoreRenderer.cpp
View File

@@ -155,9 +155,9 @@ GraphicsShape* createGraphicsShapeFromCompoundShape(btCompoundShape* compound)
GraphicsVertex vtx;
btVector3 pos(orgVerts[j].xyzw[0],orgVerts[j].xyzw[1],orgVerts[j].xyzw[2]);
pos = tr*pos;
vtx.xyzw[0] = childGfxShape->m_scaling[0]*pos.x();
vtx.xyzw[1] = childGfxShape->m_scaling[1]*pos.y();
vtx.xyzw[2] = childGfxShape->m_scaling[2]*pos.z();
vtx.xyzw[0] = childGfxShape->m_scaling[0]*pos.getX();
vtx.xyzw[1] = childGfxShape->m_scaling[1]*pos.getY();
vtx.xyzw[2] = childGfxShape->m_scaling[2]*pos.getZ();
vtx.xyzw[3] = 10.f;
vtx.uv[0] = 0.5f;
@@ -165,9 +165,9 @@ GraphicsShape* createGraphicsShapeFromCompoundShape(btCompoundShape* compound)
btVector3 normal(orgVerts[j].normal[0],orgVerts[j].normal[1],orgVerts[j].normal[2]);
normal = tr.getBasis()*normal;
vtx.normal[0] = normal.x();
vtx.normal[1] = normal.y();
vtx.normal[2] = normal.z();
vtx.normal[0] = normal.getX();
vtx.normal[1] = normal.getY();
vtx.normal[2] = normal.getZ();
vertexArray->push_back(vtx);
}
}

6
demo/gpudemo/GpuDemo.h
View File

@@ -38,9 +38,9 @@ public:
:useOpenCL(true),
preferredOpenCLPlatformIndex(-1),
preferredOpenCLDeviceIndex(-1),
arraySizeX(25),
arraySizeY(23),
arraySizeZ(23),
arraySizeX(15),
arraySizeY(15),
arraySizeZ(15),
m_useConcaveMesh(false),
gapX(14.3),
gapY(14.0),

11
demo/gpudemo/main_opengl3core.cpp
View File

@@ -65,11 +65,16 @@ btAlignedObjectArray<const char*> demoNames;
int selectedDemo = 0;
GpuDemo::CreateFunc* allDemos[]=
{
GpuSphereScene::MyCreateFunc,
GpuCompoundScene::MyCreateFunc,
GpuConvexScene::MyCreateFunc,
ConcaveScene::MyCreateFunc,
GpuCompoundScene::MyCreateFunc,
PairBench::MyCreateFunc,
@@ -363,10 +368,14 @@ void DumpSimulationTime(FILE* f)
///extern const char* g_deviceName;
const char* g_deviceName = "blaat";
extern bool useNewBatchingKernel;
#include "BulletCommon/btVector3.h"
int main(int argc, char* argv[])
{
btVector3 test(1,2,3);
test.x = 1;
test.y = 4;
printf("main start");

85
demo/gpudemo/rigidbody/GpuCompoundScene.cpp
View File

@@ -32,15 +32,84 @@ void GpuCompoundScene::setupScene(const ConstructionInfo& ci)
btAlignedObjectArray<int> indexArray;
//int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
int group=1;
int mask=1;
int index=10;
int index=0;
float scaling[4] = {1,1,1,1};
int colIndex = 0;
{
if (1)
{
float radius = 41;
int prevGraphicsShapeIndex = -1;
{
if (radius>=100)
{
int numVertices = sizeof(detailed_sphere_vertices)/strideInBytes;
int numIndices = sizeof(detailed_sphere_indices)/sizeof(int);
prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&detailed_sphere_vertices[0],numVertices,detailed_sphere_indices,numIndices);
} else
{
bool usePointSprites = false;
if (usePointSprites)
{
int numVertices = sizeof(point_sphere_vertices)/strideInBytes;
int numIndices = sizeof(point_sphere_indices)/sizeof(int);
prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&point_sphere_vertices[0],numVertices,point_sphere_indices,numIndices,BT_GL_POINTS);
} else
{
if (radius>=10)
{
int numVertices = sizeof(medium_sphere_vertices)/strideInBytes;
int numIndices = sizeof(medium_sphere_indices)/sizeof(int);
prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&medium_sphere_vertices[0],numVertices,medium_sphere_indices,numIndices);
} else
{
int numVertices = sizeof(low_sphere_vertices)/strideInBytes;
int numIndices = sizeof(low_sphere_indices)/sizeof(int);
prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&low_sphere_vertices[0],numVertices,low_sphere_indices,numIndices);
}
}
}
}
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 1;
//int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
int colIndex = m_data->m_np->registerSphereShape(radius);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
float mass = 0.f;
//btVector3 position((j&1)+i*2.2,1+j*2.,(j&1)+k*2.2);
btVector3 position(0,-41,0);
btQuaternion orn(0,0,0,1);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
btVector4 scaling(radius,radius,radius,1);
int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling);
int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index);
index++;
}
}
GLInstanceVertex* cubeVerts = (GLInstanceVertex*)&cube_vertices[0];
int stride2 = sizeof(GLInstanceVertex);
btAssert(stride2 == strideInBytes);
@@ -48,12 +117,14 @@ void GpuCompoundScene::setupScene(const ConstructionInfo& ci)
{
int childColIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
btVector3 childPositions[3] = {
btVector3(0,-2,0),
btVector3(0,0,0),
btVector3(0,2,0)
};
btAlignedObjectArray<btGpuChildShape> childShapes;
int numChildShapes = 3;
for (int i=0;i<numChildShapes;i++)
@@ -96,6 +167,7 @@ void GpuCompoundScene::setupScene(const ConstructionInfo& ci)
}
//int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
int shapeId = ci.m_instancingRenderer->registerShape(&vertexArray[0].xyzw[0],vertexArray.size(),&indexArray[0],indexArray.size());
btVector4 colors[4] =
@@ -113,10 +185,11 @@ void GpuCompoundScene::setupScene(const ConstructionInfo& ci)
{
for (int k=0;k<ci.arraySizeZ;k++)
{
float mass = j==0? 0.f : 1.f;
float mass = 1;//j==0? 0.f : 1.f;
btVector3 position(i*ci.gapX,j*ci.gapY,k*ci.gapZ);
btQuaternion orn(1,0,0,0);
btVector3 position(i*ci.gapX,10+j*ci.gapY,k*ci.gapZ);
//btQuaternion orn(0,0,0,1);
btQuaternion orn(btVector3(1,0,0),0.7);
btVector4 color = colors[curColor];
curColor++;

118
demo/gpudemo/rigidbody/GpuSphereScene.cpp
View File

@@ -32,36 +32,87 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
int mask=1;
int index=0;
if (1)
if (0)
{
float radius = 40;
int prevGraphicsShapeIndex = -1;
{
int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
btVector4 scaling(120,2,120,1);
int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
btVector3 normal(0,-1,0);
float constant=2;
//int colIndex = m_data->m_np->registerPlaneShape(normal,constant);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
btVector4 position(0,50,0,0);
//btQuaternion orn(0,0,0,1);
btQuaternion orn(btVector3(1,0,0),0.3);
btVector4 color(0,0,1,1);
int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index);
if (1)//radius>=100)
{
int numVertices = sizeof(detailed_sphere_vertices)/strideInBytes;
int numIndices = sizeof(detailed_sphere_indices)/sizeof(int);
prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&detailed_sphere_vertices[0],numVertices,detailed_sphere_indices,numIndices);
} else
{
bool usePointSprites = false;
if (usePointSprites)
{
int numVertices = sizeof(point_sphere_vertices)/strideInBytes;
int numIndices = sizeof(point_sphere_indices)/sizeof(int);
prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&point_sphere_vertices[0],numVertices,point_sphere_indices,numIndices,BT_GL_POINTS);
} else
{
if (radius>=10)
{
int numVertices = sizeof(medium_sphere_vertices)/strideInBytes;
int numIndices = sizeof(medium_sphere_indices)/sizeof(int);
prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&medium_sphere_vertices[0],numVertices,medium_sphere_indices,numIndices);
} else
{
int numVertices = sizeof(low_sphere_vertices)/strideInBytes;
int numIndices = sizeof(low_sphere_indices)/sizeof(int);
prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&low_sphere_vertices[0],numVertices,low_sphere_indices,numIndices);
}
}
}
}
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
//int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
int colIndex = m_data->m_np->registerSphereShape(radius);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
float mass = 0.f;
//btVector3 position((j&1)+i*2.2,1+j*2.,(j&1)+k*2.2);
btVector3 position(0,-40,0);
btQuaternion orn(0,0,0,1);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
btVector4 scaling(radius,radius,radius,1);
int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling);
int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index);
index++;
}
{
int prevGraphicsShapeIndex = -1;
float radius = 1;
if (radius>=100)
float radius = 41;
if (1)//radius>=100)
{
int numVertices = sizeof(detailed_sphere_vertices)/strideInBytes;
int numIndices = sizeof(detailed_sphere_indices)/sizeof(int);
@@ -119,10 +170,10 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
{
for (int k=0;k<ci.arraySizeZ;k++)
{
float mass = 1.f;
float mass = 0.f;
//btVector3 position((j&1)+i*2.2,1+j*2.,(j&1)+k*2.2);
btVector3 position(i*radius*3,j*radius*3,k*radius*3);
btVector3 position((j&1)+i*142.2,-51+j*142.,(j&1)+k*142.2);
//btVector3 position(0,-41,0);//0,0,0);//i*radius*3,-41+j*radius*3,k*radius*3);
btQuaternion orn(0,0,0,1);
@@ -138,10 +189,39 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
}
}
}
if (1)
{
int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
btVector4 scaling(0.5,0.5,0.5,1);//1,1,1,1);//0.1,0.1,0.1,1);
int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
btVector3 normal(0,-1,0);
float constant=2;
for (int j=-10;j<10;j++)
for (int i=-10;i<10;i++)
for (int k=0;k<30;k++)
//int i=0;int j=0;
{
//int colIndex = m_data->m_np->registerPlaneShape(normal,constant);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
btVector4 position(2*i,k*2,2*j+8,0);
//btQuaternion orn(0,0,0,1);
btQuaternion orn(btVector3(1,0,0),0.3);
btVector4 color(0,0,1,1);
int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(1.f,position,orn,colIndex,index);
index++;
}
}
float camPos[4]={ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0};
//float camPos[4]={1,12.5,1.5,0};
m_instancingRenderer->setCameraTargetPosition(camPos);
m_instancingRenderer->setCameraDistance(150);
m_instancingRenderer->setCameraDistance(30);
char msg[1024];

5
opencl/basic_initialize/btOpenCLUtils.cpp
View File

@@ -525,6 +525,11 @@ cl_program btOpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
{
const char* additionalMacros = additionalMacrosArg?additionalMacrosArg:"";
if (disableBinaryCaching)
{
kernelSourceOrg = 0;
}
cl_program m_cpProgram=0;
cl_int status;

8
opencl/gpu_rigidbody/host/Solver.cpp
View File

@@ -344,8 +344,8 @@ void solveContact(btGpuConstraint4& cs,
btVector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
btVector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x()));
btAssert(_finite(linImp1.x()));
btAssert(_finite(linImp0.getX()));
btAssert(_finite(linImp1.getX()));
#endif
if( JACOBI )
{
@@ -427,8 +427,8 @@ void solveContact(btGpuConstraint4& cs,
btVector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
btVector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x()));
btAssert(_finite(linImp1.x()));
btAssert(_finite(linImp0.getX()));
btAssert(_finite(linImp1.getX()));
#endif
linVelA += linImp0;
angVelA += angImp0;

4
opencl/gpu_rigidbody/host/btConfig.h
View File

@@ -6,6 +6,7 @@ struct btConfig
int m_maxConvexBodies;
int m_maxConvexShapes;
int m_maxBroadphasePairs;
int m_maxContactCapacity;
int m_maxVerticesPerFace;
int m_maxFacesPerShape;
@@ -18,7 +19,7 @@ struct btConfig
int m_maxTriConvexPairCapacity;
btConfig()
:m_maxConvexBodies(32*1024),
:m_maxConvexBodies(128*1024),
m_maxConvexShapes(8192),
m_maxVerticesPerFace(64),
m_maxFacesPerShape(64),
@@ -29,6 +30,7 @@ struct btConfig
m_maxTriConvexPairCapacity(512*1024)
{
m_maxBroadphasePairs = 16*m_maxConvexBodies;
m_maxContactCapacity = m_maxBroadphasePairs;
}
};

8
opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp
View File

@@ -182,8 +182,8 @@ static __inline void solveContact(btGpuConstraint4& cs,
btVector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
btVector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x()));
btAssert(_finite(linImp1.x()));
btAssert(_finite(linImp0.getX()));
btAssert(_finite(linImp1.getX()));
#endif
if (invMassA)
@@ -304,8 +304,8 @@ static inline void solveFriction(btGpuConstraint4& cs,
btVector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
btVector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x()));
btAssert(_finite(linImp1.x()));
btAssert(_finite(linImp0.getX()));
btAssert(_finite(linImp1.getX()));
#endif
if (invMassA)
{

10
opencl/gpu_rigidbody/host/btGpuNarrowPhase.cpp
View File

@@ -103,8 +103,7 @@ m_queue(queue)
m_data->m_inertiaBufferCPU = new btAlignedObjectArray<btInertiaCL>();
m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies);
m_data->m_pBufContactOutGPU = new btOpenCLArray<btContact4>(ctx,queue, config.m_maxBroadphasePairs,true);
btContact4 test = m_data->m_pBufContactOutGPU->forcedAt(0);
m_data->m_pBufContactOutGPU = new btOpenCLArray<btContact4>(ctx,queue, config.m_maxContactCapacity,true);
m_data->m_inertiaBufferGPU = new btOpenCLArray<btInertiaCL>(ctx,queue,config.m_maxConvexBodies,false);
m_data->m_collidablesGPU = new btOpenCLArray<btCollidable>(ctx,queue,config.m_maxConvexShapes);
@@ -601,9 +600,9 @@ int btGpuNarrowPhase::registerConcaveMeshShape(btAlignedObjectArray<btVector3>*
btVector3 normal = ((vert1-vert0).cross(vert2-vert0)).normalize();
btScalar c = -(normal.dot(vert0));
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[0] = normal.x();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[1] = normal.y();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[2] = normal.z();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[0] = normal.getX();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[1] = normal.getY();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[2] = normal.getZ();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[3] = c;
int indexOffset = m_data->m_convexIndices.size();
int numIndices = 3;
@@ -718,6 +717,7 @@ void btGpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphase
m_data->m_bodyBufferGPU,
m_data->m_pBufContactOutGPU,
nContactOut,
m_data->m_config.m_maxContactCapacity,
*m_data->m_convexPolyhedraGPU,
*m_data->m_convexVerticesGPU,
*m_data->m_uniqueEdgesGPU,

152
opencl/gpu_sat/host/ConvexHullContact.cpp
View File

@@ -53,6 +53,7 @@ m_totalContactsOut(m_context, m_queue)
m_totalContactsOut.push_back(0);
cl_int errNum=0;
bool disableKernelCaching = true;
if (1)
{
@@ -125,12 +126,16 @@ m_totalContactsOut(m_context, m_queue)
{
const char* primitiveContactsSrc = primitiveContactsKernelsCL;
cl_program primitiveContactsProg = btOpenCLUtils::compileCLProgramFromString(m_context,m_device,primitiveContactsSrc,&errNum,"","opencl/gpu_sat/kernels/primitiveContacts.cl");
cl_program primitiveContactsProg = btOpenCLUtils::compileCLProgramFromString(m_context,m_device,primitiveContactsSrc,&errNum,"","opencl/gpu_sat/kernels/primitiveContacts.cl",disableKernelCaching);
btAssert(errNum==CL_SUCCESS);
m_primitiveContactsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "primitiveContactsKernel",&errNum,primitiveContactsProg,"");
btAssert(errNum==CL_SUCCESS);
m_processCompoundPairsPrimitivesKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "processCompoundPairsPrimitivesKernel",&errNum,primitiveContactsProg,"");
btAssert(errNum==CL_SUCCESS);
btAssert(m_processCompoundPairsPrimitivesKernel);
}
@@ -161,6 +166,9 @@ GpuSatCollision::~GpuSatCollision()
if (m_primitiveContactsKernel)
clReleaseKernel(m_primitiveContactsKernel);
if (m_processCompoundPairsPrimitivesKernel)
clReleaseKernel(m_processCompoundPairsPrimitivesKernel);
if (m_clipHullHullKernel)
clReleaseKernel(m_clipHullHullKernel);
if (m_clipCompoundsHullHullKernel)
@@ -203,59 +211,67 @@ float signedDistanceFromPointToPlane(const float4& point, const float4& planeEqn
inline bool IsPointInPolygon(const btVector3& p,
const btVector3& posConvex,
const btQuaternion& ornConvex,
const btGpuFace* face,
const btVector3* baseVertex,
const int* convexIndices,
btVector3* out)
#define cross3(a,b) (a.cross(b))
btVector3 transform(btVector3* v, const btVector3* pos, const btVector3* orn)
{
btVector3 a;
btVector3 b;
btVector3 ab;
btVector3 ap;
btVector3 v;
btTransform tr;
tr.setIdentity();
tr.setOrigin(*pos);
btQuaternion* o = (btQuaternion*) orn;
tr.setRotation(*o);
btVector3 res = tr(*v);
return res;
}
btVector3 plane (face->m_plane[0],face->m_plane[1],face->m_plane[2]);
inline bool IsPointInPolygon(const float4& p,
const btGpuFace* face,
const float4* baseVertex,
const int* convexIndices,
float4* out)
{
float4 a;
float4 b;
float4 ab;
float4 ap;
float4 v;
float4 plane = make_float4(face->m_plane.x,face->m_plane.y,face->m_plane.z,0.f);
if (face->m_numIndices<2)
return false;
btTransform tr;
tr.setIdentity();
tr.setOrigin(posConvex);
tr.setRotation(ornConvex);
float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]];
btVector3 worldV0 = tr(v0);
b = worldV0;
b = v0;
for(unsigned i=0; i != face->m_numIndices; ++i)
{
a = b;
float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];
btVector3 worldVi = tr(vi);
b = worldVi;
b = vi;
ab = b-a;
ap = p-a;
v = ab.cross(plane);
v = cross3(ab,plane);
if (btDot(ap, v) > 0.f)
{
btScalar ab_m2 = btDot(ab, ab);
btScalar s = ab_m2 != btScalar(0.0) ? btDot(ab, ap) / ab_m2 : btScalar(0.0);
if (s <= btScalar(0.0))
float ab_m2 = btDot(ab, ab);
float rt = ab_m2 != 0.f ? btDot(ab, ap) / ab_m2 : 0.f;
if (rt <= 0.f)
{
*out = a;
}
else if (s >= btScalar(1.0))
else if (rt >= 1.f)
{
*out = b;
}
else
{
out->setInterpolate3(a,b,s);
float s = 1.f - rt;
out[0].x = s * a.x + rt * b.x;
out[0].y = s * a.y + rt * b.y;
out[0].z = s * a.z + rt * b.z;
}
return false;
}
@@ -264,7 +280,6 @@ inline bool IsPointInPolygon(const btVector3& p,
}
void computeContactSphereConvex(int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
@@ -286,9 +301,18 @@ void computeContactSphereConvex(int pairIndex,
float4 pos = rigidBodies[bodyIndexB].m_pos;
float4 spherePos = spherePos1-pos;
btQuaternion quat = rigidBodies[bodyIndexB].m_quat;
btTransform tr;
tr.setIdentity();
tr.setOrigin(pos);
tr.setRotation(quat);
btTransform 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;
@@ -297,12 +321,13 @@ void computeContactSphereConvex(int pairIndex,
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++ )
{
btGpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
float4 planeEqn;
float4 localPlaneNormal = make_float4(face.m_plane.x(),face.m_plane.y(),face.m_plane.z(),0.f);
float4 n1 = quatRotate(quat,localPlaneNormal);
float4 localPlaneNormal = make_float4(face.m_plane.getX(),face.m_plane.getY(),face.m_plane.getZ(),0.f);
float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
planeEqn = n1;
planeEqn[3] = face.m_plane[3];
@@ -319,9 +344,8 @@ void computeContactSphereConvex(int pairIndex,
{
//might hit an edge or vertex
btVector3 out;
bool isInPoly = IsPointInPolygon(spherePos,
pos,
quat,
&face,
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
convexIndices,
@@ -332,7 +356,7 @@ void computeContactSphereConvex(int pairIndex,
{
minDist = dist;
closestPnt = pntReturn;
hitNormalWorld = planeEqn;
localHitNormal = planeEqn;
region=1;
}
} else
@@ -346,7 +370,7 @@ void computeContactSphereConvex(int pairIndex,
{
minDist = dist;
closestPnt = out;
hitNormalWorld = tmp/dist;
localHitNormal = tmp/dist;
region=2;
}
@@ -363,19 +387,23 @@ void computeContactSphereConvex(int pairIndex,
{
minDist = dist;
closestPnt = pntReturn;
hitNormalWorld = planeEqn;
localHitNormal = planeEqn;
region=3;
}
}
}
static int numChecks = 0;
numChecks++;
if (bCollide && minDist > -100)
if (bCollide && minDist > -10000)
{
float4 normalOnSurfaceB1 = -hitNormalWorld;
float4 pOnB1 = closestPnt+pos;
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.getX(),normalOnSurfaceB1.getY(),normalOnSurfaceB1.getZ());
//printf("region=,%d,\n", region);
@@ -401,6 +429,7 @@ void computeContactSphereConvex(int pairIndex,
int numPoints = 1;
c->m_worldNormal[3] = numPoints;
}//if (dstIdx < numPairs)
}
}//if (hasCollision)
}
@@ -409,7 +438,7 @@ void computeContactSphereConvex(int pairIndex,
void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btInt2>* pairs, int nPairs,
const btOpenCLArray<btRigidBodyCL>* bodyBuf,
btOpenCLArray<btContact4>* contactOut, int& nContacts,
int maxContactCapacity,
const btOpenCLArray<btConvexPolyhedronCL>& convexData,
const btOpenCLArray<btVector3>& gpuVertices,
const btOpenCLArray<btVector3>& gpuUniqueEdges,
@@ -488,7 +517,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
{
printf("sphere-convex\n");
computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&hostBodyBuf[0],
&hostCollidables[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,nPairs);
}
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
@@ -534,6 +564,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
btLauncherCL launcher(m_queue, m_primitiveContactsKernel);
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(btBufferInfoCL) );
launcher.setConst( nPairs );
launcher.setConst(maxContactCapacity);
int num = nPairs;
launcher.launch1D( num);
clFinish(m_queue);
@@ -542,7 +573,9 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
contactOut->resize(nContacts);
}
}
#endif//CHECK_ON_HOST
BT_PROFILE("computeConvexConvexContactsGPUSAT");
// printf("nContacts = %d\n",nContacts);
@@ -720,6 +753,39 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
gpuCompoundSepNormals.resize(numCompoundPairs);
if (numCompoundPairs)
{
#ifndef CHECK_ON_HOST
BT_PROFILE("processCompoundPairsPrimitivesKernel");
btBufferInfoCL bInfo[] =
{
btBufferInfoCL( gpuCompoundPairs.getBufferCL(), true ),
btBufferInfoCL( bodyBuf->getBufferCL(),true),
btBufferInfoCL( gpuCollidables.getBufferCL(),true),
btBufferInfoCL( convexData.getBufferCL(),true),
btBufferInfoCL( gpuVertices.getBufferCL(),true),
btBufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
btBufferInfoCL( gpuFaces.getBufferCL(),true),
btBufferInfoCL( gpuIndices.getBufferCL(),true),
btBufferInfoCL( clAabbsWS.getBufferCL(),true),
btBufferInfoCL( gpuChildShapes.getBufferCL(),true),
btBufferInfoCL( contactOut->getBufferCL()),
btBufferInfoCL( m_totalContactsOut.getBufferCL())
};
btLauncherCL launcher(m_queue, m_processCompoundPairsPrimitivesKernel);
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(btBufferInfoCL) );
launcher.setConst( numCompoundPairs );
launcher.setConst(maxContactCapacity);
int num = numCompoundPairs;
launcher.launch1D( num);
clFinish(m_queue);
nContacts = m_totalContactsOut.at(0);
#endif
}
if (numCompoundPairs)
{

2
opencl/gpu_sat/host/ConvexHullContact.h
View File

@@ -53,6 +53,7 @@ struct GpuSatCollision
cl_kernel m_bvhTraversalKernel;
cl_kernel m_primitiveContactsKernel;
cl_kernel m_processCompoundPairsPrimitivesKernel;
btOpenCLArray<int> m_totalContactsOut;
@@ -64,6 +65,7 @@ struct GpuSatCollision
void computeConvexConvexContactsGPUSAT( const btOpenCLArray<btInt2>* pairs, int nPairs,
const btOpenCLArray<btRigidBodyCL>* bodyBuf,
btOpenCLArray<btContact4>* contactOut, int& nContacts,
int maxContactCapacity,
const btOpenCLArray<btConvexPolyhedronCL>& hostConvexData,
const btOpenCLArray<btVector3>& vertices,
const btOpenCLArray<btVector3>& uniqueEdges,

14
opencl/gpu_sat/host/btConvexUtility.cpp
View File

@@ -271,7 +271,7 @@ bool btConvexUtility::initializePolyhedralFeatures(const btVector3* orgVertices,
inline bool IsAlmostZero(const btVector3& v)
{
if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
if(fabsf(v.getX())>1e-6 || fabsf(v.getY())>1e-6 || fabsf(v.getZ())>1e-6) return false;
return true;
}
@@ -453,12 +453,12 @@ void btConvexUtility::initialize()
for(int i=0; i<m_vertices.size(); i++)
{
const btVector3& pt = m_vertices[i];
if(pt.x()<MinX) MinX = pt.x();
if(pt.x()>MaxX) MaxX = pt.x();
if(pt.y()<MinY) MinY = pt.y();
if(pt.y()>MaxY) MaxY = pt.y();
if(pt.z()<MinZ) MinZ = pt.z();
if(pt.z()>MaxZ) MaxZ = pt.z();
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);

18
opencl/gpu_sat/host/btOptimizedBvh.cpp
View File

@@ -116,20 +116,20 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
//PCK: add these checks for zero dimensions of aabb
const btScalar MIN_AABB_DIMENSION = btScalar(0.002);
const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001);
if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION)
if (aabbMax.getX() - aabbMin.getX() < MIN_AABB_DIMENSION)
{
aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION);
aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION);
aabbMax.setX(aabbMax.getX() + MIN_AABB_HALF_DIMENSION);
aabbMin.setX(aabbMin.getX() - MIN_AABB_HALF_DIMENSION);
}
if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION)
if (aabbMax.getY() - aabbMin.getY() < MIN_AABB_DIMENSION)
{
aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION);
aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION);
aabbMax.setY(aabbMax.getY() + MIN_AABB_HALF_DIMENSION);
aabbMin.setY(aabbMin.getY() - MIN_AABB_HALF_DIMENSION);
}
if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION)
if (aabbMax.getZ() - aabbMin.getZ() < MIN_AABB_DIMENSION)
{
aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION);
aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION);
aabbMax.setZ(aabbMax.getZ() + MIN_AABB_HALF_DIMENSION);
aabbMin.setZ(aabbMin.getZ() - MIN_AABB_HALF_DIMENSION);
}
m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);

203
opencl/gpu_sat/kernels/primitiveContacts.cl
View File

@@ -51,6 +51,21 @@ typedef struct
int m_bodyBPtrAndSignBit;
} Contact4;
typedef struct
{
union
{
float4 m_min;
float m_minElems[4];
int m_minIndices[4];
};
union
{
float4 m_max;
float m_maxElems[4];
int m_maxIndices[4];
};
} btAabbCL;
///keep this in sync with btCollidable.h
typedef struct
@@ -273,8 +288,6 @@ float signedDistanceFromPointToPlane(float4 point, float4 planeEqn, float4* clos
inline bool IsPointInPolygon(float4 p,
float4 posConvex,
float4 ornConvex,
const btGpuFace* face,
__global const float4* baseVertex,
__global const int* convexIndices,
@@ -293,16 +306,14 @@ inline bool IsPointInPolygon(float4 p,
float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]];
float4 worldV0 = transform(&v0, &posConvex, &ornConvex);
b = worldV0;
b = v0;
for(unsigned i=0; i != face->m_numIndices; ++i)
{
a = b;
float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];
float4 worldVi = transform(&vi, &posConvex, &ornConvex);
b = worldVi;
b = vi;
ab = b-a;
ap = p-a;
v = cross3(ab,plane);
@@ -346,22 +357,22 @@ void computeContactSphereConvex(int pairIndex,
__global const btGpuFace* faces,
__global Contact4* restrict globalContactsOut,
counter32_t nGlobalContactsOut,
int numPairs)
int maxContactCapacity,
float4 spherePos2,
float radius,
float4 pos,
float4 quat
)
{
float radius = collidables[collidableIndexA].m_radius;
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
float4 sphereOrn = rigidBodies[bodyIndexA].m_quat;
float4 invPos;
float4 invOrn;
trInverse(pos,quat, &invPos,&invOrn);
float4 spherePos = transform(&spherePos2,&invPos,&invOrn);
float4 pos = rigidBodies[bodyIndexB].m_pos;
float4 quat = rigidBodies[bodyIndexB].m_quat;
float4 spherePos = spherePos1 - pos;
int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
int numFaces = convexShapes[shapeIndex].m_numFaces;
float4 closestPnt = (float4)(0, 0, 0, 0);
float4 hitNormalWorld = (float4)(0, 0, 0, 0);
@@ -374,7 +385,8 @@ void computeContactSphereConvex(int pairIndex,
// set up a plane equation
float4 planeEqn;
float4 n1 = qtRotate(quat, (float4)(face.m_plane.xyz, 0));
float4 n1 = face.m_plane;
n1.w = 0.f;
planeEqn = n1;
planeEqn.w = face.m_plane.w;
@@ -395,9 +407,9 @@ void computeContactSphereConvex(int pairIndex,
{
//might hit an edge or vertex
float4 out;
float4 zeroPos = make_float4(0,0,0,0);
bool isInPoly = IsPointInPolygon(spherePos,
pos,
quat,
&face,
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
convexIndices,
@@ -446,17 +458,24 @@ void computeContactSphereConvex(int pairIndex,
if (bCollide)
if (bCollide && minDist > -10000)
{
float4 normalOnSurfaceB1 = -hitNormalWorld;
float4 pOnB1 = closestPnt+pos;
float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);
float4 pOnB1 = transform(&closestPnt,&pos,&quat);
//printf("pOnB1=%f,%f,%f\n", pOnB1.x,pOnB1.y,pOnB1.z);
float actualDepth = minDist-radius;
if (actualDepth<=0.f)
{
//printf("actualDepth = %f\n", actualDepth );
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
pOnB1.w = actualDepth;
int dstIdx;
AppendInc( nGlobalContactsOut, dstIdx );
if (dstIdx < numPairs)
//printf("maxContactCapacity=%d\n", maxContactCapacity);
if (1)//dstIdx < maxContactCapacity)
{
__global Contact4* c = &globalContactsOut[dstIdx];
c->m_worldNormal = normalOnSurfaceB1;
@@ -466,7 +485,9 @@ void computeContactSphereConvex(int pairIndex,
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
c->m_worldPos[0] = pOnB1;
GET_NPOINTS(*c) = 1;
}//if (dstIdx < numPairs)
}
}
}//if (hasCollision)
}
@@ -481,7 +502,7 @@ void computeContactPlaneConvex(int pairIndex,
__global const btGpuFace* faces,
__global Contact4* restrict globalContactsOut,
counter32_t nGlobalContactsOut,
int numPairs)
int maxContactCapacity)
{
float4 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
float radius = collidables[collidableIndexB].m_radius;
@@ -520,7 +541,7 @@ void computeContactPlaneConvex(int pairIndex,
int dstIdx;
AppendInc( nGlobalContactsOut, dstIdx );
if (dstIdx < numPairs)
if (dstIdx < maxContactCapacity)
{
__global Contact4* c = &globalContactsOut[dstIdx];
c->m_worldNormal = normalOnSurfaceB1;
@@ -547,7 +568,7 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
__global const int* indices,
__global Contact4* restrict globalContactsOut,
counter32_t nGlobalContactsOut,
int numPairs)
int numPairs, int maxContactCapacity)
{
int i = get_global_id(0);
@@ -579,7 +600,7 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA,
rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,numPairs);
rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);
return;
}
@@ -589,7 +610,7 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,numPairs);
rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);
return;
}
@@ -598,8 +619,15 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
{
float4 spherePos = rigidBodies[bodyIndexA].m_pos;
float sphereRadius = collidables[collidableIndexA].m_radius;
float4 convexPos = rigidBodies[bodyIndexB].m_pos;
float4 convexOrn = rigidBodies[bodyIndexB].m_quat;
computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,numPairs);
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
spherePos,sphereRadius,convexPos,convexOrn);
return;
}
@@ -607,8 +635,14 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
{
float4 spherePos = rigidBodies[bodyIndexB].m_pos;
float sphereRadius = collidables[collidableIndexB].m_radius;
float4 convexPos = rigidBodies[bodyIndexA].m_pos;
float4 convexOrn = rigidBodies[bodyIndexA].m_quat;
computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA,
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,numPairs);
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
spherePos,sphereRadius,convexPos,convexOrn);
return;
}
@@ -644,7 +678,7 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
int dstIdx;
AppendInc( nGlobalContactsOut, dstIdx );
if (dstIdx < numPairs)
if (dstIdx < maxContactCapacity)
{
__global Contact4* c = &globalContactsOut[dstIdx];
c->m_worldNormal = -normalOnSurfaceB;
@@ -665,3 +699,106 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
}// if (i<numPairs)
}
// work-in-progress
__kernel void processCompoundPairsPrimitivesKernel( __global const int4* gpuCompoundPairs,
__global const BodyData* rigidBodies,
__global const btCollidableGpu* collidables,
__global const ConvexPolyhedronCL* convexShapes,
__global const float4* vertices,
__global const float4* uniqueEdges,
__global const btGpuFace* faces,
__global const int* indices,
__global btAabbCL* aabbs,
__global const btGpuChildShape* gpuChildShapes,
__global Contact4* restrict globalContactsOut,
counter32_t nGlobalContactsOut,
int numCompoundPairs, int maxContactCapacity
)
{
int i = get_global_id(0);
if (i<numCompoundPairs)
{
int bodyIndexA = gpuCompoundPairs[i].x;
int bodyIndexB = gpuCompoundPairs[i].y;
int childShapeIndexA = gpuCompoundPairs[i].z;
int childShapeIndexB = gpuCompoundPairs[i].w;
int collidableIndexA = -1;
int collidableIndexB = -1;
float4 ornA = rigidBodies[bodyIndexA].m_quat;
float4 posA = rigidBodies[bodyIndexA].m_pos;
float4 ornB = rigidBodies[bodyIndexB].m_quat;
float4 posB = rigidBodies[bodyIndexB].m_pos;
if (childShapeIndexA >= 0)
{
collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
float4 newPosA = qtRotate(ornA,childPosA)+posA;
float4 newOrnA = qtMul(ornA,childOrnA);
posA = newPosA;
ornA = newOrnA;
} else
{
collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
}
if (childShapeIndexB>=0)
{
collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
float4 newPosB = transform(&childPosB,&posB,&ornB);
float4 newOrnB = qtMul(ornB,childOrnB);
posB = newPosB;
ornB = newOrnB;
} else
{
collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
}
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
int shapeTypeA = collidables[collidableIndexA].m_shapeType;
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
int pairIndex = i;
if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE))
{
float4 spherePos = rigidBodies[bodyIndexB].m_pos;
float sphereRadius = collidables[collidableIndexB].m_radius;
float4 convexPos = posA;
float4 convexOrn = ornA;
//printf("convex-sphere with radius %f\n",sphereRadius);
computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA , collidableIndexB,collidableIndexA,
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
spherePos,sphereRadius,convexPos,convexOrn);
return;
}
if ((shapeTypeA == SHAPE_SPHERE) && (shapeTypeB == SHAPE_CONVEX_HULL))
{
float4 spherePos = rigidBodies[bodyIndexA].m_pos;
float sphereRadius = collidables[collidableIndexA].m_radius;
float4 convexPos = posB;
float4 convexOrn = ornB;
//printf("sphere-convex with radius %f\n", sphereRadius);
computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
spherePos,sphereRadius,convexPos,convexOrn);
return;
}
}// if (i<numCompoundPairs)
}

9
opencl/gpu_sat/kernels/sat.cl
View File

@@ -706,6 +706,15 @@ __kernel void processCompoundPairsKernel( __global const int4* gpuCompoundPair
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
int shapeTypeA = collidables[collidableIndexA].m_shapeType;
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
if ((shapeTypeA != SHAPE_CONVEX_HULL) || (shapeTypeB != SHAPE_CONVEX_HULL))
{
return;
}
int hasSeparatingAxis = 5;
int numFacesA = convexShapes[shapeIndexA].m_numFaces;

138
src/BulletCommon/btMatrix3x3.h
View File

@@ -259,10 +259,10 @@ public:
m_el[1] = V2;
m_el[2] = V3;
#else
btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
btScalar xs = q.getX() * s, ys = q.getY() * s, zs = q.getZ() * s;
btScalar wx = q.getW() * xs, wy = q.getW() * ys, wz = q.getW() * zs;
btScalar xx = q.getX() * xs, xy = q.getX() * ys, xz = q.getX() * zs;
btScalar yy = q.getY() * ys, yz = q.getY() * zs, zz = q.getZ() * zs;
setValue(
btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
@@ -375,17 +375,17 @@ public:
vm[1] = v1;
vm[2] = v2;
#else
m[0] = btScalar(m_el[0].x());
m[1] = btScalar(m_el[1].x());
m[2] = btScalar(m_el[2].x());
m[0] = btScalar(m_el[0].getX());
m[1] = btScalar(m_el[1].getX());
m[2] = btScalar(m_el[2].getX());
m[3] = btScalar(0.0);
m[4] = btScalar(m_el[0].y());
m[5] = btScalar(m_el[1].y());
m[6] = btScalar(m_el[2].y());
m[4] = btScalar(m_el[0].getY());
m[5] = btScalar(m_el[1].getY());
m[6] = btScalar(m_el[2].getY());
m[7] = btScalar(0.0);
m[8] = btScalar(m_el[0].z());
m[9] = btScalar(m_el[1].z());
m[10] = btScalar(m_el[2].z());
m[8] = btScalar(m_el[0].getZ());
m[9] = btScalar(m_el[1].getZ());
m[10] = btScalar(m_el[2].getZ());
m[11] = btScalar(0.0);
#endif
}
@@ -395,7 +395,7 @@ public:
void getRotation(btQuaternion& q) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
btScalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
btScalar s, x;
union {
@@ -407,25 +407,25 @@ public:
{
x = trace + btScalar(1.0);
temp.f[0]=m_el[2].y() - m_el[1].z();
temp.f[1]=m_el[0].z() - m_el[2].x();
temp.f[2]=m_el[1].x() - m_el[0].y();
temp.f[0]=m_el[2].getY() - m_el[1].getZ();
temp.f[1]=m_el[0].getZ() - m_el[2].getX();
temp.f[2]=m_el[1].getX() - m_el[0].getY();
temp.f[3]=x;
//temp.f[3]= s * btScalar(0.5);
}
else
{
int i, j, k;
if(m_el[0].x() < m_el[1].y())
if(m_el[0].getX() < m_el[1].getY())
{
if( m_el[1].y() < m_el[2].z() )
if( m_el[1].getY() < m_el[2].getZ() )
{ i = 2; j = 0; k = 1; }
else
{ i = 1; j = 2; k = 0; }
}
else
{
if( m_el[0].x() < m_el[2].z())
if( m_el[0].getX() < m_el[2].getZ())
{ i = 2; j = 0; k = 1; }
else
{ i = 0; j = 1; k = 2; }
@@ -446,7 +446,7 @@ public:
q *= s;
#else
btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
btScalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
btScalar temp[4];
@@ -456,15 +456,15 @@ public:
temp[3]=(s * btScalar(0.5));
s = btScalar(0.5) / s;
temp[0]=((m_el[2].y() - m_el[1].z()) * s);
temp[1]=((m_el[0].z() - m_el[2].x()) * s);
temp[2]=((m_el[1].x() - m_el[0].y()) * s);
temp[0]=((m_el[2].getY() - m_el[1].getZ()) * s);
temp[1]=((m_el[0].getZ() - m_el[2].getX()) * s);
temp[2]=((m_el[1].getX() - m_el[0].getY()) * s);
}
else
{
int i = m_el[0].x() < m_el[1].y() ?
(m_el[1].y() < m_el[2].z() ? 2 : 1) :
(m_el[0].x() < m_el[2].z() ? 2 : 0);
int i = m_el[0].getX() < m_el[1].getY() ?
(m_el[1].getY() < m_el[2].getZ() ? 2 : 1) :
(m_el[0].getX() < m_el[2].getZ() ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;
@@ -488,9 +488,9 @@ public:
{
// first use the normal calculus
yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
pitch = btScalar(btAsin(-m_el[2].x()));
roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
yaw = btScalar(btAtan2(m_el[1].getX(), m_el[0].getX()));
pitch = btScalar(btAsin(-m_el[2].getX()));
roll = btScalar(btAtan2(m_el[2].getY(), m_el[2].getZ()));
// on pitch = +/-HalfPI
if (btFabs(pitch)==SIMD_HALF_PI)
@@ -527,14 +527,14 @@ public:
//get the pointer to the raw data
// Check that pitch is not at a singularity
if (btFabs(m_el[2].x()) >= 1)
if (btFabs(m_el[2].getX()) >= 1)
{
euler_out.yaw = 0;
euler_out2.yaw = 0;
// From difference of angles formula
btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
if (m_el[2].x() > 0) //gimbal locked up
btScalar delta = btAtan2(m_el[0].getX(),m_el[0].getZ());
if (m_el[2].getX() > 0) //gimbal locked up
{
euler_out.pitch = SIMD_PI / btScalar(2.0);
euler_out2.pitch = SIMD_PI / btScalar(2.0);
@@ -551,18 +551,18 @@ public:
}
else
{
euler_out.pitch = - btAsin(m_el[2].x());
euler_out.pitch = - btAsin(m_el[2].getX());
euler_out2.pitch = SIMD_PI - euler_out.pitch;
euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),
m_el[2].z()/btCos(euler_out.pitch));
euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),
m_el[2].z()/btCos(euler_out2.pitch));
euler_out.roll = btAtan2(m_el[2].getY()/btCos(euler_out.pitch),
m_el[2].getZ()/btCos(euler_out.pitch));
euler_out2.roll = btAtan2(m_el[2].getY()/btCos(euler_out2.pitch),
m_el[2].getZ()/btCos(euler_out2.pitch));
euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),
m_el[0].x()/btCos(euler_out.pitch));
euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),
m_el[0].x()/btCos(euler_out2.pitch));
euler_out.yaw = btAtan2(m_el[1].getX()/btCos(euler_out.pitch),
m_el[0].getX()/btCos(euler_out.pitch));
euler_out2.yaw = btAtan2(m_el[1].getX()/btCos(euler_out2.pitch),
m_el[0].getX()/btCos(euler_out2.pitch));
}
if (solution_number == 1)
@@ -588,9 +588,9 @@ public:
return btMatrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
#else
return btMatrix3x3(
m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
m_el[0].getX() * s.getX(), m_el[0].getY() * s.getY(), m_el[0].getZ() * s.getZ(),
m_el[1].getX() * s.getX(), m_el[1].getY() * s.getY(), m_el[1].getZ() * s.getZ(),
m_el[2].getX() * s.getX(), m_el[2].getY() * s.getY(), m_el[2].getZ() * s.getZ());
#endif
}
@@ -610,15 +610,15 @@ public:
SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
{
return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
return m_el[0].getX() * v.getX() + m_el[1].getX() * v.getY() + m_el[2].getX() * v.getZ();
}
SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
{
return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
return m_el[0].getY() * v.getX() + m_el[1].getY() * v.getY() + m_el[2].getY() * v.getZ();
}
SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
{
return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
return m_el[0].getZ() * v.getX() + m_el[1].getZ() * v.getY() + m_el[2].getZ() * v.getZ();
}
@@ -865,9 +865,9 @@ operator*(const btMatrix3x3& m, const btScalar & k)
vmulq_n_f32(m[2].mVec128, k));
#else
return btMatrix3x3(
m[0].x()*k,m[0].y()*k,m[0].z()*k,
m[1].x()*k,m[1].y()*k,m[1].z()*k,
m[2].x()*k,m[2].y()*k,m[2].z()*k);
m[0].getX()*k,m[0].getY()*k,m[0].getZ()*k,
m[1].getX()*k,m[1].getY()*k,m[1].getZ()*k,
m[2].getX()*k,m[2].getY()*k,m[2].getZ()*k);
#endif
}
@@ -965,9 +965,9 @@ btMatrix3x3::absolute() const
(float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask));
#else
return btMatrix3x3(
btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
btFabs(m_el[0].getX()), btFabs(m_el[0].getY()), btFabs(m_el[0].getZ()),
btFabs(m_el[1].getX()), btFabs(m_el[1].getY()), btFabs(m_el[1].getZ()),
btFabs(m_el[2].getX()), btFabs(m_el[2].getY()), btFabs(m_el[2].getZ()));
#endif
}
@@ -1002,9 +1002,9 @@ btMatrix3x3::transpose() const
float32x4_t v2 = vcombine_f32( vget_high_f32(top.val[0]), q ); // z0 z1 z2 0
return btMatrix3x3( v0, v1, v2 );
#else
return btMatrix3x3( m_el[0].x(), m_el[1].x(), m_el[2].x(),
m_el[0].y(), m_el[1].y(), m_el[2].y(),
m_el[0].z(), m_el[1].z(), m_el[2].z());
return btMatrix3x3( m_el[0].getX(), m_el[1].getX(), m_el[2].getX(),
m_el[0].getY(), m_el[1].getY(), m_el[2].getY(),
m_el[0].getZ(), m_el[1].getZ(), m_el[2].getZ());
#endif
}
@@ -1023,9 +1023,9 @@ btMatrix3x3::inverse() const
btScalar det = (*this)[0].dot(co);
btFullAssert(det != btScalar(0.0));
btScalar s = btScalar(1.0) / det;
return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
return btMatrix3x3(co.getX() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
co.getY() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
co.getZ() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
}
SIMD_FORCE_INLINE btMatrix3x3
@@ -1072,15 +1072,15 @@ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
return btMatrix3x3( r0, r1, r2 );
#else
return btMatrix3x3(
m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
m_el[0].getX() * m[0].getX() + m_el[1].getX() * m[1].getX() + m_el[2].getX() * m[2].getX(),
m_el[0].getX() * m[0].getY() + m_el[1].getX() * m[1].getY() + m_el[2].getX() * m[2].getY(),
m_el[0].getX() * m[0].getZ() + m_el[1].getX() * m[1].getZ() + m_el[2].getX() * m[2].getZ(),
m_el[0].getY() * m[0].getX() + m_el[1].getY() * m[1].getX() + m_el[2].getY() * m[2].getX(),
m_el[0].getY() * m[0].getY() + m_el[1].getY() * m[1].getY() + m_el[2].getY() * m[2].getY(),
m_el[0].getY() * m[0].getZ() + m_el[1].getY() * m[1].getZ() + m_el[2].getY() * m[2].getZ(),
m_el[0].getZ() * m[0].getX() + m_el[1].getZ() * m[1].getX() + m_el[2].getZ() * m[2].getX(),
m_el[0].getZ() * m[0].getY() + m_el[1].getZ() * m[1].getY() + m_el[2].getZ() * m[2].getY(),
m_el[0].getZ() * m[0].getZ() + m_el[1].getZ() * m[1].getZ() + m_el[2].getZ() * m[2].getZ());
#endif
}

11
src/BulletCommon/btQuadWord.h
View File

@@ -48,13 +48,14 @@ public:
{
return mVec128;
}
protected:
#else //__CELLOS_LV2__ __SPU__
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
union {
btSimdFloat4 mVec128;
btScalar m_floats[4];
struct {btScalar x,y,z,w;};
};
public:
SIMD_FORCE_INLINE btSimdFloat4 get128() const
@@ -113,13 +114,7 @@ public:
/**@brief Set the w value */
SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w;};
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
/**@brief Return the y value */
SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
/**@brief Return the w value */
SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
//SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }

72
src/BulletCommon/btQuaternion.h
View File

@@ -73,7 +73,9 @@ public:
/**@brief Constructor from scalars */
btQuaternion(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
: btQuadWord(_x, _y, _z, _w)
{}
{
btAssert(!((_x==1.f) && (_y==0.f) && (_z==0.f) && (_w==0.f)));
}
/**@brief Axis angle Constructor
* @param axis The axis which the rotation is around
* @param angle The magnitude of the rotation around the angle (Radians) */
@@ -101,7 +103,7 @@ public:
btScalar d = axis.length();
btAssert(d != btScalar(0.0));
btScalar s = btSin(_angle * btScalar(0.5)) / d;
setValue(axis.x() * s, axis.y() * s, axis.z() * s,
setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
btCos(_angle * btScalar(0.5)));
}
/**@brief Set the quaternion using Euler angles
@@ -153,9 +155,9 @@ public:
#elif defined(BT_USE_NEON)
mVec128 = vaddq_f32(mVec128, q.mVec128);
#else
m_floats[0] += q.x();
m_floats[1] += q.y();
m_floats[2] += q.z();
m_floats[0] += q.getX();
m_floats[1] += q.getY();
m_floats[2] += q.getZ();
m_floats[3] += q.m_floats[3];
#endif
return *this;
@@ -170,9 +172,9 @@ public:
#elif defined(BT_USE_NEON)
mVec128 = vsubq_f32(mVec128, q.mVec128);
#else
m_floats[0] -= q.x();
m_floats[1] -= q.y();
m_floats[2] -= q.z();
m_floats[0] -= q.getX();
m_floats[1] -= q.getY();
m_floats[2] -= q.getZ();
m_floats[3] -= q.m_floats[3];
#endif
return *this;
@@ -274,10 +276,10 @@ public:
mVec128 = A0;
#else
setValue(
m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
m_floats[3] * q.getX() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.getZ() - m_floats[2] * q.getY(),
m_floats[3] * q.getY() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.getX() - m_floats[0] * q.getZ(),
m_floats[3] * q.getZ() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.getY() - m_floats[1] * q.getX(),
m_floats[3] * q.m_floats[3] - m_floats[0] * q.getX() - m_floats[1] * q.getY() - m_floats[2] * q.getZ());
#endif
return *this;
}
@@ -302,9 +304,9 @@ public:
x = vpadd_f32(x, x);
return vget_lane_f32(x, 0);
#else
return m_floats[0] * q.x() +
m_floats[1] * q.y() +
m_floats[2] * q.z() +
return m_floats[0] * q.getX() +
m_floats[1] * q.getY() +
m_floats[2] * q.getZ() +
m_floats[3] * q.m_floats[3];
#endif
}
@@ -359,7 +361,7 @@ public:
#elif defined(BT_USE_NEON)
return btQuaternion(vmulq_n_f32(mVec128, s));
#else
return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
return btQuaternion(getX() * s, getY() * s, getZ() * s, m_floats[3] * s);
#endif
}
@@ -433,7 +435,7 @@ public:
return btQuaternion(vaddq_f32(mVec128, q2.mVec128));
#else
const btQuaternion& q1 = *this;
return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
return btQuaternion(q1.getX() + q2.getX(), q1.getY() + q2.getY(), q1.getZ() + q2.getZ(), q1.m_floats[3] + q2.m_floats[3]);
#endif
}
@@ -448,7 +450,7 @@ public:
return btQuaternion(vsubq_f32(mVec128, q2.mVec128));
#else
const btQuaternion& q1 = *this;
return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
return btQuaternion(q1.getX() - q2.getX(), q1.getY() - q2.getY(), q1.getZ() - q2.getZ(), q1.m_floats[3] - q2.m_floats[3]);
#endif
}
@@ -462,7 +464,7 @@ public:
return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask) );
#else
const btQuaternion& q2 = *this;
return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
return btQuaternion( - q2.getX(), - q2.getY(), - q2.getZ(), - q2.m_floats[3]);
#endif
}
/**@todo document this and it's use */
@@ -509,9 +511,9 @@ public:
const btScalar s0 = btSin((btScalar(1.0) - t) * theta);
return btQuaternion(
(m_floats[0] * s0 + q.x() * s1) * d,
(m_floats[1] * s0 + q.y() * s1) * d,
(m_floats[2] * s0 + q.z() * s1) * d,
(m_floats[0] * s0 + q.getX() * s1) * d,
(m_floats[1] * s0 + q.getY() * s1) * d,
(m_floats[2] * s0 + q.getZ() * s1) * d,
(m_floats[3] * s0 + q.m_floats[3] * s1) * d);
}
else
@@ -617,10 +619,10 @@ operator*(const btQuaternion& q1, const btQuaternion& q2)
#else
return btQuaternion(
q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
q1.getW() * q2.getX() + q1.getX() * q2.getW() + q1.getY() * q2.getZ() - q1.getZ() * q2.getY(),
q1.getW() * q2.getY() + q1.getY() * q2.getW() + q1.getZ() * q2.getX() - q1.getX() * q2.getZ(),
q1.getW() * q2.getZ() + q1.getZ() * q2.getW() + q1.getX() * q2.getY() - q1.getY() * q2.getX(),
q1.getW() * q2.getW() - q1.getX() * q2.getX() - q1.getY() * q2.getY() - q1.getZ() * q2.getZ());
#endif
}
@@ -700,10 +702,10 @@ operator*(const btQuaternion& q, const btVector3& w)
#else
return btQuaternion(
q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
-q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
q.getW() * w.getX() + q.getY() * w.getZ() - q.getZ() * w.getY(),
q.getW() * w.getY() + q.getZ() * w.getX() - q.getX() * w.getZ(),
q.getW() * w.getZ() + q.getX() * w.getY() - q.getY() * w.getX(),
-q.getX() * w.getX() - q.getY() * w.getY() - q.getZ() * w.getZ());
#endif
}
@@ -783,10 +785,10 @@ operator*(const btVector3& w, const btQuaternion& q)
#else
return btQuaternion(
+w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
+w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
+w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
-w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
+w.getX() * q.getW() + w.getY() * q.getZ() - w.getZ() * q.getY(),
+w.getY() * q.getW() + w.getZ() * q.getX() - w.getX() * q.getZ(),
+w.getZ() * q.getW() + w.getX() * q.getY() - w.getY() * q.getX(),
-w.getX() * q.getX() - w.getY() * q.getY() - w.getZ() * q.getZ());
#endif
}
@@ -854,7 +856,7 @@ shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Ge
{
btVector3 n,unused;
btPlaneSpace1(v0,n,unused);
return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
return btQuaternion(n.getX(),n.getY(),n.getZ(),0.0f); // just pick any vector that is orthogonal to v0
}
btScalar s = btSqrt((1.0f + d) * 2.0f);

6
src/BulletCommon/btTransform.h
View File

@@ -139,9 +139,9 @@ public:
void getOpenGLMatrix(btScalar *m) const
{
m_basis.getOpenGLSubMatrix(m);
m[12] = m_origin.x();
m[13] = m_origin.y();
m[14] = m_origin.z();
m[12] = m_origin.getX();
m[13] = m_origin.getY();
m[14] = m_origin.getZ();
m[15] = btScalar(1.0);
}

12
src/BulletCommon/btTransformUtil.h
View File

@@ -24,9 +24,9 @@ subject to the following restrictions:
SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
{
return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z());
return btVector3(supportDir.getX() < btScalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
supportDir.getY() < btScalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
supportDir.getZ() < btScalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
}
@@ -70,7 +70,7 @@ public:
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
}
btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
btQuaternion dorn (axis.getX(),axis.getY(),axis.getZ(),btCos( fAngle*timeStep*btScalar(0.5) ));
btQuaternion orn0 = curTrans.getRotation();
btQuaternion predictedOrn = dorn * orn0;
@@ -99,7 +99,7 @@ public:
btQuaternion orn1 = orn0.nearest(orn1a);
btQuaternion dorn = orn1 * orn0.inverse();
angle = dorn.getAngle();
axis = btVector3(dorn.x(),dorn.y(),dorn.z());
axis = btVector3(dorn.getX(),dorn.getY(),dorn.getZ());
axis[3] = btScalar(0.);
//check for axis length
btScalar len = axis.length2();
@@ -128,7 +128,7 @@ public:
dorn.normalize();
angle = dorn.getAngle();
axis = btVector3(dorn.x(),dorn.y(),dorn.z());
axis = btVector3(dorn.getX(),dorn.getY(),dorn.getZ());
axis[3] = btScalar(0.);
//check for axis length
btScalar len = axis.length2();

23
src/BulletCommon/btVector3.h
View File

@@ -94,6 +94,8 @@ public:
union {
btSimdFloat4 mVec128;
btScalar m_floats[4];
struct {btScalar x,y,z,w;};
};
SIMD_FORCE_INLINE btSimdFloat4 get128() const
{
@@ -548,6 +550,9 @@ public:
SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
/**@brief Return the w value */
SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
/**@brief Set the x value */
SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x;};
/**@brief Set the y value */
@@ -556,14 +561,6 @@ public:
SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z;};
/**@brief Set the w value */
SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w;};
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
/**@brief Return the y value */
SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
/**@brief Return the w value */
SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
//SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
@@ -601,7 +598,7 @@ public:
btSetMax(m_floats[0], other.m_floats[0]);
btSetMax(m_floats[1], other.m_floats[1]);
btSetMax(m_floats[2], other.m_floats[2]);
btSetMax(m_floats[3], other.w());
btSetMax(m_floats[3], other.m_floats[3]);
#endif
}
@@ -618,7 +615,7 @@ public:
btSetMin(m_floats[0], other.m_floats[0]);
btSetMin(m_floats[1], other.m_floats[1]);
btSetMin(m_floats[2], other.m_floats[2]);
btSetMin(m_floats[3], other.w());
btSetMin(m_floats[3], other.m_floats[3]);
#endif
}
@@ -647,9 +644,9 @@ public:
v1->mVec128 = V1;
v2->mVec128 = V2;
#else
v0->setValue(0. ,-z() ,y());
v1->setValue(z() ,0. ,-x());
v2->setValue(-y() ,x() ,0.);
v0->setValue(0. ,-getZ() ,getY());
v1->setValue(getZ() ,0. ,-getX());
v2->setValue(-getY() ,getX() ,0.);
#endif
}