diff --git a/Bullet/CollisionDispatch/CollisionDispatcher.cpp b/Bullet/CollisionDispatch/CollisionDispatcher.cpp index 59a01bf5c..06e999fbe 100644 --- a/Bullet/CollisionDispatch/CollisionDispatcher.cpp +++ b/Bullet/CollisionDispatch/CollisionDispatcher.cpp @@ -21,6 +21,7 @@ subject to the following restrictions: #include "CollisionDispatch/ConvexConvexAlgorithm.h" #include "CollisionDispatch/EmptyCollisionAlgorithm.h" #include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h" +#include "CollisionDispatch/CompoundCollisionAlgorithm.h" #include "CollisionShapes/CollisionShape.h" #include "CollisionDispatch/CollisionObject.h" #include @@ -122,6 +123,17 @@ CollisionAlgorithm* CollisionDispatcher::InternalFindAlgorithm(BroadphaseProxy& return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0); } + if (body0->m_collisionShape->IsCompound()) + { + return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1); + } else + { + if (body1->m_collisionShape->IsCompound()) + { + return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0); + } + } + //failed to find an algorithm return new EmptyAlgorithm(ci); diff --git a/Bullet/CollisionDispatch/CollisionWorld.cpp b/Bullet/CollisionDispatch/CollisionWorld.cpp index 011f05066..bc4413c34 100644 --- a/Bullet/CollisionDispatch/CollisionWorld.cpp +++ b/Bullet/CollisionDispatch/CollisionWorld.cpp @@ -20,6 +20,7 @@ subject to the following restrictions: #include "CollisionShapes/SphereShape.h" //for raycasting #include "CollisionShapes/TriangleMeshShape.h" //for raycasting #include "NarrowPhaseCollision/RaycastCallback.h" +#include "CollisionShapes/CompoundShape.h" #include "NarrowPhaseCollision/SubSimplexConvexCast.h" #include "BroadphaseCollision/BroadphaseInterface.h" @@ -139,58 +140,27 @@ void CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject) } } - - -void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback) +void RayTestSingle(const SimdTransform& rayFromTrans,const SimdTransform& rayToTrans, + CollisionObject* collisionObject, + const CollisionShape* collisionShape, + const SimdTransform& colObjWorldTransform, + CollisionWorld::RayResultCallback& resultCallback) { - - SimdTransform rayFromTrans,rayToTrans; - rayFromTrans.setIdentity(); - rayFromTrans.setOrigin(rayFromWorld); - rayToTrans.setIdentity(); - - rayToTrans.setOrigin(rayToWorld); - - //do culling based on aabb (rayFrom/rayTo) - SimdVector3 rayAabbMin = rayFromWorld; - SimdVector3 rayAabbMax = rayFromWorld; - rayAabbMin.setMin(rayToWorld); - rayAabbMax.setMax(rayToWorld); - SphereShape pointShape(0.0f); - /// brute force go over all objects. Once there is a broadphase, use that, or - /// add a raycast against aabb first. - - std::vector::iterator iter; - - for (iter=m_collisionObjects.begin(); - !(iter==m_collisionObjects.end()); iter++) - { - - CollisionObject* collisionObject= (*iter); - - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax); - - //check aabb overlap - - if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax)) - { - if (collisionObject->m_collisionShape->IsConvex()) + if (collisionShape->IsConvex()) { ConvexCast::CastResult castResult; castResult.m_fraction = 1.f;//?? - ConvexShape* convexShape = (ConvexShape*) collisionObject->m_collisionShape; + ConvexShape* convexShape = (ConvexShape*) collisionShape; VoronoiSimplexSolver simplexSolver; SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,collisionObject->m_worldTransform,collisionObject->m_worldTransform,castResult)) + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { //add hit if (castResult.m_normal.length2() > 0.0001f) @@ -217,12 +187,12 @@ void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& else { - if (collisionObject->m_collisionShape->IsConcave()) + if (collisionShape->IsConcave()) { - TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionObject->m_collisionShape; + TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionShape; - SimdTransform worldTocollisionObject = collisionObject->m_worldTransform.inverse(); + SimdTransform worldTocollisionObject = colObjWorldTransform.inverse(); SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); @@ -231,12 +201,12 @@ void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback { - RayResultCallback* m_resultCallback; + CollisionWorld::RayResultCallback* m_resultCallback; CollisionObject* m_collisionObject; TriangleMeshShape* m_triangleMesh; BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to, - RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape* triangleMesh): + CollisionWorld::RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape* triangleMesh): TriangleRaycastCallback(from,to), m_resultCallback(resultCallback), m_collisionObject(collisionObject), @@ -275,10 +245,75 @@ void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); - } - + } else + { + //todo: use AABB tree or other BVH acceleration structure! + if (collisionShape->IsCompound()) + { + const CompoundShape* compoundShape = static_cast(collisionShape); + int i=0; + for (i=0;iGetNumChildShapes();i++) + { + SimdTransform childTrans = compoundShape->GetChildTransform(i); + const CollisionShape* childCollisionShape = compoundShape->GetChildShape(i); + SimdTransform childWorldTrans = colObjWorldTransform * childTrans; + RayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback); + } + + + } + } } +} + +void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback) +{ + + + SimdTransform rayFromTrans,rayToTrans; + rayFromTrans.setIdentity(); + rayFromTrans.setOrigin(rayFromWorld); + rayToTrans.setIdentity(); + + rayToTrans.setOrigin(rayToWorld); + + //do culling based on aabb (rayFrom/rayTo) + SimdVector3 rayAabbMin = rayFromWorld; + SimdVector3 rayAabbMax = rayFromWorld; + rayAabbMin.setMin(rayToWorld); + rayAabbMax.setMax(rayToWorld); + + + /// brute force go over all objects. Once there is a broadphase, use that, or + /// add a raycast against aabb first. + + std::vector::iterator iter; + + for (iter=m_collisionObjects.begin(); + !(iter==m_collisionObjects.end()); iter++) + { + + CollisionObject* collisionObject= (*iter); + + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax); + + //check aabb overlap + + if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax)) + { + RayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + collisionObject->m_collisionShape, + collisionObject->m_worldTransform, + resultCallback); + } } diff --git a/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp b/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp index a608df2cb..f42497999 100644 --- a/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp +++ b/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp @@ -14,19 +14,70 @@ subject to the following restrictions: */ #include "CollisionDispatch/CompoundCollisionAlgorithm.h" +#include "CollisionDispatch/CollisionObject.h" +#include "CollisionShapes/CompoundShape.h" CompoundCollisionAlgorithm::CompoundCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) +:m_dispatcher(ci.m_dispatcher), +m_compoundProxy(*proxy0), +m_otherProxy(*proxy1) { + CollisionObject* colObj = static_cast(m_compoundProxy.m_clientObject); + assert (colObj->m_collisionShape->IsCompound()); + + CompoundShape* compoundShape = static_cast(colObj->m_collisionShape); + int numChildren = compoundShape->GetNumChildShapes(); + m_childProxies.resize( numChildren ); + int i; + for (i=0;iGetChildShape(i); + CollisionObject* colObj = static_cast(m_childProxies[i].m_clientObject); + CollisionShape* orgShape = colObj->m_collisionShape; + colObj->m_collisionShape = childShape; + m_childCollisionAlgorithms[i] = m_dispatcher->FindAlgorithm(m_childProxies[i],m_otherProxy); + colObj->m_collisionShape =orgShape; + } } + CompoundCollisionAlgorithm::~CompoundCollisionAlgorithm() { + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i(m_compoundProxy.m_clientObject); + assert (colObj->m_collisionShape->IsCompound()); + CompoundShape* compoundShape = static_cast(colObj->m_collisionShape); + + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;iGetChildShape(i); + CollisionObject* colObj = static_cast(m_childProxies[i].m_clientObject); + CollisionShape* orgShape = colObj->m_collisionShape; + colObj->m_collisionShape = childShape; + m_childCollisionAlgorithms[i]->ProcessCollision(&m_childProxies[i],&m_otherProxy,dispatchInfo); + //revert back + colObj->m_collisionShape =orgShape; + } } float CompoundCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) diff --git a/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h b/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h index ee82675b8..a41d3a8f4 100644 --- a/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h +++ b/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h @@ -23,14 +23,19 @@ subject to the following restrictions: #include "NarrowPhaseCollision/PersistentManifold.h" class Dispatcher; #include "BroadphaseCollision/BroadphaseProxy.h" - +#include /// CompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes /// Place holder, not fully implemented yet class CompoundCollisionAlgorithm : public CollisionAlgorithm { + BroadphaseProxy m_compoundProxy; + BroadphaseProxy m_otherProxy; + std::vector m_childProxies; + std::vector m_childCollisionAlgorithms; + Dispatcher* m_dispatcher; BroadphaseProxy m_compound; BroadphaseProxy m_other; diff --git a/Bullet/CollisionShapes/CompoundShape.cpp b/Bullet/CollisionShapes/CompoundShape.cpp index 8104a3afa..3de0c61db 100644 --- a/Bullet/CollisionShapes/CompoundShape.cpp +++ b/Bullet/CollisionShapes/CompoundShape.cpp @@ -20,6 +20,8 @@ subject to the following restrictions: CompoundShape::CompoundShape() +:m_localAabbMin(1e30f,1e30f,1e30f), +m_localAabbMax(-1e30f,-1e30f,-1e30f) { } @@ -28,21 +30,66 @@ CompoundShape::~CompoundShape() { } +void CompoundShape::AddChildShape(const SimdTransform& localTransform,CollisionShape* shape) +{ + m_childTransforms.push_back(localTransform); + m_childShapes.push_back(shape); + + //extend the local aabbMin/aabbMax + SimdVector3 localAabbMin,localAabbMax; + shape->GetAabb(localTransform,localAabbMin,localAabbMax); + for (int i=0;i<3;i++) + { + if (m_localAabbMin[i] > localAabbMin[i]) + { + m_localAabbMin[i] = localAabbMin[i]; + } + if (m_localAabbMax[i] < localAabbMax[i]) + { + m_localAabbMax[i] = localAabbMax[i]; + } + + } +} + + ///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void CompoundShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const +void CompoundShape::GetAabb(const SimdTransform& trans,SimdVector3& aabbMin,SimdVector3& aabbMax) const { - SimdVector3 margin(GetMargin(),GetMargin(),GetMargin()); + SimdVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin); + SimdVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin); + + SimdMatrix3x3 abs_b = trans.getBasis().absolute(); - aabbMin = t.getOrigin() - margin; + SimdPoint3 center = trans(localCenter); - aabbMax = t.getOrigin() + margin; + SimdVector3 extent = SimdVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + extent += SimdVector3(GetMargin(),GetMargin(),GetMargin()); + aabbMin = center - extent; + aabbMax = center + extent; } void CompoundShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) { - assert(0); + //approximation: take the inertia from the aabb for now + SimdTransform ident; + SimdVector3 aabbMin,aabbMax; + GetAabb(ident,aabbMin,aabbMax); + + SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + + SimdScalar lx=2.f*(halfExtents.x()); + SimdScalar ly=2.f*(halfExtents.y()); + SimdScalar lz=2.f*(halfExtents.z()); + + inertia[0] = mass/(12.0f) * (ly*ly + lz*lz); + inertia[1] = mass/(12.0f) * (lx*lx + lz*lz); + inertia[2] = mass/(12.0f) * (lx*lx + ly*ly); + } diff --git a/Bullet/CollisionShapes/CompoundShape.h b/Bullet/CollisionShapes/CompoundShape.h index 86041b05b..1d3f249f8 100644 --- a/Bullet/CollisionShapes/CompoundShape.h +++ b/Bullet/CollisionShapes/CompoundShape.h @@ -28,14 +28,43 @@ subject to the following restrictions: /// CompoundShape allows to store multiple other CollisionShapes /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape. -/// Place holder, not fully implemented yet class CompoundShape : public CollisionShape { + std::vector m_childTransforms; + std::vector m_childShapes; + SimdVector3 m_localAabbMin; + SimdVector3 m_localAabbMax; + + public: CompoundShape(); virtual ~CompoundShape(); + void AddChildShape(const SimdTransform& localTransform,CollisionShape* shape); + + int GetNumChildShapes() const + { + return m_childShapes.size(); + } + + CollisionShape* GetChildShape(int index) + { + return m_childShapes[index]; + } + const CollisionShape* GetChildShape(int index) const + { + return m_childShapes[index]; + } + + SimdTransform GetChildTransform(int index) + { + return m_childTransforms[index]; + } + const SimdTransform GetChildTransform(int index) const + { + return m_childTransforms[index]; + } ///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const; diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index a8f2d4931..9b60bed20 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -26,7 +26,7 @@ subject to the following restrictions: #include "CollisionShapes/SphereShape.h" #include "CollisionShapes/ConeShape.h" #include "CollisionShapes/StaticPlaneShape.h" - +#include "CollisionShapes/CompoundShape.h" #include "CollisionShapes/Simplex1to4Shape.h" #include "CollisionShapes/EmptyShape.h" @@ -126,7 +126,7 @@ CollisionShape* shapePtr[numShapes] = #else new BoxShape (SimdVector3(450,10,450)), #endif - + new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)), new SphereShape (CUBE_HALF_EXTENTS- 0.05f), @@ -205,8 +205,13 @@ int main(int argc,char** argv) shapeIndex[i] = 0; } + CompoundShape* compoundShape = new CompoundShape(); + //shapePtr[1] = compoundShape; + SimdTransform ident; + ident.setIdentity(); + compoundShape->AddChildShape(ident,new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS))); for (i=0;iGetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId); physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug); + + float vec[16]; + SimdTransform ident; + ident.setIdentity(); + ident.getOpenGLMatrix(vec); + glPushMatrix(); + glLoadMatrixf(vec); + GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode()); + glPopMatrix(); ///this block is just experimental code to show some internal issues with replacing shapes on the fly. if (getDebugMode()!=0 && (i>0)) { diff --git a/Demos/OpenGL/GL_ShapeDrawer.cpp b/Demos/OpenGL/GL_ShapeDrawer.cpp index 81b57e215..175749ef7 100644 --- a/Demos/OpenGL/GL_ShapeDrawer.cpp +++ b/Demos/OpenGL/GL_ShapeDrawer.cpp @@ -34,6 +34,7 @@ subject to the following restrictions: #include "CollisionShapes/ConeShape.h" #include "CollisionShapes/CylinderShape.h" #include "CollisionShapes/Simplex1to4Shape.h" +#include "CollisionShapes/CompoundShape.h" #include "CollisionShapes/ConvexTriangleMeshShape.h" @@ -105,225 +106,242 @@ public: void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const SimdVector3& color,int debugMode) { + + glPushMatrix(); - glLoadMatrixf(m); + glMultMatrixf(m); - //DrawCoordSystem(); - - glPushMatrix(); - glEnable(GL_COLOR_MATERIAL); - glColor3f(color.x(),color.y(), color.z()); - - glRasterPos3f(0.0, 0.0, 0.0); - - bool useWireframeFallback = true; - - if (!(debugMode & IDebugDraw::DBG_DrawWireframe)) + if (shape->GetShapeType() == COMPOUND_SHAPE_PROXYTYPE) { - switch (shape->GetShapeType()) + const CompoundShape* compoundShape = static_cast(shape); + for (int i=0;iGetNumChildShapes();i++) { - case BOX_SHAPE_PROXYTYPE: - { - const BoxShape* boxShape = static_cast(shape); - SimdVector3 halfExtent = boxShape->GetHalfExtents(); - glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]); - glutSolidCube(1.0); - useWireframeFallback = false; - break; - } - case TRIANGLE_SHAPE_PROXYTYPE: - case TETRAHEDRAL_SHAPE_PROXYTYPE: - { - const BU_Simplex1to4* tetra = static_cast(shape); - //todo: - useWireframeFallback = false; - break; - } - case CONVEX_HULL_SHAPE_PROXYTYPE: - break; - case SPHERE_SHAPE_PROXYTYPE: - { - const SphereShape* sphereShape = static_cast(shape); - float radius = sphereShape->GetMargin();//radius doesn't include the margin, so draw with margin - glutSolidSphere(radius,10,10); - useWireframeFallback = false; - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: - case CONE_SHAPE_PROXYTYPE: - { - const ConeShape* coneShape = static_cast(shape); - float radius = coneShape->GetRadius();//+coneShape->GetMargin(); - float height = coneShape->GetHeight();//+coneShape->GetMargin(); - //glRotatef(-90.0, 1.0, 0.0, 0.0); - glTranslatef(0.0, 0.0, -0.5*height); - glutSolidCone(radius,height,10,10); - useWireframeFallback = false; - break; + SimdTransform childTrans = compoundShape->GetChildTransform(i); + const CollisionShape* colShape = compoundShape->GetChildShape(i); + float childMat[16]; + childTrans.getOpenGLMatrix(childMat); + DrawOpenGL(childMat,colShape,color,debugMode); + } - } - case CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE: - { - useWireframeFallback = false; - break; - } + return; + } else + { + //DrawCoordSystem(); + + glPushMatrix(); + glEnable(GL_COLOR_MATERIAL); + glColor3f(color.x(),color.y(), color.z()); - case CONVEX_SHAPE_PROXYTYPE: - case CYLINDER_SHAPE_PROXYTYPE: + glRasterPos3f(0.0, 0.0, 0.0); + + bool useWireframeFallback = true; + + if (!(debugMode & IDebugDraw::DBG_DrawWireframe)) + { + switch (shape->GetShapeType()) { - const CylinderShape* cylinder = static_cast(shape); - int upAxis = cylinder->GetUpAxis(); - - GLUquadricObj *quadObj = gluNewQuadric(); - - glPushMatrix(); - switch (upAxis) + case BOX_SHAPE_PROXYTYPE: { - case 0: - glRotatef(-90.0, 0.0, 1.0, 0.0); - glTranslatef(0.0, 0.0, -1.0); + const BoxShape* boxShape = static_cast(shape); + SimdVector3 halfExtent = boxShape->GetHalfExtents(); + glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]); + glutSolidCube(1.0); + useWireframeFallback = false; break; - case 1: - glRotatef(-90.0, 1.0, 0.0, 0.0); - glTranslatef(0.0, 0.0, -1.0); + } + case TRIANGLE_SHAPE_PROXYTYPE: + case TETRAHEDRAL_SHAPE_PROXYTYPE: + { + const BU_Simplex1to4* tetra = static_cast(shape); + //todo: + useWireframeFallback = false; break; - case 2: + } + case CONVEX_HULL_SHAPE_PROXYTYPE: + break; + case SPHERE_SHAPE_PROXYTYPE: + { + const SphereShape* sphereShape = static_cast(shape); + float radius = sphereShape->GetMargin();//radius doesn't include the margin, so draw with margin + glutSolidSphere(radius,10,10); + useWireframeFallback = false; + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + case CONE_SHAPE_PROXYTYPE: + { + const ConeShape* coneShape = static_cast(shape); + float radius = coneShape->GetRadius();//+coneShape->GetMargin(); + float height = coneShape->GetHeight();//+coneShape->GetMargin(); + //glRotatef(-90.0, 1.0, 0.0, 0.0); + glTranslatef(0.0, 0.0, -0.5*height); + glutSolidCone(radius,height,10,10); + useWireframeFallback = false; + break; + + } + case CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE: + { + useWireframeFallback = false; + break; + } + + case CONVEX_SHAPE_PROXYTYPE: + case CYLINDER_SHAPE_PROXYTYPE: + { + const CylinderShape* cylinder = static_cast(shape); + int upAxis = cylinder->GetUpAxis(); - glTranslatef(0.0, 0.0, -1.0); - break; - default: + GLUquadricObj *quadObj = gluNewQuadric(); + + glPushMatrix(); + switch (upAxis) { - assert(0); + case 0: + glRotatef(-90.0, 0.0, 1.0, 0.0); + glTranslatef(0.0, 0.0, -1.0); + break; + case 1: + glRotatef(-90.0, 1.0, 0.0, 0.0); + glTranslatef(0.0, 0.0, -1.0); + break; + case 2: + + glTranslatef(0.0, 0.0, -1.0); + break; + default: + { + assert(0); + } + + } + + //The gluCylinder subroutine draws a cylinder that is oriented along the z axis. + //The base of the cylinder is placed at z = 0; the top of the cylinder is placed at z=height. + //Like a sphere, the cylinder is subdivided around the z axis into slices and along the z axis into stacks. + + gluQuadricDrawStyle(quadObj, (GLenum)GLU_FILL); + gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH); + float radius = cylinder->GetHalfExtents().getX(); + float height = 2.f*cylinder->GetHalfExtents().getY(); + gluCylinder(quadObj, radius, radius, height, 15, 10); + glPopMatrix(); + glEndList(); + + break; + } + default: + { + } + + }; + + } + + + + + if (useWireframeFallback) + { + /// for polyhedral shapes + if (shape->IsPolyhedral()) + { + PolyhedralConvexShape* polyshape = (PolyhedralConvexShape*) shape; + + + glBegin(GL_LINES); + + + int i; + for (i=0;iGetNumEdges();i++) + { + SimdPoint3 a,b; + polyshape->GetEdge(i,a,b); + + glVertex3f(a.getX(),a.getY(),a.getZ()); + glVertex3f(b.getX(),b.getY(),b.getZ()); + + + } + glEnd(); + + + if (debugMode==IDebugDraw::DBG_DrawFeaturesText) + { + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->GetExtraDebugInfo()); + + glColor3f(1.f, 1.f, 1.f); + for (i=0;iGetNumVertices();i++) + { + SimdPoint3 vtx; + polyshape->GetVertex(i,vtx); + glRasterPos3f(vtx.x(), vtx.y(), vtx.z()); + char buf[12]; + sprintf(buf," %d",i); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); } + for (i=0;iGetNumPlanes();i++) + { + SimdVector3 normal; + SimdPoint3 vtx; + polyshape->GetPlane(normal,vtx,i); + SimdScalar d = vtx.dot(normal); + + glRasterPos3f(normal.x()*d, normal.y()*d, normal.z()*d); + char buf[12]; + sprintf(buf," plane %d",i); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + + } } + - //The gluCylinder subroutine draws a cylinder that is oriented along the z axis. - //The base of the cylinder is placed at z = 0; the top of the cylinder is placed at z=height. - //Like a sphere, the cylinder is subdivided around the z axis into slices and along the z axis into stacks. - - gluQuadricDrawStyle(quadObj, (GLenum)GLU_FILL); - gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH); - float radius = cylinder->GetHalfExtents().getX(); - float height = 2.f*cylinder->GetHalfExtents().getY(); - gluCylinder(quadObj, radius, radius, height, 15, 10); - glPopMatrix(); - glEndList(); - - break; } - default: - { - } - - }; - - } - - - - - if (useWireframeFallback) - { - /// for polyhedral shapes - if (shape->IsPolyhedral()) - { - PolyhedralConvexShape* polyshape = (PolyhedralConvexShape*) shape; - - - glBegin(GL_LINES); - - - int i; - for (i=0;iGetNumEdges();i++) - { - SimdPoint3 a,b; - polyshape->GetEdge(i,a,b); - - glVertex3f(a.getX(),a.getY(),a.getZ()); - glVertex3f(b.getX(),b.getY(),b.getZ()); - - - } - glEnd(); - - - if (debugMode==IDebugDraw::DBG_DrawFeaturesText) - { - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->GetExtraDebugInfo()); - - glColor3f(1.f, 1.f, 1.f); - for (i=0;iGetNumVertices();i++) - { - SimdPoint3 vtx; - polyshape->GetVertex(i,vtx); - glRasterPos3f(vtx.x(), vtx.y(), vtx.z()); - char buf[12]; - sprintf(buf," %d",i); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - } - - for (i=0;iGetNumPlanes();i++) - { - SimdVector3 normal; - SimdPoint3 vtx; - polyshape->GetPlane(normal,vtx,i); - SimdScalar d = vtx.dot(normal); - - glRasterPos3f(normal.x()*d, normal.y()*d, normal.z()*d); - char buf[12]; - sprintf(buf," plane %d",i); - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); - - } - } - - } - } - if (shape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - TriangleMeshShape* concaveMesh = (TriangleMeshShape*) shape; - //SimdVector3 aabbMax(1e30f,1e30f,1e30f); - //SimdVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); + if (shape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + TriangleMeshShape* concaveMesh = (TriangleMeshShape*) shape; + //SimdVector3 aabbMax(1e30f,1e30f,1e30f); + //SimdVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); + + extern float eye[3]; + SimdVector3 aabbMax(eye[0]+100,eye[1]+100,eye[2]+100);//1e30f,1e30f,1e30f); + SimdVector3 aabbMin(eye[0]-100,eye[1]-100,eye[2]-100);//1e30f,1e30f,1e30f); + + GlDrawcallback drawCallback; + + concaveMesh->ProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + + + } + + if (shape->GetShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + ConvexTriangleMeshShape* convexMesh = (ConvexTriangleMeshShape*) shape; + + extern float eye[3]; + SimdVector3 aabbMax(eye[0]+100,eye[1]+100,eye[2]+100); + SimdVector3 aabbMin(eye[0]-100,eye[1]-100,eye[2]-100); + TriangleGlDrawcallback drawCallback; + convexMesh->GetStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + + } - extern float eye[3]; - SimdVector3 aabbMax(eye[0]+100,eye[1]+100,eye[2]+100);//1e30f,1e30f,1e30f); - SimdVector3 aabbMin(eye[0]-100,eye[1]-100,eye[2]-100);//1e30f,1e30f,1e30f); - - GlDrawcallback drawCallback; - - concaveMesh->ProcessAllTriangles(&drawCallback,aabbMin,aabbMax); - + glDisable(GL_DEPTH_BUFFER_BIT); + if (debugMode==IDebugDraw::DBG_DrawText) + { + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetName()); + } + if (debugMode==IDebugDraw::DBG_DrawFeaturesText) + { + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetExtraDebugInfo()); + } + glEnable(GL_DEPTH_BUFFER_BIT); + glPopMatrix(); } - - if (shape->GetShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) - { - ConvexTriangleMeshShape* convexMesh = (ConvexTriangleMeshShape*) shape; - - extern float eye[3]; - SimdVector3 aabbMax(eye[0]+100,eye[1]+100,eye[2]+100); - SimdVector3 aabbMin(eye[0]-100,eye[1]-100,eye[2]-100); - TriangleGlDrawcallback drawCallback; - convexMesh->GetStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); - - } - - glDisable(GL_DEPTH_BUFFER_BIT); - if (debugMode==IDebugDraw::DBG_DrawText) - { - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetName()); - } - - if (debugMode==IDebugDraw::DBG_DrawFeaturesText) - { - BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetExtraDebugInfo()); - } - glEnable(GL_DEPTH_BUFFER_BIT); - - glPopMatrix(); glPopMatrix(); }