From 883d3278ec5e53a9dddd9d8c4dcd4a0653a5c002 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Thu, 3 Apr 2008 18:35:28 +0000 Subject: [PATCH] + fixed btMinkowskiSumShape Sa+b(v) = Sa(v)-Sb(-v) instead of Sa(v)+Sb(v) + fix related btMinkowskiSumShape issue in btSubsimplexConvexCast and btGjkConvexCast + add hitpoint for btSubsimplexConvexCast + reduce maximum number of iterations in conservative advancement/CCD implementations --- .../LinearConvexCastDemo.cpp | 34 +++- Demos/Raytracer/Raytracer.cpp | 1 + .../btConvexConvexAlgorithm.cpp | 1 - .../CollisionShapes/btMinkowskiSumShape.cpp | 4 +- .../btContinuousConvexCollision.cpp | 3 +- .../NarrowPhaseCollision/btGjkConvexCast.cpp | 159 +++++++++--------- .../btMinkowskiPenetrationDepthSolver.cpp | 3 +- .../btSubSimplexConvexCast.cpp | 62 ++++--- 8 files changed, 139 insertions(+), 128 deletions(-) diff --git a/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp b/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp index 1f2495e9a..f62d6fdf2 100644 --- a/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp +++ b/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp @@ -84,6 +84,7 @@ void LinearConvexCastDemo::initPhysics() shapeA->addPoint( btVector3( -r, -0.25f * h, -r ) ); + // Triangle btConvexHullShape* shapeB = new btConvexHullShape; shapeB->addPoint( btVector3( 0.0f, 1.0f, 0.0f ) ); @@ -118,25 +119,44 @@ void LinearConvexCastDemo::displayCallback(void) GL_ShapeDrawer::drawCoordSystem(); + + + static btScalar angle = 0.f; + angle+=getDeltaTimeMicroseconds()/1000000.0; + + tr[1].setRotation(btQuaternion(btVector3(1,0,0),angle)); + btTransform toA, toB; - toA.setIdentity(); - toA.setOrigin( btVector3( 0.0f, 1.5f, 0.0f ) ); - toB.setIdentity(); - toB.setOrigin( btVector3( 0.0f, 4.0f, 0.0f ) ); + toA = tr[0]; + toA.setOrigin( btVector3( 0.0f, 0.f, 0.0f ) ); + toB = tr[1]; + toB.setOrigin( btVector3( 0.0f, 0.0f, 0.0f ) ); gGjkSimplexSolver.reset(); - btGjkConvexCast convexCaster(shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver ); - //btSubsimplexConvexCast convexCaster( shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver ); + + //btGjkConvexCast convexCaster(shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver ); + btSubsimplexConvexCast convexCaster( shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver ); btConvexCast::CastResult result; + + result.m_hitPoint.setValue(0,0,0); + convexCaster.calcTimeOfImpact( tr[ 0 ], toA, tr[ 1 ], toB, result ); - btScalar m1[16], m2[16]; + btScalar m1[16], m2[16],m3[16]; tr[ 0 ].getOpenGLMatrix( m1 ); tr[ 1 ].getOpenGLMatrix( m2 ); + btSphereShape sphere(0.2); + + btTransform tmp = tr[0]; + tmp.setOrigin(result.m_hitPoint); + tmp.getOpenGLMatrix(m3); + m_shapeDrawer.drawOpenGL( m3, &sphere, btVector3( 1, 0, 1 ), getDebugMode() ); + + m_shapeDrawer.drawOpenGL( m1, shapePtr[ 0 ], btVector3( 1, 0, 0 ), getDebugMode() ); m_shapeDrawer.drawOpenGL( m2, shapePtr[ 1 ], btVector3( 1, 0, 0 ), getDebugMode() ); diff --git a/Demos/Raytracer/Raytracer.cpp b/Demos/Raytracer/Raytracer.cpp index 9b06f639a..a6f353299 100644 --- a/Demos/Raytracer/Raytracer.cpp +++ b/Demos/Raytracer/Raytracer.cpp @@ -101,6 +101,7 @@ void Raytracer::initPhysics() { transforms[i].setIdentity(); btVector3 pos(0.f,0.f,-(2.5* numObjects * 0.5)+i*2.5f); + transforms[i].setIdentity(); transforms[i].setOrigin( pos ); btQuaternion orn; if (i < 2) diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 7ae33901e..cb830f889 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -33,7 +33,6 @@ subject to the following restrictions: -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" diff --git a/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 7f7f28221..8f893ac98 100644 --- a/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -26,9 +26,9 @@ m_shapeB(shapeB) btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis())); + btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(-vec*m_transA.getBasis())); btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis())); - return supVertexA + supVertexB; + return supVertexA - supVertexB; } void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const diff --git a/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 562c0dc47..0d27eef40 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -16,7 +16,6 @@ subject to the following restrictions: #include "btContinuousConvexCollision.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "LinearMath/btTransformUtil.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -35,7 +34,7 @@ m_convexA(convexA),m_convexB(convexB) /// This maximum should not be necessary. It allows for untested/degenerate cases in production code. /// You don't want your game ever to lock-up. -#define MAX_ITERATIONS 1000 +#define MAX_ITERATIONS 64 bool btContinuousConvexCollision::calcTimeOfImpact( const btTransform& fromA, diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp index c9e690ab7..450b59560 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -17,10 +17,15 @@ subject to the following restrictions: #include "btGjkConvexCast.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "btGjkPairDetector.h" #include "btPointCollector.h" +#include "LinearMath/btTransformUtil.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define MAX_ITERATIONS 64 +#else +#define MAX_ITERATIONS 32 +#endif btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) :m_simplexSolver(simplexSolver), @@ -38,121 +43,113 @@ bool btGjkConvexCast::calcTimeOfImpact( { - btMinkowskiSumShape combi(m_convexA,m_convexB); - btMinkowskiSumShape* convex = &combi; + m_simplexSolver->reset(); - btTransform rayFromLocalA; - btTransform rayToLocalA; - - rayFromLocalA = fromA.inverse()* fromB; - rayToLocalA = toA.inverse()* toB; - - - btTransform trA,trB; - trA = btTransform(fromA); - trB = btTransform(fromB); - trA.setOrigin(btPoint3(0,0,0)); - trB.setOrigin(btPoint3(0,0,0)); - - convex->setTransformA(trA); - convex->setTransformB(trB); - - - - - btScalar radius = btScalar(0.01); + /// compute linear velocity for this interval, to interpolate + //assume no rotation/angular velocity, assert here? + btVector3 linVelA,linVelB; + linVelA = toA.getOrigin()-fromA.getOrigin(); + linVelB = toB.getOrigin()-fromB.getOrigin(); + btScalar radius = btScalar(0.001); btScalar lambda = btScalar(0.); - btVector3 s = rayFromLocalA.getOrigin(); - btVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin(); - btVector3 x = s; + btVector3 v(1,0,0); + + int maxIter = MAX_ITERATIONS; + btVector3 n; - n.setValue(0,0,0); + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); bool hasResult = false; btVector3 c; + btVector3 r = (linVelA-linVelB); btScalar lastLambda = lambda; + //btScalar epsilon = btScalar(0.001); + int numIter = 0; //first solution, using GJK - //no penetration support for now, perhaps pass a pointer when we really want it - btConvexPenetrationDepthSolver* penSolverPtr = 0; btTransform identityTrans; identityTrans.setIdentity(); - btSphereShape raySphere(btScalar(0.0)); - raySphere.setMargin(btScalar(0.)); - btTransform sphereTr; - sphereTr.setIdentity(); - sphereTr.setOrigin( rayFromLocalA.getOrigin()); +// result.drawCoordSystem(sphereTr); - result.drawCoordSystem(sphereTr); - { - btPointCollector pointCollector1; - btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr); + btPointCollector pointCollector; - btGjkPairDetector::ClosestPointInput input; - input.m_transformA = sphereTr; - input.m_transformB = identityTrans; - gjk.getClosestPoints(input,pointCollector1,0); + + btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver); + btGjkPairDetector::ClosestPointInput input; - hasResult = pointCollector1.m_hasResult; - c = pointCollector1.m_pointInWorld; - n = pointCollector1.m_normalOnBInWorld; - } + //we don't use margins during CCD + // gjk.setIgnoreMargin(true); - + input.m_transformA = fromA; + input.m_transformB = fromB; + gjk.getClosestPoints(input,pointCollector,0); + + hasResult = pointCollector.m_hasResult; + c = pointCollector.m_pointInWorld; if (hasResult) { btScalar dist; - dist = (c-x).length(); - if (dist < radius) - { - //penetration - lastLambda = btScalar(1.); - } + dist = pointCollector.m_distance; + n = pointCollector.m_normalOnBInWorld; + + //not close enough while (dist > radius) { - - n = x - c; - btScalar nDotr = n.dot(r); + numIter++; + if (numIter > maxIter) + { + return false; //todo: report a failure + } + btScalar dLambda = btScalar(0.); - if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON)) + btScalar projectedLinearVelocity = r.dot(n); + + dLambda = dist / (projectedLinearVelocity); + + lambda = lambda - dLambda; + + if (lambda > btScalar(1.)) return false; - - lambda = lambda - n.dot(n) / nDotr; - if (lambda <= lastLambda) - break; + if (lambda < btScalar(0.)) + return false; + + //todo: next check with relative epsilon + if (lambda <= lastLambda) + { + return false; + //n.setValue(0,0,0); + break; + } lastLambda = lambda; - x = s + lambda * r; - - sphereTr.setOrigin( x ); - result.drawCoordSystem(sphereTr); - btPointCollector pointCollector; - btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr); - btGjkPairDetector::ClosestPointInput input; - input.m_transformA = sphereTr; - input.m_transformB = identityTrans; + //interpolate to next lambda + result.DebugDraw( lambda ); + input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); + input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); + gjk.getClosestPoints(input,pointCollector,0); if (pointCollector.m_hasResult) { if (pointCollector.m_distance < btScalar(0.)) { - //degeneracy, report a hit result.m_fraction = lastLambda; - result.m_normal = n; + n = pointCollector.m_normalOnBInWorld; + result.m_normal=n; result.m_hitPoint = pointCollector.m_pointInWorld; return true; } - c = pointCollector.m_pointInWorld; - dist = (c-x).length(); + c = pointCollector.m_pointInWorld; + n = pointCollector.m_normalOnBInWorld; + dist = pointCollector.m_distance; } else { //?? @@ -161,16 +158,14 @@ bool btGjkConvexCast::calcTimeOfImpact( } - if (lastLambda < btScalar(1.)) - { - - result.m_fraction = lastLambda; - result.m_normal = n; - result.m_hitPoint = c; - return true; - } + result.m_fraction = lambda; + result.m_normal = n; + result.m_hitPoint = c; + return true; } return false; + + } diff --git a/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index 100bc2407..6b1e27a63 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -14,11 +14,10 @@ subject to the following restrictions: */ #include "btMinkowskiPenetrationDepthSolver.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" - +#include "BulletCollision/CollisionShapes/btConvexShape.h" diff --git a/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index 687738b7f..29174385a 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -16,9 +16,11 @@ subject to the following restrictions: #include "btSubSimplexConvexCast.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" + #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" - +#include "btPointCollector.h" +#include "LinearMath/btTransformUtil.h" btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) :m_simplexSolver(simplexSolver), @@ -41,34 +43,24 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( CastResult& result) { - btMinkowskiSumShape combi(m_convexA,m_convexB); - btMinkowskiSumShape* convex = &combi; - - btTransform rayFromLocalA; - btTransform rayToLocalA; - - rayFromLocalA = fromA.inverse()* fromB; - rayToLocalA = toA.inverse()* toB; - - m_simplexSolver->reset(); - convex->setTransformB(btTransform(rayFromLocalA.getBasis())); - - //btScalar radius = btScalar(0.01); + btVector3 linVelA,linVelB; + linVelA = toA.getOrigin()-fromA.getOrigin(); + linVelB = toB.getOrigin()-fromB.getOrigin(); btScalar lambda = btScalar(0.); - //todo: need to verify this: - //because of minkowski difference, we need the inverse direction - - btVector3 s = -rayFromLocalA.getOrigin(); - btVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin()); - btVector3 x = s; - btVector3 v; - btVector3 arbitraryPoint = convex->localGetSupportingVertex(r); - - v = x - arbitraryPoint; + btTransform interpolatedTransA = fromA; + btTransform interpolatedTransB = fromB; + + ///take relative motion + btVector3 r = (linVelA-linVelB); + btVector3 v; + + btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis())); + btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis())); + v = supVertexA-supVertexB; int maxIter = MAX_ITERATIONS; btVector3 n; @@ -90,8 +82,9 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( while ( (dist2 > epsilon) && maxIter--) { - p = convex->localGetSupportingVertex( v); - w = x - p; + supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis())); + supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis())); + w = supVertexA-supVertexB; btScalar VdotW = v.dot(w); @@ -104,16 +97,19 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( else { lambda = lambda - VdotW / VdotR; - x = s + lambda * r; - m_simplexSolver->reset(); + //interpolate to next lambda + // x = s + lambda * r; + interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); + interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); + //m_simplexSolver->reset(); //check next line - w = x-p; + w = supVertexA-supVertexB; lastLambda = lambda; n = v; hasResult = true; } } - m_simplexSolver->addVertex( w, x , p); + m_simplexSolver->addVertex( w, supVertexA , supVertexB); if (m_simplexSolver->closest(v)) { dist2 = v.length2(); @@ -130,8 +126,10 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( //int numiter = MAX_ITERATIONS - maxIter; // printf("number of iterations: %d", numiter); result.m_fraction = lambda; - result.m_normal = n; - + result.m_normal = n.normalized(); + btVector3 hitA,hitB; + m_simplexSolver->compute_points(hitA,hitB); + result.m_hitPoint=hitB; return true; }