Add support for generic 2d convex shapes, through wrapper class btConvex2dShape. See Bullet/Demos/Box2dDemo for example wrapping a btCylinderShape and 2d btConvexHullShape.
Add some extra degeneracy debugging check in btGjkPairDetector
This commit is contained in:
erwin.coumans
@@ -13,13 +13,18 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "btBox2dShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btBox2dShape.h"
|
||||
#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
|
||||
#include "btBox2dBox2dCollisionAlgorithm.h"
|
||||
#include "BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h"
|
||||
#include "BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h"
|
||||
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btConvex2dShape.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
|
||||
|
||||
///create 125 (5x5x5) dynamic object
|
||||
#define ARRAY_SIZE_X 17
|
||||
#define ARRAY_SIZE_Y 17
|
||||
#define ARRAY_SIZE_X 1
|
||||
#define ARRAY_SIZE_Y 2
|
||||
#define ARRAY_SIZE_Z 1
|
||||
|
||||
//maximum number of objects (and allow user to shoot additional boxes)
|
||||
@@ -97,10 +102,19 @@ void Box2dDemo::initPhysics()
|
||||
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
|
||||
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
|
||||
|
||||
btVoronoiSimplexSolver* simplex = new btVoronoiSimplexSolver();
|
||||
btMinkowskiPenetrationDepthSolver* pdSolver = new btMinkowskiPenetrationDepthSolver();
|
||||
|
||||
|
||||
btConvex2dConvex2dAlgorithm::CreateFunc* convexAlgo2d = new btConvex2dConvex2dAlgorithm::CreateFunc(simplex,pdSolver);
|
||||
|
||||
m_dispatcher->registerCollisionCreateFunc(CUSTOM_CONVEX_SHAPE_TYPE,CUSTOM_CONVEX_SHAPE_TYPE,new btBox2dBox2dCollisionAlgorithm::CreateFunc);
|
||||
m_dispatcher->registerCollisionCreateFunc(CONVEX_2D_SHAPE_PROXYTYPE,CONVEX_2D_SHAPE_PROXYTYPE,convexAlgo2d);
|
||||
m_dispatcher->registerCollisionCreateFunc(BOX_2D_SHAPE_PROXYTYPE,CONVEX_2D_SHAPE_PROXYTYPE,convexAlgo2d);
|
||||
m_dispatcher->registerCollisionCreateFunc(CONVEX_2D_SHAPE_PROXYTYPE,BOX_2D_SHAPE_PROXYTYPE,convexAlgo2d);
|
||||
m_dispatcher->registerCollisionCreateFunc(BOX_2D_SHAPE_PROXYTYPE,BOX_2D_SHAPE_PROXYTYPE,new btBox2dBox2dCollisionAlgorithm::CreateFunc());
|
||||
|
||||
m_broadphase = new btDbvtBroadphase();
|
||||
//m_broadphase = new btSimpleBroadphase();
|
||||
|
||||
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
|
||||
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
|
||||
@@ -149,11 +163,24 @@ void Box2dDemo::initPhysics()
|
||||
//create a few dynamic rigidbodies
|
||||
// Re-using the same collision is better for memory usage and performance
|
||||
|
||||
//btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
|
||||
btCollisionShape* colShape = new btBox2dShape(btVector3(SCALING*1,SCALING*1,0.));
|
||||
colShape->setMargin(0.);
|
||||
btScalar u = 1*SCALING-0.04;
|
||||
btVector3 points[3] = {btVector3(0,u,0),btVector3(-u,-u,0),btVector3(u,-u,0)};
|
||||
btConvexShape* colShape= new btConvex2dShape(new btBoxShape(btVector3(SCALING*1,SCALING*1,0.04)));
|
||||
//btCollisionShape* colShape = new btBox2dShape(btVector3(SCALING*1,SCALING*1,0.04));
|
||||
|
||||
btConvexShape* colShape2= new btConvex2dShape(new btConvexHullShape(&points[0].getX(),3));
|
||||
btConvexShape* colShape3= new btConvex2dShape(new btCylinderShapeZ(btVector3(SCALING*1,SCALING*1,0.04)));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//btUniformScalingShape* colShape = new btUniformScalingShape(convexColShape,1.f);
|
||||
colShape->setMargin(0.03);
|
||||
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
|
||||
m_collisionShapes.push_back(colShape);
|
||||
m_collisionShapes.push_back(colShape2);
|
||||
|
||||
|
||||
/// Create Dynamic Objects
|
||||
btTransform startTransform;
|
||||
@@ -183,12 +210,25 @@ void Box2dDemo::initPhysics()
|
||||
|
||||
for (int j = i; j < ARRAY_SIZE_Y; ++j)
|
||||
{
|
||||
startTransform.setOrigin(y);
|
||||
startTransform.setOrigin(y-btVector3(-10,0,0));
|
||||
|
||||
|
||||
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(0,0,0);
|
||||
switch (j%3)
|
||||
{
|
||||
#if 0
|
||||
case 0:
|
||||
rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape,localInertia);
|
||||
break;
|
||||
case 1:
|
||||
rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape3,localInertia);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape3,localInertia);
|
||||
}
|
||||
btRigidBody* body = new btRigidBody(rbInfo);
|
||||
//body->setContactProcessingThreshold(colShape->getContactBreakingThreshold());
|
||||
body->setActivationState(ISLAND_SLEEPING);
|
||||
@@ -199,7 +239,8 @@ void Box2dDemo::initPhysics()
|
||||
body->setActivationState(ISLAND_SLEEPING);
|
||||
|
||||
|
||||
y += deltaY;
|
||||
y += -0.8*deltaY;
|
||||
//y += deltaY;
|
||||
}
|
||||
|
||||
x += deltaX;
|
||||
|
||||
@@ -46,6 +46,8 @@ IMPLICIT_CONVEX_SHAPES_START_HERE,
|
||||
UNIFORM_SCALING_SHAPE_PROXYTYPE,
|
||||
MINKOWSKI_SUM_SHAPE_PROXYTYPE,
|
||||
MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE,
|
||||
BOX_2D_SHAPE_PROXYTYPE,
|
||||
CONVEX_2D_SHAPE_PROXYTYPE,
|
||||
CUSTOM_CONVEX_SHAPE_TYPE,
|
||||
//concave shapes
|
||||
CONCAVE_SHAPES_START_HERE,
|
||||
@@ -152,6 +154,12 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
{
|
||||
return (proxyType == STATIC_PLANE_PROXYTYPE);
|
||||
}
|
||||
|
||||
static SIMD_FORCE_INLINE bool isConvex2d(int proxyType)
|
||||
{
|
||||
return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
;
|
||||
|
||||
@@ -21,7 +21,7 @@ subject to the following restrictions:
|
||||
#include "BulletCollision/CollisionShapes/btBoxShape.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
|
||||
#include "btBox2dShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btBox2dShape.h"
|
||||
|
||||
#define USE_PERSISTENT_CONTACTS 1
|
||||
|
||||
@@ -51,7 +51,7 @@ btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
|
||||
|
||||
void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
|
||||
|
||||
#include <stdio.h>
|
||||
//#include <stdio.h>
|
||||
void btBox2dBox2dCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
|
||||
{
|
||||
if (!m_manifoldPtr)
|
||||
@@ -0,0 +1,247 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "btConvex2dConvex2dAlgorithm.h"
|
||||
|
||||
//#include <stdio.h>
|
||||
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
|
||||
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
|
||||
#include "BulletCollision/CollisionShapes/btBoxShape.h"
|
||||
#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
|
||||
|
||||
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
|
||||
#include "BulletCollision/CollisionShapes/btSphereShape.h"
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
|
||||
|
||||
|
||||
btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
|
||||
{
|
||||
m_numPerturbationIterations = 0;
|
||||
m_minimumPointsPerturbationThreshold = 3;
|
||||
m_simplexSolver = simplexSolver;
|
||||
m_pdSolver = pdSolver;
|
||||
}
|
||||
|
||||
btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
|
||||
{
|
||||
}
|
||||
|
||||
btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
|
||||
: btActivatingCollisionAlgorithm(ci,body0,body1),
|
||||
m_simplexSolver(simplexSolver),
|
||||
m_pdSolver(pdSolver),
|
||||
m_ownManifold (false),
|
||||
m_manifoldPtr(mf),
|
||||
m_lowLevelOfDetail(false),
|
||||
m_numPerturbationIterations(numPerturbationIterations),
|
||||
m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
|
||||
{
|
||||
(void)body0;
|
||||
(void)body1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
|
||||
{
|
||||
if (m_ownManifold)
|
||||
{
|
||||
if (m_manifoldPtr)
|
||||
m_dispatcher->releaseManifold(m_manifoldPtr);
|
||||
}
|
||||
}
|
||||
|
||||
void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
|
||||
{
|
||||
m_lowLevelOfDetail = useLowLevel;
|
||||
}
|
||||
|
||||
|
||||
|
||||
extern btScalar gContactBreakingThreshold;
|
||||
|
||||
|
||||
//
|
||||
// Convex-Convex collision algorithm
|
||||
//
|
||||
void btConvex2dConvex2dAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
|
||||
{
|
||||
|
||||
if (!m_manifoldPtr)
|
||||
{
|
||||
//swapped?
|
||||
m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
|
||||
m_ownManifold = true;
|
||||
}
|
||||
resultOut->setPersistentManifold(m_manifoldPtr);
|
||||
|
||||
//comment-out next line to test multi-contact generation
|
||||
//resultOut->getPersistentManifold()->clearManifold();
|
||||
|
||||
|
||||
btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
|
||||
btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
|
||||
|
||||
btVector3 normalOnB;
|
||||
btVector3 pointOnBWorld;
|
||||
|
||||
{
|
||||
|
||||
|
||||
btGjkPairDetector::ClosestPointInput input;
|
||||
|
||||
btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
|
||||
//TODO: if (dispatchInfo.m_useContinuous)
|
||||
gjkPairDetector.setMinkowskiA(min0);
|
||||
gjkPairDetector.setMinkowskiB(min1);
|
||||
|
||||
{
|
||||
input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
|
||||
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
|
||||
}
|
||||
|
||||
input.m_stackAlloc = dispatchInfo.m_stackAllocator;
|
||||
input.m_transformA = body0->getWorldTransform();
|
||||
input.m_transformB = body1->getWorldTransform();
|
||||
|
||||
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
|
||||
|
||||
btVector3 v0,v1;
|
||||
btVector3 sepNormalWorldSpace;
|
||||
|
||||
}
|
||||
|
||||
if (m_ownManifold)
|
||||
{
|
||||
resultOut->refreshContactPoints();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
|
||||
{
|
||||
(void)resultOut;
|
||||
(void)dispatchInfo;
|
||||
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
|
||||
|
||||
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
|
||||
///col0->m_worldTransform,
|
||||
btScalar resultFraction = btScalar(1.);
|
||||
|
||||
|
||||
btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
|
||||
btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
|
||||
|
||||
if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
|
||||
squareMot1 < col1->getCcdSquareMotionThreshold())
|
||||
return resultFraction;
|
||||
|
||||
|
||||
//An adhoc way of testing the Continuous Collision Detection algorithms
|
||||
//One object is approximated as a sphere, to simplify things
|
||||
//Starting in penetration should report no time of impact
|
||||
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
|
||||
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
|
||||
|
||||
|
||||
/// Convex0 against sphere for Convex1
|
||||
{
|
||||
btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
|
||||
|
||||
btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
|
||||
btConvexCast::CastResult result;
|
||||
btVoronoiSimplexSolver voronoiSimplex;
|
||||
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
|
||||
///Simplification, one object is simplified as a sphere
|
||||
btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
|
||||
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
|
||||
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
|
||||
col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
|
||||
{
|
||||
|
||||
//store result.m_fraction in both bodies
|
||||
|
||||
if (col0->getHitFraction()> result.m_fraction)
|
||||
col0->setHitFraction( result.m_fraction );
|
||||
|
||||
if (col1->getHitFraction() > result.m_fraction)
|
||||
col1->setHitFraction( result.m_fraction);
|
||||
|
||||
if (resultFraction > result.m_fraction)
|
||||
resultFraction = result.m_fraction;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/// Sphere (for convex0) against Convex1
|
||||
{
|
||||
btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
|
||||
|
||||
btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
|
||||
btConvexCast::CastResult result;
|
||||
btVoronoiSimplexSolver voronoiSimplex;
|
||||
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
|
||||
///Simplification, one object is simplified as a sphere
|
||||
btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
|
||||
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
|
||||
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
|
||||
col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
|
||||
{
|
||||
|
||||
//store result.m_fraction in both bodies
|
||||
|
||||
if (col0->getHitFraction() > result.m_fraction)
|
||||
col0->setHitFraction( result.m_fraction);
|
||||
|
||||
if (col1->getHitFraction() > result.m_fraction)
|
||||
col1->setHitFraction( result.m_fraction);
|
||||
|
||||
if (resultFraction > result.m_fraction)
|
||||
resultFraction = result.m_fraction;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return resultFraction;
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,95 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef CONVEX_2D_CONVEX_2D_ALGORITHM_H
|
||||
#define CONVEX_2D_CONVEX_2D_ALGORITHM_H
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
|
||||
#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
|
||||
|
||||
class btConvexPenetrationDepthSolver;
|
||||
|
||||
|
||||
///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape
|
||||
///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation
|
||||
class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm
|
||||
{
|
||||
btSimplexSolverInterface* m_simplexSolver;
|
||||
btConvexPenetrationDepthSolver* m_pdSolver;
|
||||
|
||||
|
||||
bool m_ownManifold;
|
||||
btPersistentManifold* m_manifoldPtr;
|
||||
bool m_lowLevelOfDetail;
|
||||
|
||||
int m_numPerturbationIterations;
|
||||
int m_minimumPointsPerturbationThreshold;
|
||||
|
||||
public:
|
||||
|
||||
btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
|
||||
|
||||
|
||||
virtual ~btConvex2dConvex2dAlgorithm();
|
||||
|
||||
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
|
||||
|
||||
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
|
||||
|
||||
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
|
||||
{
|
||||
///should we use m_ownManifold to avoid adding duplicates?
|
||||
if (m_manifoldPtr && m_ownManifold)
|
||||
manifoldArray.push_back(m_manifoldPtr);
|
||||
}
|
||||
|
||||
|
||||
void setLowLevelOfDetail(bool useLowLevel);
|
||||
|
||||
|
||||
const btPersistentManifold* getManifold()
|
||||
{
|
||||
return m_manifoldPtr;
|
||||
}
|
||||
|
||||
struct CreateFunc :public btCollisionAlgorithmCreateFunc
|
||||
{
|
||||
|
||||
btConvexPenetrationDepthSolver* m_pdSolver;
|
||||
btSimplexSolverInterface* m_simplexSolver;
|
||||
int m_numPerturbationIterations;
|
||||
int m_minimumPointsPerturbationThreshold;
|
||||
|
||||
CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
|
||||
|
||||
virtual ~CreateFunc();
|
||||
|
||||
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
|
||||
{
|
||||
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm));
|
||||
return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //CONVEX_2D_CONVEX_2D_ALGORITHM_H
|
||||
@@ -97,7 +97,7 @@ public:
|
||||
m_normals[2].setValue(0,1,0);
|
||||
m_normals[3].setValue(-1,0,0);
|
||||
|
||||
m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE;
|
||||
m_shapeType = BOX_2D_SHAPE_PROXYTYPE;
|
||||
btVector3 margin(getMargin(),getMargin(),getMargin());
|
||||
m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
|
||||
};
|
||||
@@ -60,6 +60,11 @@ public:
|
||||
return btBroadphaseProxy::isPolyhedral(getShapeType());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE bool isConvex2d() const
|
||||
{
|
||||
return btBroadphaseProxy::isConvex2d(getShapeType());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE bool isConvex() const
|
||||
{
|
||||
return btBroadphaseProxy::isConvex(getShapeType());
|
||||
|
||||
92
src/BulletCollision/CollisionShapes/btConvex2dShape.cpp
Normal file
92
src/BulletCollision/CollisionShapes/btConvex2dShape.cpp
Normal file
@@ -0,0 +1,92 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "btConvex2dShape.h"
|
||||
|
||||
btConvex2dShape::btConvex2dShape( btConvexShape* convexChildShape):
|
||||
btConvexShape (), m_childConvexShape(convexChildShape)
|
||||
{
|
||||
m_shapeType = CONVEX_2D_SHAPE_PROXYTYPE;
|
||||
}
|
||||
|
||||
btConvex2dShape::~btConvex2dShape()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
|
||||
btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
|
||||
{
|
||||
return m_childConvexShape->localGetSupportingVertexWithoutMargin(vec);
|
||||
}
|
||||
|
||||
void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
|
||||
{
|
||||
m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors);
|
||||
}
|
||||
|
||||
|
||||
btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec)const
|
||||
{
|
||||
return m_childConvexShape->localGetSupportingVertex(vec);
|
||||
}
|
||||
|
||||
|
||||
void btConvex2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
|
||||
{
|
||||
///this linear upscaling is not realistic, but we don't deal with large mass ratios...
|
||||
m_childConvexShape->calculateLocalInertia(mass,inertia);
|
||||
}
|
||||
|
||||
|
||||
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
|
||||
void btConvex2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
|
||||
{
|
||||
m_childConvexShape->getAabb(t,aabbMin,aabbMax);
|
||||
}
|
||||
|
||||
void btConvex2dShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
|
||||
{
|
||||
m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax);
|
||||
}
|
||||
|
||||
void btConvex2dShape::setLocalScaling(const btVector3& scaling)
|
||||
{
|
||||
m_childConvexShape->setLocalScaling(scaling);
|
||||
}
|
||||
|
||||
const btVector3& btConvex2dShape::getLocalScaling() const
|
||||
{
|
||||
return m_childConvexShape->getLocalScaling();
|
||||
}
|
||||
|
||||
void btConvex2dShape::setMargin(btScalar margin)
|
||||
{
|
||||
m_childConvexShape->setMargin(margin);
|
||||
}
|
||||
btScalar btConvex2dShape::getMargin() const
|
||||
{
|
||||
return m_childConvexShape->getMargin();
|
||||
}
|
||||
|
||||
int btConvex2dShape::getNumPreferredPenetrationDirections() const
|
||||
{
|
||||
return m_childConvexShape->getNumPreferredPenetrationDirections();
|
||||
}
|
||||
|
||||
void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
|
||||
{
|
||||
m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector);
|
||||
}
|
||||
80
src/BulletCollision/CollisionShapes/btConvex2dShape.h
Normal file
80
src/BulletCollision/CollisionShapes/btConvex2dShape.h
Normal file
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef BT_CONVEX_2D_SHAPE_H
|
||||
#define BT_CONVEX_2D_SHAPE_H
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btConvexShape.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
|
||||
|
||||
///The btConvex2dShape allows to use arbitrary convex shapes are 2d convex shapes, with the Z component assumed to be 0.
|
||||
///For 2d boxes, the btBox2dShape is recommended.
|
||||
class btConvex2dShape : public btConvexShape
|
||||
{
|
||||
btConvexShape* m_childConvexShape;
|
||||
|
||||
public:
|
||||
|
||||
btConvex2dShape( btConvexShape* convexChildShape);
|
||||
|
||||
virtual ~btConvex2dShape();
|
||||
|
||||
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
|
||||
|
||||
virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
|
||||
|
||||
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
|
||||
|
||||
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
|
||||
|
||||
btConvexShape* getChildShape()
|
||||
{
|
||||
return m_childConvexShape;
|
||||
}
|
||||
|
||||
const btConvexShape* getChildShape() const
|
||||
{
|
||||
return m_childConvexShape;
|
||||
}
|
||||
|
||||
virtual const char* getName()const
|
||||
{
|
||||
return "Convex2dShape";
|
||||
}
|
||||
|
||||
|
||||
|
||||
///////////////////////////
|
||||
|
||||
|
||||
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
|
||||
void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
|
||||
|
||||
virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
|
||||
|
||||
virtual void setLocalScaling(const btVector3& scaling) ;
|
||||
virtual const btVector3& getLocalScaling() const ;
|
||||
|
||||
virtual void setMargin(btScalar margin);
|
||||
virtual btScalar getMargin() const;
|
||||
|
||||
virtual int getNumPreferredPenetrationDirections() const;
|
||||
|
||||
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_CONVEX_2D_SHAPE_H
|
||||
@@ -39,7 +39,8 @@ int gNumGjkChecks = 0;
|
||||
|
||||
|
||||
btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
|
||||
:m_penetrationDepthSolver(penetrationDepthSolver),
|
||||
:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
|
||||
m_penetrationDepthSolver(penetrationDepthSolver),
|
||||
m_simplexSolver(simplexSolver),
|
||||
m_minkowskiA(objectA),
|
||||
m_minkowskiB(objectB),
|
||||
@@ -53,7 +54,7 @@ m_catchDegeneracies(1)
|
||||
{
|
||||
}
|
||||
btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
|
||||
:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)),
|
||||
:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
|
||||
m_penetrationDepthSolver(penetrationDepthSolver),
|
||||
m_simplexSolver(simplexSolver),
|
||||
m_minkowskiA(objectA),
|
||||
@@ -92,6 +93,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
localTransA.getOrigin() -= positionOffset;
|
||||
localTransB.getOrigin() -= positionOffset;
|
||||
|
||||
bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
|
||||
|
||||
btScalar marginA = m_marginA;
|
||||
btScalar marginB = m_marginB;
|
||||
@@ -171,12 +173,19 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
spu_printf("got local supporting vertices\n");
|
||||
#endif
|
||||
|
||||
if (check2d)
|
||||
{
|
||||
pWorld[2] = 0.f;
|
||||
qWorld[2] = 0.f;
|
||||
}
|
||||
|
||||
btVector3 w = pWorld - qWorld;
|
||||
delta = m_cachedSeparatingAxis.dot(w);
|
||||
|
||||
// potential exit, they don't overlap
|
||||
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
|
||||
{
|
||||
m_degenerateSimplex = 10;
|
||||
checkSimplex=true;
|
||||
//checkPenetration = false;
|
||||
break;
|
||||
@@ -198,6 +207,9 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
if (f0 <= btScalar(0.))
|
||||
{
|
||||
m_degenerateSimplex = 2;
|
||||
} else
|
||||
{
|
||||
m_degenerateSimplex = 11;
|
||||
}
|
||||
checkSimplex = true;
|
||||
break;
|
||||
@@ -231,6 +243,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
|
||||
btScalar previousSquaredDistance = squaredDistance;
|
||||
squaredDistance = newCachedSeparatingAxis.length2();
|
||||
#if 0
|
||||
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
|
||||
if (squaredDistance>previousSquaredDistance)
|
||||
{
|
||||
m_degenerateSimplex = 7;
|
||||
@@ -238,6 +252,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
checkSimplex = false;
|
||||
break;
|
||||
}
|
||||
#endif //
|
||||
|
||||
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
||||
|
||||
@@ -248,6 +263,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
{
|
||||
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
checkSimplex = true;
|
||||
m_degenerateSimplex = 12;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -278,6 +295,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
{
|
||||
//do we need this backup_closest here ?
|
||||
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
m_degenerateSimplex = 13;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -286,7 +304,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
{
|
||||
m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
normalInB = pointOnA-pointOnB;
|
||||
btScalar lenSqr = m_cachedSeparatingAxis.length2();
|
||||
btScalar lenSqr =m_cachedSeparatingAxis.length2();
|
||||
|
||||
//valid normal
|
||||
if (lenSqr < 0.0001)
|
||||
{
|
||||
@@ -390,6 +409,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
pointOnA -= m_cachedSeparatingAxis * marginA ;
|
||||
pointOnB += m_cachedSeparatingAxis * marginB ;
|
||||
normalInB = m_cachedSeparatingAxis;
|
||||
normalInB.normalize();
|
||||
isValid = true;
|
||||
m_lastUsedMethod = 6;
|
||||
} else
|
||||
@@ -404,16 +424,19 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
}
|
||||
}
|
||||
|
||||
if (isValid)
|
||||
|
||||
|
||||
if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
|
||||
{
|
||||
#ifdef __SPU__
|
||||
//spu_printf("distance\n");
|
||||
#endif //__CELLOS_LV2__
|
||||
#if 0
|
||||
///some debugging
|
||||
// if (check2d)
|
||||
{
|
||||
printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
|
||||
printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef DEBUG_SPU_COLLISION_DETECTION
|
||||
spu_printf("output 1\n");
|
||||
#endif
|
||||
m_cachedSeparatingAxis = normalInB;
|
||||
m_cachedSeparatingDistance = distance;
|
||||
|
||||
@@ -422,10 +445,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
pointOnB+positionOffset,
|
||||
distance);
|
||||
|
||||
#ifdef DEBUG_SPU_COLLISION_DETECTION
|
||||
spu_printf("output 2\n");
|
||||
#endif
|
||||
//printf("gjk add:%f",distance);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -78,6 +78,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
(void)stackAlloc;
|
||||
(void)v;
|
||||
|
||||
bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
|
||||
|
||||
struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
|
||||
{
|
||||
@@ -132,7 +133,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
|
||||
for (i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
btVector3 norm = sPenetrationDirections[i];
|
||||
seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
|
||||
seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
|
||||
}
|
||||
@@ -173,29 +174,44 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
|
||||
|
||||
|
||||
|
||||
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
|
||||
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
|
||||
|
||||
for (i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
seperatingAxisInA = seperatingAxisInABatch[i];
|
||||
seperatingAxisInB = seperatingAxisInBBatch[i];
|
||||
|
||||
pInA = supportVerticesABatch[i];
|
||||
qInB = supportVerticesBBatch[i];
|
||||
|
||||
pWorld = transA(pInA);
|
||||
qWorld = transB(qInB);
|
||||
w = qWorld - pWorld;
|
||||
btScalar delta = norm.dot(w);
|
||||
//find smallest delta
|
||||
if (delta < minProj)
|
||||
btVector3 norm = sPenetrationDirections[i];
|
||||
if (check2d)
|
||||
{
|
||||
minProj = delta;
|
||||
minNorm = norm;
|
||||
minA = pWorld;
|
||||
minB = qWorld;
|
||||
norm[2] = 0.f;
|
||||
}
|
||||
if (norm.length2()>0.01)
|
||||
{
|
||||
|
||||
seperatingAxisInA = seperatingAxisInABatch[i];
|
||||
seperatingAxisInB = seperatingAxisInBBatch[i];
|
||||
|
||||
pInA = supportVerticesABatch[i];
|
||||
qInB = supportVerticesBBatch[i];
|
||||
|
||||
pWorld = transA(pInA);
|
||||
qWorld = transB(qInB);
|
||||
if (check2d)
|
||||
{
|
||||
pWorld[2] = 0.f;
|
||||
qWorld[2] = 0.f;
|
||||
}
|
||||
|
||||
w = qWorld - pWorld;
|
||||
btScalar delta = norm.dot(w);
|
||||
//find smallest delta
|
||||
if (delta < minProj)
|
||||
{
|
||||
minProj = delta;
|
||||
minNorm = norm;
|
||||
minA = pWorld;
|
||||
minB = qWorld;
|
||||
}
|
||||
}
|
||||
}
|
||||
#else
|
||||
@@ -264,7 +280,8 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
if (minProj < btScalar(0.))
|
||||
return false;
|
||||
|
||||
minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
|
||||
btScalar extraSeparation = 0.5f;///scale dependent
|
||||
minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
|
||||
|
||||
|
||||
|
||||
@@ -305,6 +322,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
|
||||
|
||||
btIntermediateResult res;
|
||||
gjkdet.setCachedSeperatingAxis(-minNorm);
|
||||
gjkdet.getClosestPoints(input,res,debugDraw);
|
||||
|
||||
btScalar correctedMinNorm = minProj - res.m_depth;
|
||||
@@ -313,12 +331,14 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
//the penetration depth is over-estimated, relax it
|
||||
btScalar penetration_relaxation= btScalar(1.);
|
||||
minNorm*=penetration_relaxation;
|
||||
|
||||
|
||||
if (res.m_hasResult)
|
||||
{
|
||||
|
||||
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
|
||||
pb = res.m_pointInWorld;
|
||||
v = minNorm;
|
||||
|
||||
#ifdef DEBUG_DRAW
|
||||
if (debugDraw)
|
||||
|
||||
@@ -171,8 +171,8 @@ class btHashMap
|
||||
|
||||
for(i=0;i<curHashtableSize;i++)
|
||||
{
|
||||
const Value& value = m_valueArray[i];
|
||||
const Key& key = m_keyArray[i];
|
||||
//const Value& value = m_valueArray[i];
|
||||
//const Key& key = m_keyArray[i];
|
||||
|
||||
int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1); // New hash value with new mask
|
||||
m_next[i] = m_hashTable[hashValue];
|
||||
|
||||
Reference in New Issue
Block a user