From 4a93c2e704d3ca3cc0982ac5438948c9cc347dcf Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Wed, 3 Apr 2013 18:27:36 -0700 Subject: [PATCH] x() -> x or getX() or [0] 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) --- btgui/OpenGLWindow/GLInstancingRenderer.cpp | 30 +-- demo/donttouch/OpenGL3CoreRenderer.cpp | 12 +- demo/gpudemo/GpuDemo.h | 6 +- demo/gpudemo/main_opengl3core.cpp | 11 +- demo/gpudemo/rigidbody/GpuCompoundScene.cpp | 87 ++++++- demo/gpudemo/rigidbody/GpuSphereScene.cpp | 124 +++++++-- opencl/basic_initialize/btOpenCLUtils.cpp | 5 + opencl/gpu_rigidbody/host/Solver.cpp | 8 +- opencl/gpu_rigidbody/host/btConfig.h | 4 +- .../gpu_rigidbody/host/btGpuJacobiSolver.cpp | 8 +- .../gpu_rigidbody/host/btGpuNarrowPhase.cpp | 12 +- opencl/gpu_sat/host/ConvexHullContact.cpp | 158 ++++++++---- opencl/gpu_sat/host/ConvexHullContact.h | 2 + opencl/gpu_sat/host/btConvexUtility.cpp | 14 +- opencl/gpu_sat/host/btOptimizedBvh.cpp | 18 +- opencl/gpu_sat/kernels/primitiveContacts.cl | 237 ++++++++++++++---- opencl/gpu_sat/kernels/sat.cl | 9 + src/BulletCommon/btMatrix3x3.h | 138 +++++----- src/BulletCommon/btQuadWord.h | 11 +- src/BulletCommon/btQuaternion.h | 72 +++--- src/BulletCommon/btTransform.h | 6 +- src/BulletCommon/btTransformUtil.h | 12 +- src/BulletCommon/btVector3.h | 23 +- 23 files changed, 692 insertions(+), 315 deletions(-) diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.cpp b/btgui/OpenGLWindow/GLInstancingRenderer.cpp index 04bf7b2e2..86b9f9fde 100644 --- a/btgui/OpenGLWindow/GLInstancingRenderer.cpp +++ b/btgui/OpenGLWindow/GLInstancingRenderer.cpp @@ -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(); } diff --git a/demo/donttouch/OpenGL3CoreRenderer.cpp b/demo/donttouch/OpenGL3CoreRenderer.cpp index 0fcf956b7..ca1a55a3f 100644 --- a/demo/donttouch/OpenGL3CoreRenderer.cpp +++ b/demo/donttouch/OpenGL3CoreRenderer.cpp @@ -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); } } diff --git a/demo/gpudemo/GpuDemo.h b/demo/gpudemo/GpuDemo.h index cd929f8d7..bb8563f21 100644 --- a/demo/gpudemo/GpuDemo.h +++ b/demo/gpudemo/GpuDemo.h @@ -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), diff --git a/demo/gpudemo/main_opengl3core.cpp b/demo/gpudemo/main_opengl3core.cpp index e2d131e55..97b633646 100644 --- a/demo/gpudemo/main_opengl3core.cpp +++ b/demo/gpudemo/main_opengl3core.cpp @@ -65,11 +65,16 @@ btAlignedObjectArray 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"); diff --git a/demo/gpudemo/rigidbody/GpuCompoundScene.cpp b/demo/gpudemo/rigidbody/GpuCompoundScene.cpp index ed1d71379..b799dd0ca 100644 --- a/demo/gpudemo/rigidbody/GpuCompoundScene.cpp +++ b/demo/gpudemo/rigidbody/GpuCompoundScene.cpp @@ -32,21 +32,91 @@ void GpuCompoundScene::setupScene(const ConstructionInfo& ci) btAlignedObjectArray 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); { int childColIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); + btVector3 childPositions[3] = { btVector3(0,-2,0), @@ -54,6 +124,7 @@ void GpuCompoundScene::setupScene(const ConstructionInfo& ci) btVector3(0,2,0) }; + btAlignedObjectArray childShapes; int numChildShapes = 3; for (int i=0;im_np->registerCompoundShape(&childShapes); - + } + //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;kregisterShape(&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); + if (0) + { + float radius = 40; + int prevGraphicsShapeIndex = -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;kregisterShape(&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]; diff --git a/opencl/basic_initialize/btOpenCLUtils.cpp b/opencl/basic_initialize/btOpenCLUtils.cpp index fc47c4005..57dcda2ba 100644 --- a/opencl/basic_initialize/btOpenCLUtils.cpp +++ b/opencl/basic_initialize/btOpenCLUtils.cpp @@ -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; diff --git a/opencl/gpu_rigidbody/host/Solver.cpp b/opencl/gpu_rigidbody/host/Solver.cpp index f9a54fe12..3ce7c2a3d 100644 --- a/opencl/gpu_rigidbody/host/Solver.cpp +++ b/opencl/gpu_rigidbody/host/Solver.cpp @@ -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; diff --git a/opencl/gpu_rigidbody/host/btConfig.h b/opencl/gpu_rigidbody/host/btConfig.h index e3d0953c8..25ca1ad89 100644 --- a/opencl/gpu_rigidbody/host/btConfig.h +++ b/opencl/gpu_rigidbody/host/btConfig.h @@ -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; } }; diff --git a/opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp b/opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp index 0eed19a9e..955ed4d55 100644 --- a/opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp +++ b/opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp @@ -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) { diff --git a/opencl/gpu_rigidbody/host/btGpuNarrowPhase.cpp b/opencl/gpu_rigidbody/host/btGpuNarrowPhase.cpp index 406d26365..99a8e7732 100644 --- a/opencl/gpu_rigidbody/host/btGpuNarrowPhase.cpp +++ b/opencl/gpu_rigidbody/host/btGpuNarrowPhase.cpp @@ -103,9 +103,8 @@ m_queue(queue) m_data->m_inertiaBufferCPU = new btAlignedObjectArray(); m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies); - m_data->m_pBufContactOutGPU = new btOpenCLArray(ctx,queue, config.m_maxBroadphasePairs,true); - btContact4 test = m_data->m_pBufContactOutGPU->forcedAt(0); - + m_data->m_pBufContactOutGPU = new btOpenCLArray(ctx,queue, config.m_maxContactCapacity,true); + m_data->m_inertiaBufferGPU = new btOpenCLArray(ctx,queue,config.m_maxConvexBodies,false); m_data->m_collidablesGPU = new btOpenCLArray(ctx,queue,config.m_maxConvexShapes); @@ -601,9 +600,9 @@ int btGpuNarrowPhase::registerConcaveMeshShape(btAlignedObjectArray* 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, diff --git a/opencl/gpu_sat/host/ConvexHullContact.cpp b/opencl/gpu_sat/host/ConvexHullContact.cpp index 899126a21..584c77c9a 100644 --- a/opencl/gpu_sat/host/ConvexHullContact.cpp +++ b/opencl/gpu_sat/host/ConvexHullContact.cpp @@ -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,11 +126,15 @@ 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, @@ -278,7 +293,7 @@ void computeContactSphereConvex(int pairIndex, int& nGlobalContactsOut, int maxContactCapacity) { - + float radius = collidables[collidableIndexA].m_radius; float4 spherePos1 = rigidBodies[bodyIndexA].m_pos; btQuaternion sphereOrn = rigidBodies[bodyIndexA].m_quat; @@ -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; } } } - - - if (bCollide && minDist > -100) + static int numChecks = 0; + numChecks++; + + 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* pairs, int nPairs, const btOpenCLArray* bodyBuf, btOpenCLArray* contactOut, int& nContacts, - + int maxContactCapacity, const btOpenCLArray& convexData, const btOpenCLArray& gpuVertices, const btOpenCLArray& gpuUniqueEdges, @@ -488,7 +517,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArrayresize(nContacts); } } + #endif//CHECK_ON_HOST + BT_PROFILE("computeConvexConvexContactsGPUSAT"); // printf("nContacts = %d\n",nContacts); @@ -720,6 +753,39 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArraygetBufferCL(),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) { diff --git a/opencl/gpu_sat/host/ConvexHullContact.h b/opencl/gpu_sat/host/ConvexHullContact.h index c596f3715..d32275db9 100644 --- a/opencl/gpu_sat/host/ConvexHullContact.h +++ b/opencl/gpu_sat/host/ConvexHullContact.h @@ -53,6 +53,7 @@ struct GpuSatCollision cl_kernel m_bvhTraversalKernel; cl_kernel m_primitiveContactsKernel; + cl_kernel m_processCompoundPairsPrimitivesKernel; btOpenCLArray m_totalContactsOut; @@ -64,6 +65,7 @@ struct GpuSatCollision void computeConvexConvexContactsGPUSAT( const btOpenCLArray* pairs, int nPairs, const btOpenCLArray* bodyBuf, btOpenCLArray* contactOut, int& nContacts, + int maxContactCapacity, const btOpenCLArray& hostConvexData, const btOpenCLArray& vertices, const btOpenCLArray& uniqueEdges, diff --git a/opencl/gpu_sat/host/btConvexUtility.cpp b/opencl/gpu_sat/host/btConvexUtility.cpp index 62c68d13f..7664ac348 100644 --- a/opencl/gpu_sat/host/btConvexUtility.cpp +++ b/opencl/gpu_sat/host/btConvexUtility.cpp @@ -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; iMaxX) MaxX = pt.x(); - if(pt.y()MaxY) MaxY = pt.y(); - if(pt.z()MaxZ) MaxZ = pt.z(); + if(pt.getX()MaxX) MaxX = pt.getX(); + if(pt.getY()MaxY) MaxY = pt.getY(); + if(pt.getZ()MaxZ) MaxZ = pt.getZ(); } mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ); mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ); diff --git a/opencl/gpu_sat/host/btOptimizedBvh.cpp b/opencl/gpu_sat/host/btOptimizedBvh.cpp index a8dff1691..29c445d53 100644 --- a/opencl/gpu_sat/host/btOptimizedBvh.cpp +++ b/opencl/gpu_sat/host/btOptimizedBvh.cpp @@ -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); diff --git a/opencl/gpu_sat/kernels/primitiveContacts.cl b/opencl/gpu_sat/kernels/primitiveContacts.cl index 7bb4113ba..e90a3abbc 100644 --- a/opencl/gpu_sat/kernels/primitiveContacts.cl +++ b/opencl/gpu_sat/kernels/primitiveContacts.cl @@ -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); @@ -322,9 +333,9 @@ inline bool IsPointInPolygon(float4 p, else { 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; + 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; } @@ -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,27 +458,36 @@ 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; - pOnB1.w = actualDepth; - - int dstIdx; - AppendInc( nGlobalContactsOut, dstIdx ); - - if (dstIdx < numPairs) + if (actualDepth<=0.f) { - __global Contact4* c = &globalContactsOut[dstIdx]; - c->m_worldNormal = normalOnSurfaceB1; - c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16); - 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_worldPos[0] = pOnB1; - GET_NPOINTS(*c) = 1; - }//if (dstIdx < numPairs) + //printf("actualDepth = %f\n", actualDepth ); + //printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z); + + pOnB1.w = actualDepth; + + int dstIdx; + AppendInc( nGlobalContactsOut, dstIdx ); + + //printf("maxContactCapacity=%d\n", maxContactCapacity); + if (1)//dstIdx < maxContactCapacity) + { + __global Contact4* c = &globalContactsOut[dstIdx]; + c->m_worldNormal = normalOnSurfaceB1; + c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16); + 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_worldPos[0] = pOnB1; + GET_NPOINTS(*c) = 1; + } + + } }//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; } @@ -642,9 +676,9 @@ __kernel void primitiveContactsKernel( __global const int2* pairs, contactPosB.w = dist; int dstIdx; - AppendInc( nGlobalContactsOut, 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= 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= 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 } diff --git a/src/BulletCommon/btQuadWord.h b/src/BulletCommon/btQuadWord.h index 11067ef47..488546424 100644 --- a/src/BulletCommon/btQuadWord.h +++ b/src/BulletCommon/btQuadWord.h @@ -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]; } diff --git a/src/BulletCommon/btQuaternion.h b/src/BulletCommon/btQuaternion.h index 7d7f25fb4..843dedfa5 100644 --- a/src/BulletCommon/btQuaternion.h +++ b/src/BulletCommon/btQuaternion.h @@ -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); diff --git a/src/BulletCommon/btTransform.h b/src/BulletCommon/btTransform.h index 907627379..0744b5647 100644 --- a/src/BulletCommon/btTransform.h +++ b/src/BulletCommon/btTransform.h @@ -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); } diff --git a/src/BulletCommon/btTransformUtil.h b/src/BulletCommon/btTransformUtil.h index 2303c2742..4136ef5b1 100644 --- a/src/BulletCommon/btTransformUtil.h +++ b/src/BulletCommon/btTransformUtil.h @@ -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(); diff --git a/src/BulletCommon/btVector3.h b/src/BulletCommon/btVector3.h index 5001dfa9f..312286940 100644 --- a/src/BulletCommon/btVector3.h +++ b/src/BulletCommon/btVector3.h @@ -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 }