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

+ fixed btMinkowskiSumShape Sa+b(v) = Sa(v)-Sb(-v) instead of Sa(v)+Sb(v)

Browse Source 
+ fix related btMinkowskiSumShape issue in btSubsimplexConvexCast and btGjkConvexCast
+ add hitpoint for btSubsimplexConvexCast
+ reduce maximum number of iterations in conservative advancement/CCD implementations
This commit is contained in:
erwin.coumans
2008-04-03 18:35:28 +00:00
parent 8ddbf12f1a
commit 883d3278ec
8 changed files with 139 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp
View File

@@ -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() );

1
Demos/Raytracer/Raytracer.cpp
View File

@@ -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)

1
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -33,7 +33,6 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"

4
src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
View File

@@ -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

3
src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
View File

@@ -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,

159
src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
View File

@@ -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;
}

3
src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
View File

@@ -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"

62
src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
View File

@@ -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;
}