From 6d47d9492ed62dbefa30c39c64763cb4f235d76e Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Thu, 9 Nov 2006 01:58:33 +0000 Subject: [PATCH] added non-uniform scaling to btMultiSphereShape added ray-aabb check modified Raycast demo to be more useful for debugging collision shapes --- Demos/OpenGL/GLDebugDrawer.cpp | 33 +++++ Demos/OpenGL/GLDebugDrawer.h | 2 + Demos/OpenGL/RenderTexture.cpp | 2 +- Demos/OpenGL/RenderTexture.h | 9 ++ Demos/Raytracer/Raytracer.cpp | 123 ++++++++++++------ .../CollisionDispatch/btCollisionObject.h | 2 +- .../CollisionDispatch/btCollisionWorld.cpp | 4 +- .../CollisionShapes/btMultiSphereShape.cpp | 12 +- .../CollisionShapes/btMultiSphereShape.h | 4 +- src/LinearMath/btAabbUtil2.h | 69 ++++++++++ src/LinearMath/btScalar.h | 4 +- src/LinearMath/btSimdMinMax.h | 1 + 12 files changed, 211 insertions(+), 54 deletions(-) diff --git a/Demos/OpenGL/GLDebugDrawer.cpp b/Demos/OpenGL/GLDebugDrawer.cpp index b616198d5..8c886bf77 100644 --- a/Demos/OpenGL/GLDebugDrawer.cpp +++ b/Demos/OpenGL/GLDebugDrawer.cpp @@ -61,3 +61,36 @@ void GLDebugDrawer::drawContactPoint(const btVector3& pointOnB,const btVector3& } } + + +void GLDebugDrawer::drawAabb(const btVector3& from,const btVector3& to,const btVector3& color) +{ + + btVector3 halfExtents = (to-from)* 0.5f; + btVector3 center = (to+from) *0.5f; + int i,j; + + btVector3 edgecoord(1.f,1.f,1.f),pa,pb; + for (i=0;i<4;i++) + { + for (j=0;j<3;j++) + { + pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], + edgecoord[2]*halfExtents[2]); + pa+=center; + + int othercoord = j%3; + edgecoord[othercoord]*=-1.f; + pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], + edgecoord[2]*halfExtents[2]); + pb+=center; + + drawLine(pa,pb,color); + } + edgecoord = btVector3(-1.f,-1.f,-1.f); + if (i<3) + edgecoord[i]*=-1.f; + } + + +} \ No newline at end of file diff --git a/Demos/OpenGL/GLDebugDrawer.h b/Demos/OpenGL/GLDebugDrawer.h index 1aa89a683..9bb49b200 100644 --- a/Demos/OpenGL/GLDebugDrawer.h +++ b/Demos/OpenGL/GLDebugDrawer.h @@ -13,6 +13,8 @@ public: GLDebugDrawer(); + void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color); + virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color); virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,float distance,int lifeTime,const btVector3& color); diff --git a/Demos/OpenGL/RenderTexture.cpp b/Demos/OpenGL/RenderTexture.cpp index 90ea08513..c958ae4d7 100644 --- a/Demos/OpenGL/RenderTexture.cpp +++ b/Demos/OpenGL/RenderTexture.cpp @@ -53,7 +53,7 @@ void renderTexture::grapicalPrintf(char* str, BMF_FontData* fontData, int startx for (int x=0;x>=1; diff --git a/Demos/OpenGL/RenderTexture.h b/Demos/OpenGL/RenderTexture.h index 2fd70614b..1ced24773 100644 --- a/Demos/OpenGL/RenderTexture.h +++ b/Demos/OpenGL/RenderTexture.h @@ -43,6 +43,15 @@ public: pixel[3] = (unsigned char)(255*rgba.getW()); } + inline btVector4 getPixel(int x,int y) + { + unsigned char* pixel = &m_buffer[ (x+y*m_width) * 4]; + return btVector4(pixel[0]*1.f/255.f, + pixel[1]*1.f/255.f, + pixel[2]*1.f/255.f, + pixel[3]*1.f/255.f); + } + const unsigned char* getBuffer() const { return m_buffer;} int getWidth() const { return m_width;} int getHeight() const { return m_height;} diff --git a/Demos/Raytracer/Raytracer.cpp b/Demos/Raytracer/Raytracer.cpp index d4dacdd53..6d536a114 100644 --- a/Demos/Raytracer/Raytracer.cpp +++ b/Demos/Raytracer/Raytracer.cpp @@ -22,10 +22,12 @@ Very basic raytracer, rendering into a texture. #include "LinearMath/btQuaternion.h" #include "LinearMath/btTransform.h" #include "GL_ShapeDrawer.h" +#include "GLDebugDrawer.h" #include "Raytracer.h" #include "GlutStuff.h" + #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" @@ -37,8 +39,13 @@ Very basic raytracer, rendering into a texture. #include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" + #include "BulletCollision/CollisionShapes/btConvexHullShape.h" +#include "LinearMath/btAabbUtil2.h" #include "BulletCollision/CollisionShapes/btBoxShape.h" + + + #include "BulletCollision/CollisionShapes/btTetrahedronShape.h" #include "BulletCollision/CollisionShapes/btConeShape.h" #include "BulletCollision/CollisionShapes/btCylinderShape.h" @@ -48,11 +55,13 @@ Very basic raytracer, rendering into a texture. #include "RenderTexture.h" + + btVoronoiSimplexSolver simplexSolver; float yaw=0.f,pitch=0.f,roll=0.f; const int maxNumObjects = 4; -const int numObjects = 4; +const int numObjects = 1; /// simplex contains the vertices, and some extra code to draw and debug GL_Simplex1to4 simplex; @@ -62,8 +71,10 @@ btTransform transforms[maxNumObjects]; renderTexture* raytracePicture = 0; +//this applies to the raytracer virtual screen/image buffer int screenWidth = 128; -int screenHeight = 128; +float aspectRatio = (3.f/4.f); +int screenHeight = screenWidth * aspectRatio; GLuint glTextureId; btSphereShape mySphere(1); @@ -72,6 +83,9 @@ btCylinderShape myCylinder(btVector3(0.3f,0.3f,0.3f)); btConeShape myCone(1,1); btMinkowskiSumShape myMink(&myCylinder,&myBox); +GLDebugDrawer debugDrawer; + + /// @@ -85,7 +99,7 @@ int main(int argc,char** argv) raytraceDemo->setCameraDistance(6.f); - return glutmain(argc, argv,screenWidth,screenHeight,"Minkowski-Sum Raytracer Demo",raytraceDemo); + return glutmain(argc, argv,640,480,"Bullet GJK Implicit Shape Raytracer Demo",raytraceDemo); } void Raytracer::initPhysics() @@ -113,12 +127,18 @@ void Raytracer::initPhysics() btVector3(0.f, 0.f, 0.f) }; - // btMultiSphereShape* multiSphereShape = new btMultiSphereShape(inertiaHalfExtents,positions,radi,NUM_SPHERES); + //btMultiSphereShape* multiSphereShape = new btMultiSphereShape(inertiaHalfExtents,positions,radi,NUM_SPHERES); + btVector3 sphereOffset(0,0,0); + btScalar sphereRadius = 2.f; + btVector3 nonUniformScaling(0.5,2,0.5); + btMultiSphereShape* nonuniformScaledSphere = new btMultiSphereShape(inertiaHalfExtents,&sphereOffset,&sphereRadius,1); + nonuniformScaledSphere->setLocalScaling(nonUniformScaling); + nonuniformScaledSphere->setMargin(0.04); btConvexHullShape* convexHullShape = new btConvexHullShape(&positions[0].getX(),3); //choose shape - shapePtr[0] = &myCone; + shapePtr[0] = &myCone;//&myBox;//nonuniformScaledSphere;//&myCone; shapePtr[1] =&simplex; shapePtr[2] =convexHullShape; shapePtr[3] =&myMink;//myBox;//multiSphereShape @@ -138,6 +158,10 @@ void Raytracer::clientMoveAndDisplay() int once = 1; + + + + void Raytracer::displayCallback() { @@ -146,7 +170,7 @@ void Raytracer::displayCallback() for (int i=0;igetAabb(transforms[s],aabbMin,aabbMax); + btScalar hitLambda = 1.f; + btVector3 hitNormal; - //choose the continuous collision detection method - btSubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver); - //GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver); - //ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0); - - // btBU_Simplex1to4 ptShape(btVector3(0,0,0));//algebraic needs features, doesnt use 'supporting vertex' - // BU_CollisionPair convexCaster(&ptShape,shapePtr[0]); - - - //reset previous result - rayResult.m_fraction = 1.f; - - - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult)) + if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal)) { - //float fog = 1.f - 0.1f * rayResult.m_fraction; - rayResult.m_normal.normalize(); + + //choose the continuous collision detection method + btSubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver); + //GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver); + //ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0); + + //reset previous result + rayResult.m_fraction = 1.f; + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult)) + { + //float fog = 1.f - 0.1f * rayResult.m_fraction; + rayResult.m_normal.normalize(); - btVector3 worldNormal; - worldNormal = transforms[s].getBasis() *rayResult.m_normal; + btVector3 worldNormal; + worldNormal = transforms[s].getBasis() *rayResult.m_normal; + - float light = worldNormal.dot(btVector3(0.4f,-1.f,-0.4f)); - if (light < 0.2f) - light = 0.2f; - if (light > 1.f) - light = 1.f; + float lightVec0 = worldNormal.dot(btVector3(0,-1,-1));//0.4f,-1.f,-0.4f)); + float lightVec1= worldNormal.dot(btVector3(-1,0,-1));//-0.4f,-1.f,-0.4f)); - rgba = btVector4(light,light,light,1.f); - raytracePicture->setPixel(x,y,rgba); + + rgba = btVector4(lightVec0,lightVec1,0,1.f); + rgba.setMin(btVector3(1,1,1)); + rgba.setMax(btVector3(0.2,0.2,0.2)); + rgba[3] = 1.f; + raytracePicture->setPixel(x,y,rgba); + } else + { + //clear is already done + //rgba = btVector4(0.f,0.f,0.f,0.f); + //raytracePicture->setPixel(x,y,rgba); + } } else { - //clear is already done - //rgba = btVector4(0.f,0.f,0.f,0.f); - //raytracePicture->setPixel(x,y,rgba); - + btVector4 rgba = raytracePicture->getPixel(x,y); + if (!rgba.length2()) + { + raytracePicture->setPixel(x,y,btVector4(1,1,1,1)); + } } - } } @@ -371,9 +403,20 @@ void Raytracer::displayCallback() GL_ShapeDrawer::drawCoordSystem(); + + + { + for (int i=0;igetAabb(transforms[i],aabbMin,aabbMax); + debugDrawer.setDebugMode(1); + debugDrawer.drawAabb(aabbMin,aabbMax,btVector3(255,0,0)); + } + } + glPushMatrix(); - - + /* diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index ed53d65ca..b85534c1d 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -297,7 +297,7 @@ public: } ///users can point to their objects, userPointer is not used by Bullet - void* getUserPointer() + void* getUserPointer() const { return m_userObjectPointer; } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index f64bd792e..537dcd464 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -344,7 +344,9 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r //check aabb overlap - if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax)) + float hitLambda = 1.f; + btVector3 hitNormal; + if (btRayAabb(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) { rayTestSingle(rayFromTrans,rayToTrans, collisionObject, diff --git a/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index aaadb82eb..fde4d95da 100644 --- a/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -20,17 +20,17 @@ subject to the following restrictions: btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres) :m_inertiaHalfExtents(inertiaHalfExtents) { - m_minRadius = 1e30f; + float startMargin = 1e30f; m_numSpheres = numSpheres; for (int i=0;i b ? a : b)) @@ -53,5 +55,72 @@ SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices, return true; } + +SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent) +{ + return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) | + (p.getX() > halfExtent.getX() ? 0x08 : 0x0) | + (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) | + (p.getY() > halfExtent.getY() ? 0x10 : 0x0) | + (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) | + (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0); +} + +SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& aabbMin, + const btVector3& aabbMax, + btScalar& param, btVector3& normal) +{ + btVector3 aabbHalfExtent = (aabbMax-aabbMin)* 0.5f; + btVector3 aabbCenter = (aabbMax+aabbMin)* 0.5f; + btVector3 source = rayFrom - aabbCenter; + btVector3 target = rayTo - aabbCenter; + int sourceOutcode = btOutcode(source,aabbHalfExtent); + int targetOutcode = btOutcode(target,aabbHalfExtent); + if ((sourceOutcode & targetOutcode) == 0x0) + { + btScalar lambda_enter = btScalar(0.0); + btScalar lambda_exit = param; + btVector3 r = target - source; + int i; + btScalar normSign = 1; + btVector3 hitNormal(0,0,0); + int bit=1; + + for (int j=0;j<2;j++) + { + for (i = 0; i != 3; ++i) + { + if (sourceOutcode & bit) + { + btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i]; + if (lambda_enter <= lambda) + { + lambda_enter = lambda; + hitNormal.setValue(0,0,0); + hitNormal[i] = normSign; + } + } + else if (targetOutcode & bit) + { + btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i]; + btSetMin(lambda_exit, lambda); + } + bit<<=1; + } + normSign = -1.f; + } + if (lambda_enter <= lambda_exit) + { + param = lambda_enter; + normal = hitNormal; + return true; + } + } + return false; +} + + #endif diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index eb8e5aabb..aa046b89d 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -114,8 +114,8 @@ SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } */ -SIMD_FORCE_INLINE int btSign(btScalar x) { - return x < 0.0f ? -1 : x > 0.0f ? 1 : 0; +SIMD_FORCE_INLINE int btIsNegative(btScalar x) { + return x < 0.0f ? 1 : 0; } SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; } diff --git a/src/LinearMath/btSimdMinMax.h b/src/LinearMath/btSimdMinMax.h index 16c31552f..2731c8b09 100644 --- a/src/LinearMath/btSimdMinMax.h +++ b/src/LinearMath/btSimdMinMax.h @@ -16,6 +16,7 @@ subject to the following restrictions: #ifndef SIMD_MINMAX_H #define SIMD_MINMAX_H +#include "LinearMath/btScalar.h" template SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) {