forked from bartvdbraak/blender
updating Bullet 2.x with latest changes. The integration + C-API will follow at some stage.
This commit is contained in:
parent
22d97b2e34
commit
3a1b7ece40
@ -44,7 +44,9 @@ CONCAVE_SHAPES_START_HERE,
|
||||
TRIANGLE_MESH_SHAPE_PROXYTYPE,
|
||||
///used for demo integration FAST/Swift collision library and Bullet
|
||||
FAST_CONCAVE_MESH_PROXYTYPE,
|
||||
|
||||
///Used for GIMPACT Trimesh integration
|
||||
GIMPACT_SHAPE_PROXYTYPE,
|
||||
|
||||
EMPTY_SHAPE_PROXYTYPE,
|
||||
STATIC_PLANE_PROXYTYPE,
|
||||
CONCAVE_SHAPES_END_HERE,
|
||||
|
@ -19,7 +19,7 @@ subject to the following restrictions:
|
||||
struct btBroadphaseProxy;
|
||||
class btDispatcher;
|
||||
class btManifoldResult;
|
||||
struct btCollisionObject;
|
||||
class btCollisionObject;
|
||||
struct btDispatcherInfo;
|
||||
class btPersistentManifold;
|
||||
|
||||
|
@ -19,7 +19,7 @@ subject to the following restrictions:
|
||||
class btCollisionAlgorithm;
|
||||
struct btBroadphaseProxy;
|
||||
class btRigidBody;
|
||||
struct btCollisionObject;
|
||||
class btCollisionObject;
|
||||
class btOverlappingPairCache;
|
||||
|
||||
enum btCollisionDispatcherId
|
||||
@ -42,7 +42,8 @@ struct btDispatcherInfo
|
||||
m_timeOfImpact(1.f),
|
||||
m_useContinuous(false),
|
||||
m_debugDraw(0),
|
||||
m_enableSatConvex(false)
|
||||
m_enableSatConvex(false),
|
||||
m_enableSPU(false)
|
||||
{
|
||||
|
||||
}
|
||||
@ -53,6 +54,7 @@ struct btDispatcherInfo
|
||||
bool m_useContinuous;
|
||||
class btIDebugDraw* m_debugDraw;
|
||||
bool m_enableSatConvex;
|
||||
bool m_enableSPU;
|
||||
|
||||
};
|
||||
|
||||
|
@ -101,6 +101,7 @@ protected:
|
||||
virtual bool processOverlap(btBroadphasePair& pair)
|
||||
{
|
||||
assert(0);
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -42,6 +42,7 @@ ADD_LIBRARY(LibBulletCollision
|
||||
CollisionShapes/btStaticPlaneShape.cpp
|
||||
CollisionShapes/btStridingMeshInterface.cpp
|
||||
CollisionShapes/btTriangleCallback.cpp
|
||||
CollisionShapes/btTriangleBuffer.cpp
|
||||
CollisionShapes/btTriangleIndexVertexArray.cpp
|
||||
CollisionShapes/btTriangleMesh.cpp
|
||||
CollisionShapes/btTriangleMeshShape.cpp
|
||||
|
@ -18,9 +18,9 @@ subject to the following restrictions:
|
||||
|
||||
#include <vector>
|
||||
|
||||
typedef std::vector<struct btCollisionObject*> btCollisionObjectArray;
|
||||
typedef std::vector<class btCollisionObject*> btCollisionObjectArray;
|
||||
class btCollisionAlgorithm;
|
||||
struct btCollisionObject;
|
||||
class btCollisionObject;
|
||||
|
||||
struct btCollisionAlgorithmConstructionInfo;
|
||||
|
||||
|
@ -14,6 +14,7 @@ subject to the following restrictions:
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "btCollisionDispatcher.h"
|
||||
|
||||
|
||||
@ -34,8 +35,8 @@ int gNumManifold = 0;
|
||||
|
||||
btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms)
|
||||
:m_useIslands(true),
|
||||
m_count(0),
|
||||
m_convexConvexCreateFunc(0),
|
||||
m_count(0),
|
||||
m_convexConcaveCreateFunc(0),
|
||||
m_swappedConvexConcaveCreateFunc(0),
|
||||
m_compoundCreateFunc(0),
|
||||
@ -54,7 +55,7 @@ m_emptyCreateFunc(0)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
btCollisionDispatcher::btCollisionDispatcher ():
|
||||
m_useIslands(true),
|
||||
m_count(0)
|
||||
@ -150,7 +151,7 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo
|
||||
btCollisionAlgorithmConstructionInfo ci;
|
||||
ci.m_dispatcher = this;
|
||||
ci.m_manifold = sharedManifold;
|
||||
btCollisionAlgorithm* algo = m_doubleDispatch[body0->m_collisionShape->getShapeType()][body1->m_collisionShape->getShapeType()]
|
||||
btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]
|
||||
->CreateCollisionAlgorithm(ci,body0,body1);
|
||||
#else
|
||||
btCollisionAlgorithm* algo = internalFindAlgorithm(body0,body1);
|
||||
@ -201,27 +202,27 @@ btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionOb
|
||||
btCollisionAlgorithmConstructionInfo ci;
|
||||
ci.m_dispatcher = this;
|
||||
|
||||
if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConvex() )
|
||||
if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConvex() )
|
||||
{
|
||||
return new btConvexConvexAlgorithm(sharedManifold,ci,body0,body1);
|
||||
}
|
||||
|
||||
if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConcave())
|
||||
if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConcave())
|
||||
{
|
||||
return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
|
||||
}
|
||||
|
||||
if (body1->m_collisionShape->isConvex() && body0->m_collisionShape->isConcave())
|
||||
if (body1->getCollisionShape()->isConvex() && body0->getCollisionShape()->isConcave())
|
||||
{
|
||||
return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
|
||||
}
|
||||
|
||||
if (body0->m_collisionShape->isCompound())
|
||||
if (body0->getCollisionShape()->isCompound())
|
||||
{
|
||||
return new btCompoundCollisionAlgorithm(ci,body0,body1,false);
|
||||
} else
|
||||
{
|
||||
if (body1->m_collisionShape->isCompound())
|
||||
if (body1->getCollisionShape()->isCompound())
|
||||
{
|
||||
return new btCompoundCollisionAlgorithm(ci,body0,body1,true);
|
||||
}
|
||||
@ -257,7 +258,7 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO
|
||||
printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
|
||||
}
|
||||
|
||||
if ((!body0->IsActive()) && (!body1->IsActive()))
|
||||
if ((!body0->isActive()) && (!body1->isActive()))
|
||||
needsCollision = false;
|
||||
|
||||
return needsCollision ;
|
||||
|
@ -30,22 +30,22 @@ btCollisionObject::btCollisionObject()
|
||||
}
|
||||
|
||||
|
||||
void btCollisionObject::SetActivationState(int newState)
|
||||
void btCollisionObject::setActivationState(int newState)
|
||||
{
|
||||
if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
|
||||
m_activationState1 = newState;
|
||||
}
|
||||
|
||||
void btCollisionObject::ForceActivationState(int newState)
|
||||
void btCollisionObject::forceActivationState(int newState)
|
||||
{
|
||||
m_activationState1 = newState;
|
||||
}
|
||||
|
||||
void btCollisionObject::activate()
|
||||
{
|
||||
if (!(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OJBECT)))
|
||||
if (!(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT)))
|
||||
{
|
||||
SetActivationState(ACTIVE_TAG);
|
||||
setActivationState(ACTIVE_TAG);
|
||||
m_deactivationTime = 0.f;
|
||||
}
|
||||
}
|
||||
|
@ -34,8 +34,11 @@ class btCollisionShape;
|
||||
/// btCollisionObject can be used to manage collision detection objects.
|
||||
/// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
|
||||
/// They can be added to the btCollisionWorld.
|
||||
struct btCollisionObject
|
||||
class btCollisionObject
|
||||
{
|
||||
|
||||
protected:
|
||||
|
||||
btTransform m_worldTransform;
|
||||
btBroadphaseProxy* m_broadphaseHandle;
|
||||
btCollisionShape* m_collisionShape;
|
||||
@ -48,15 +51,6 @@ struct btCollisionObject
|
||||
btVector3 m_interpolationLinearVelocity;
|
||||
btVector3 m_interpolationAngularVelocity;
|
||||
|
||||
|
||||
enum CollisionFlags
|
||||
{
|
||||
CF_STATIC_OBJECT= 1,
|
||||
CF_KINEMATIC_OJBECT= 2,
|
||||
CF_NO_CONTACT_RESPONSE = 4,
|
||||
CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
|
||||
};
|
||||
|
||||
int m_collisionFlags;
|
||||
|
||||
int m_islandTag1;
|
||||
@ -66,7 +60,7 @@ struct btCollisionObject
|
||||
btScalar m_friction;
|
||||
btScalar m_restitution;
|
||||
|
||||
///users can point to their objects, m_userPointer is not used by Bullet
|
||||
///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
|
||||
void* m_userObjectPointer;
|
||||
|
||||
///m_internalOwner is reserved to point to Bullet's btRigidBody. Don't use this, use m_userObjectPointer instead.
|
||||
@ -81,10 +75,21 @@ struct btCollisionObject
|
||||
/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
|
||||
float m_ccdSquareMotionThreshold;
|
||||
|
||||
public:
|
||||
|
||||
enum CollisionFlags
|
||||
{
|
||||
CF_STATIC_OBJECT= 1,
|
||||
CF_KINEMATIC_OBJECT= 2,
|
||||
CF_NO_CONTACT_RESPONSE = 4,
|
||||
CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
|
||||
};
|
||||
|
||||
|
||||
inline bool mergesSimulationIslands() const
|
||||
{
|
||||
///static objects, kinematic and object without contact response don't merge islands
|
||||
return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OJBECT | CF_NO_CONTACT_RESPONSE) )==0);
|
||||
return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
|
||||
}
|
||||
|
||||
|
||||
@ -94,12 +99,12 @@ struct btCollisionObject
|
||||
|
||||
inline bool isKinematicObject() const
|
||||
{
|
||||
return (m_collisionFlags & CF_KINEMATIC_OJBECT) != 0;
|
||||
return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
|
||||
}
|
||||
|
||||
inline bool isStaticOrKinematicObject() const
|
||||
{
|
||||
return (m_collisionFlags & (CF_KINEMATIC_OJBECT | CF_STATIC_OBJECT)) != 0 ;
|
||||
return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
|
||||
}
|
||||
|
||||
inline bool hasContactResponse() const {
|
||||
@ -107,27 +112,47 @@ struct btCollisionObject
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
btCollisionObject();
|
||||
|
||||
|
||||
void SetCollisionShape(btCollisionShape* collisionShape)
|
||||
void setCollisionShape(btCollisionShape* collisionShape)
|
||||
{
|
||||
m_collisionShape = collisionShape;
|
||||
}
|
||||
|
||||
int GetActivationState() const { return m_activationState1;}
|
||||
|
||||
void SetActivationState(int newState);
|
||||
const btCollisionShape* getCollisionShape() const
|
||||
{
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
void ForceActivationState(int newState);
|
||||
btCollisionShape* getCollisionShape()
|
||||
{
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int getActivationState() const { return m_activationState1;}
|
||||
|
||||
void setActivationState(int newState);
|
||||
|
||||
void setDeactivationTime(float time)
|
||||
{
|
||||
m_deactivationTime = time;
|
||||
}
|
||||
float getDeactivationTime() const
|
||||
{
|
||||
return m_deactivationTime;
|
||||
}
|
||||
|
||||
void forceActivationState(int newState);
|
||||
|
||||
void activate();
|
||||
|
||||
inline bool IsActive() const
|
||||
inline bool isActive() const
|
||||
{
|
||||
return ((GetActivationState() != ISLAND_SLEEPING) && (GetActivationState() != DISABLE_SIMULATION));
|
||||
return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
|
||||
}
|
||||
|
||||
void setRestitution(float rest)
|
||||
@ -147,6 +172,141 @@ struct btCollisionObject
|
||||
return m_friction;
|
||||
}
|
||||
|
||||
///reserved for Bullet internal usage
|
||||
void* getInternalOwner()
|
||||
{
|
||||
return m_internalOwner;
|
||||
}
|
||||
|
||||
const void* getInternalOwner() const
|
||||
{
|
||||
return m_internalOwner;
|
||||
}
|
||||
|
||||
btTransform& getWorldTransform()
|
||||
{
|
||||
return m_worldTransform;
|
||||
}
|
||||
|
||||
const btTransform& getWorldTransform() const
|
||||
{
|
||||
return m_worldTransform;
|
||||
}
|
||||
|
||||
void setWorldTransform(const btTransform& worldTrans)
|
||||
{
|
||||
m_worldTransform = worldTrans;
|
||||
}
|
||||
|
||||
|
||||
btBroadphaseProxy* getBroadphaseHandle()
|
||||
{
|
||||
return m_broadphaseHandle;
|
||||
}
|
||||
|
||||
const btBroadphaseProxy* getBroadphaseHandle() const
|
||||
{
|
||||
return m_broadphaseHandle;
|
||||
}
|
||||
|
||||
void setBroadphaseHandle(btBroadphaseProxy* handle)
|
||||
{
|
||||
m_broadphaseHandle = handle;
|
||||
}
|
||||
|
||||
|
||||
const btTransform& getInterpolationWorldTransform() const
|
||||
{
|
||||
return m_interpolationWorldTransform;
|
||||
}
|
||||
|
||||
btTransform& getInterpolationWorldTransform()
|
||||
{
|
||||
return m_interpolationWorldTransform;
|
||||
}
|
||||
|
||||
void setInterpolationWorldTransform(const btTransform& trans)
|
||||
{
|
||||
m_interpolationWorldTransform = trans;
|
||||
}
|
||||
|
||||
|
||||
const btVector3& getInterpolationLinearVelocity() const
|
||||
{
|
||||
return m_interpolationLinearVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getInterpolationAngularVelocity() const
|
||||
{
|
||||
return m_interpolationAngularVelocity;
|
||||
}
|
||||
|
||||
const int getIslandTag() const
|
||||
{
|
||||
return m_islandTag1;
|
||||
}
|
||||
|
||||
void setIslandTag(int tag)
|
||||
{
|
||||
m_islandTag1 = tag;
|
||||
}
|
||||
|
||||
const float getHitFraction() const
|
||||
{
|
||||
return m_hitFraction;
|
||||
}
|
||||
|
||||
void setHitFraction(float hitFraction)
|
||||
{
|
||||
m_hitFraction = hitFraction;
|
||||
}
|
||||
|
||||
|
||||
const int getCollisionFlags() const
|
||||
{
|
||||
return m_collisionFlags;
|
||||
}
|
||||
|
||||
void setCollisionFlags(int flags)
|
||||
{
|
||||
m_collisionFlags = flags;
|
||||
}
|
||||
|
||||
///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
|
||||
float getCcdSweptSphereRadius() const
|
||||
{
|
||||
return m_ccdSweptSphereRadius;
|
||||
}
|
||||
|
||||
///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
|
||||
void setCcdSweptSphereRadius(float radius)
|
||||
{
|
||||
m_ccdSweptSphereRadius = radius;
|
||||
}
|
||||
|
||||
float getCcdSquareMotionThreshold() const
|
||||
{
|
||||
return m_ccdSquareMotionThreshold;
|
||||
}
|
||||
|
||||
|
||||
/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
|
||||
void setCcdSquareMotionThreshold(float ccdSquareMotionThreshold)
|
||||
{
|
||||
m_ccdSquareMotionThreshold = ccdSquareMotionThreshold;
|
||||
}
|
||||
|
||||
///users can point to their objects, userPointer is not used by Bullet
|
||||
void* getUserPointer() const
|
||||
{
|
||||
return m_userObjectPointer;
|
||||
}
|
||||
|
||||
///users can point to their objects, userPointer is not used by Bullet
|
||||
void setUserPointer(void* userPointer)
|
||||
{
|
||||
m_userObjectPointer = userPointer;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
@ -40,6 +40,7 @@ m_ownsBroadphasePairCache(false)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
btCollisionWorld::btCollisionWorld()
|
||||
: m_dispatcher1(new btCollisionDispatcher()),
|
||||
m_broadphasePairCache(new btSimpleBroadphase()),
|
||||
@ -60,7 +61,7 @@ btCollisionWorld::~btCollisionWorld()
|
||||
{
|
||||
btCollisionObject* collisionObject= (*i);
|
||||
|
||||
btBroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
|
||||
btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
|
||||
if (bp)
|
||||
{
|
||||
//
|
||||
@ -98,21 +99,22 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
|
||||
m_collisionObjects.push_back(collisionObject);
|
||||
|
||||
//calculate new AABB
|
||||
btTransform trans = collisionObject->m_worldTransform;
|
||||
btTransform trans = collisionObject->getWorldTransform();
|
||||
|
||||
btVector3 minAabb;
|
||||
btVector3 maxAabb;
|
||||
collisionObject->m_collisionShape->getAabb(trans,minAabb,maxAabb);
|
||||
collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
|
||||
|
||||
int type = collisionObject->m_collisionShape->getShapeType();
|
||||
collisionObject->m_broadphaseHandle = getBroadphase()->createProxy(
|
||||
int type = collisionObject->getCollisionShape()->getShapeType();
|
||||
collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
|
||||
minAabb,
|
||||
maxAabb,
|
||||
type,
|
||||
collisionObject,
|
||||
collisionFilterGroup,
|
||||
collisionFilterMask
|
||||
);
|
||||
)) ;
|
||||
|
||||
|
||||
|
||||
|
||||
@ -132,8 +134,8 @@ void btCollisionWorld::performDiscreteCollisionDetection()
|
||||
btVector3 aabbMin,aabbMax;
|
||||
for (size_t i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
m_collisionObjects[i]->m_collisionShape->getAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax);
|
||||
m_broadphasePairCache->setAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax);
|
||||
m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax);
|
||||
m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax);
|
||||
}
|
||||
|
||||
m_broadphasePairCache->refreshOverlappingPairs();
|
||||
@ -155,7 +157,7 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
|
||||
|
||||
{
|
||||
|
||||
btBroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
|
||||
btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
|
||||
if (bp)
|
||||
{
|
||||
//
|
||||
@ -163,7 +165,7 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
|
||||
//
|
||||
getBroadphase()->cleanProxyFromPairs(bp);
|
||||
getBroadphase()->destroyProxy(bp);
|
||||
collisionObject->m_broadphaseHandle = 0;
|
||||
collisionObject->setBroadphaseHandle(0);
|
||||
}
|
||||
}
|
||||
|
||||
@ -318,15 +320,7 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r
|
||||
|
||||
rayToTrans.setOrigin(rayToWorld);
|
||||
|
||||
//do culling based on aabb (rayFrom/rayTo)
|
||||
btVector3 rayAabbMin = rayFromWorld;
|
||||
btVector3 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.
|
||||
/// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
|
||||
|
||||
std::vector<btCollisionObject*>::iterator iter;
|
||||
|
||||
@ -338,16 +332,16 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r
|
||||
|
||||
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
|
||||
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
|
||||
collisionObject->m_collisionShape->getAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
|
||||
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
|
||||
|
||||
//check aabb overlap
|
||||
|
||||
if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
|
||||
float hitLambda = 1.f; //could use resultCallback.m_closestHitFraction, but needs testing
|
||||
btVector3 hitNormal;
|
||||
if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
|
||||
{
|
||||
rayTestSingle(rayFromTrans,rayToTrans,
|
||||
collisionObject,
|
||||
collisionObject->m_collisionShape,
|
||||
collisionObject->m_worldTransform,
|
||||
collisionObject->getCollisionShape(),
|
||||
collisionObject->getWorldTransform(),
|
||||
resultCallback);
|
||||
|
||||
}
|
||||
|
@ -194,7 +194,7 @@ public:
|
||||
|
||||
m_closestHitFraction = rayResult.m_hitFraction;
|
||||
m_collisionObject = rayResult.m_collisionObject;
|
||||
m_hitNormalWorld = m_collisionObject->m_worldTransform.getBasis()*rayResult.m_hitNormalLocal;
|
||||
m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
|
||||
m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
|
||||
return rayResult.m_hitFraction;
|
||||
}
|
||||
|
@ -23,9 +23,9 @@ btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlg
|
||||
{
|
||||
btCollisionObject* colObj = m_isSwapped? body1 : body0;
|
||||
btCollisionObject* otherObj = m_isSwapped? body0 : body1;
|
||||
assert (colObj->m_collisionShape->isCompound());
|
||||
assert (colObj->getCollisionShape()->isCompound());
|
||||
|
||||
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape);
|
||||
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
|
||||
int numChildren = compoundShape->getNumChildShapes();
|
||||
int i;
|
||||
|
||||
@ -33,10 +33,10 @@ btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlg
|
||||
for (i=0;i<numChildren;i++)
|
||||
{
|
||||
btCollisionShape* childShape = compoundShape->getChildShape(i);
|
||||
btCollisionShape* orgShape = colObj->m_collisionShape;
|
||||
colObj->m_collisionShape = childShape;
|
||||
btCollisionShape* orgShape = colObj->getCollisionShape();
|
||||
colObj->setCollisionShape( childShape );
|
||||
m_childCollisionAlgorithms[i] = ci.m_dispatcher->findAlgorithm(colObj,otherObj);
|
||||
colObj->m_collisionShape =orgShape;
|
||||
colObj->setCollisionShape( orgShape );
|
||||
}
|
||||
}
|
||||
|
||||
@ -56,8 +56,8 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt
|
||||
btCollisionObject* colObj = m_isSwapped? body1 : body0;
|
||||
btCollisionObject* otherObj = m_isSwapped? body0 : body1;
|
||||
|
||||
assert (colObj->m_collisionShape->isCompound());
|
||||
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape);
|
||||
assert (colObj->getCollisionShape()->isCompound());
|
||||
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
|
||||
|
||||
//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
|
||||
//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
|
||||
@ -74,18 +74,18 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt
|
||||
btCollisionShape* childShape = compoundShape->getChildShape(i);
|
||||
|
||||
//backup
|
||||
btTransform orgTrans = colObj->m_worldTransform;
|
||||
btCollisionShape* orgShape = colObj->m_collisionShape;
|
||||
btTransform orgTrans = colObj->getWorldTransform();
|
||||
btCollisionShape* orgShape = colObj->getCollisionShape();
|
||||
|
||||
btTransform childTrans = compoundShape->getChildTransform(i);
|
||||
btTransform newChildWorldTrans = orgTrans*childTrans ;
|
||||
colObj->m_worldTransform = newChildWorldTrans;
|
||||
colObj->setWorldTransform( newChildWorldTrans );
|
||||
//the contactpoint is still projected back using the original inverted worldtrans
|
||||
colObj->m_collisionShape = childShape;
|
||||
colObj->setCollisionShape( childShape );
|
||||
m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut);
|
||||
//revert back
|
||||
colObj->m_collisionShape =orgShape;
|
||||
colObj->m_worldTransform = orgTrans;
|
||||
colObj->setCollisionShape( orgShape);
|
||||
colObj->setWorldTransform( orgTrans );
|
||||
}
|
||||
}
|
||||
|
||||
@ -95,9 +95,9 @@ float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* bod
|
||||
btCollisionObject* colObj = m_isSwapped? body1 : body0;
|
||||
btCollisionObject* otherObj = m_isSwapped? body0 : body1;
|
||||
|
||||
assert (colObj->m_collisionShape->isCompound());
|
||||
assert (colObj->getCollisionShape()->isCompound());
|
||||
|
||||
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape);
|
||||
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
|
||||
|
||||
//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
|
||||
//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
|
||||
@ -116,22 +116,22 @@ float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* bod
|
||||
btCollisionShape* childShape = compoundShape->getChildShape(i);
|
||||
|
||||
//backup
|
||||
btTransform orgTrans = colObj->m_worldTransform;
|
||||
btCollisionShape* orgShape = colObj->m_collisionShape;
|
||||
btTransform orgTrans = colObj->getWorldTransform();
|
||||
btCollisionShape* orgShape = colObj->getCollisionShape();
|
||||
|
||||
btTransform childTrans = compoundShape->getChildTransform(i);
|
||||
btTransform newChildWorldTrans = orgTrans*childTrans ;
|
||||
colObj->m_worldTransform = newChildWorldTrans;
|
||||
colObj->setWorldTransform( newChildWorldTrans );
|
||||
|
||||
colObj->m_collisionShape = childShape;
|
||||
colObj->setCollisionShape( childShape );
|
||||
float frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
|
||||
if (frac<hitFraction)
|
||||
{
|
||||
hitFraction = frac;
|
||||
}
|
||||
//revert back
|
||||
colObj->m_collisionShape =orgShape;
|
||||
colObj->m_worldTransform = orgTrans;
|
||||
colObj->setCollisionShape( orgShape);
|
||||
colObj->setWorldTransform( orgTrans);
|
||||
}
|
||||
return hitFraction;
|
||||
|
||||
|
47
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
vendored
47
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
vendored
@ -89,7 +89,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
|
||||
if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
|
||||
{
|
||||
btVector3 color(255,255,0);
|
||||
btTransform& tr = ob->m_worldTransform;
|
||||
btTransform& tr = ob->getWorldTransform();
|
||||
m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
|
||||
m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
|
||||
m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
|
||||
@ -105,14 +105,14 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
|
||||
|
||||
//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
|
||||
|
||||
if (m_convexBody->m_collisionShape->isConvex())
|
||||
if (m_convexBody->getCollisionShape()->isConvex())
|
||||
{
|
||||
btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
|
||||
tm.setMargin(m_collisionMarginTriangle);
|
||||
|
||||
|
||||
btCollisionShape* tmpShape = ob->m_collisionShape;
|
||||
ob->m_collisionShape = &tm;
|
||||
btCollisionShape* tmpShape = ob->getCollisionShape();
|
||||
ob->setCollisionShape( &tm );
|
||||
|
||||
|
||||
btCollisionAlgorithm* colAlgo = ci.m_dispatcher->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
|
||||
@ -124,7 +124,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
|
||||
// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
|
||||
colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
|
||||
delete colAlgo;
|
||||
ob->m_collisionShape = tmpShape;
|
||||
ob->setCollisionShape( tmpShape );
|
||||
|
||||
}
|
||||
|
||||
@ -142,8 +142,8 @@ void btConvexTriangleCallback::setTimeStepAndCounters(float collisionMarginTrian
|
||||
|
||||
//recalc aabbs
|
||||
btTransform convexInTriangleSpace;
|
||||
convexInTriangleSpace = m_triBody->m_worldTransform.inverse() * m_convexBody->m_worldTransform;
|
||||
btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->m_collisionShape);
|
||||
convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform();
|
||||
btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape());
|
||||
//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
|
||||
convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
|
||||
float extraMargin = collisionMarginTriangle;
|
||||
@ -167,14 +167,14 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* bod
|
||||
btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
|
||||
btCollisionObject* triBody = m_isSwapped ? body0 : body1;
|
||||
|
||||
if (triBody->m_collisionShape->isConcave())
|
||||
if (triBody->getCollisionShape()->isConcave())
|
||||
{
|
||||
|
||||
|
||||
btCollisionObject* triOb = triBody;
|
||||
ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->m_collisionShape);
|
||||
ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->getCollisionShape());
|
||||
|
||||
if (convexBody->m_collisionShape->isConvex())
|
||||
if (convexBody->getCollisionShape()->isConvex())
|
||||
{
|
||||
float collisionMarginTriangle = concaveShape->getMargin();
|
||||
|
||||
@ -207,8 +207,8 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject
|
||||
|
||||
//only perform CCD above a certain threshold, this prevents blocking on the long run
|
||||
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
|
||||
float squareMot0 = (convexbody->m_interpolationWorldTransform.getOrigin() - convexbody->m_worldTransform.getOrigin()).length2();
|
||||
if (squareMot0 < convexbody->m_ccdSquareMotionThreshold)
|
||||
float squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
|
||||
if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
|
||||
{
|
||||
return 1.f;
|
||||
}
|
||||
@ -217,9 +217,9 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject
|
||||
//btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
|
||||
//todo: only do if the motion exceeds the 'radius'
|
||||
|
||||
btTransform triInv = triBody->m_worldTransform.inverse();
|
||||
btTransform convexFromLocal = triInv * convexbody->m_worldTransform;
|
||||
btTransform convexToLocal = triInv * convexbody->m_interpolationWorldTransform;
|
||||
btTransform triInv = triBody->getWorldTransform().inverse();
|
||||
btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
|
||||
btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
|
||||
|
||||
struct LocalTriangleSphereCastCallback : public btTriangleCallback
|
||||
{
|
||||
@ -270,24 +270,25 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject
|
||||
|
||||
|
||||
|
||||
if (triBody->m_collisionShape->isConcave())
|
||||
if (triBody->getCollisionShape()->isConcave())
|
||||
{
|
||||
btVector3 rayAabbMin = convexFromLocal.getOrigin();
|
||||
rayAabbMin.setMin(convexToLocal.getOrigin());
|
||||
btVector3 rayAabbMax = convexFromLocal.getOrigin();
|
||||
rayAabbMax.setMax(convexToLocal.getOrigin());
|
||||
rayAabbMin -= btVector3(convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius);
|
||||
rayAabbMax += btVector3(convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius);
|
||||
float ccdRadius0 = convexbody->getCcdSweptSphereRadius();
|
||||
rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
|
||||
rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
|
||||
|
||||
float curHitFraction = 1.f; //is this available?
|
||||
LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
|
||||
convexbody->m_ccdSweptSphereRadius,curHitFraction);
|
||||
convexbody->getCcdSweptSphereRadius(),curHitFraction);
|
||||
|
||||
raycastCallback.m_hitFraction = convexbody->m_hitFraction;
|
||||
raycastCallback.m_hitFraction = convexbody->getHitFraction();
|
||||
|
||||
btCollisionObject* concavebody = triBody;
|
||||
|
||||
ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->m_collisionShape;
|
||||
ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->getCollisionShape();
|
||||
|
||||
if (triangleMesh)
|
||||
{
|
||||
@ -296,9 +297,9 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject
|
||||
|
||||
|
||||
|
||||
if (raycastCallback.m_hitFraction < convexbody->m_hitFraction)
|
||||
if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
|
||||
{
|
||||
convexbody->m_hitFraction = raycastCallback.m_hitFraction;
|
||||
convexbody->setHitFraction( raycastCallback.m_hitFraction);
|
||||
return raycastCallback.m_hitFraction;
|
||||
}
|
||||
}
|
||||
|
@ -157,8 +157,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
|
||||
|
||||
checkPenetrationDepthSolver();
|
||||
|
||||
btConvexShape* min0 = static_cast<btConvexShape*>(body0->m_collisionShape);
|
||||
btConvexShape* min1 = static_cast<btConvexShape*>(body1->m_collisionShape);
|
||||
btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
|
||||
btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
|
||||
|
||||
btGjkPairDetector::ClosestPointInput input;
|
||||
|
||||
@ -170,8 +170,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
|
||||
|
||||
// input.m_maximumDistanceSquared = 1e30f;
|
||||
|
||||
input.m_transformA = body0->m_worldTransform;
|
||||
input.m_transformB = body1->m_worldTransform;
|
||||
input.m_transformA = body0->getWorldTransform();
|
||||
input.m_transformB = body1->getWorldTransform();
|
||||
|
||||
resultOut->setPersistentManifold(m_manifoldPtr);
|
||||
m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
|
||||
@ -190,14 +190,13 @@ float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC
|
||||
float resultFraction = 1.f;
|
||||
|
||||
|
||||
float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2();
|
||||
float squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
|
||||
float squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
|
||||
|
||||
if (squareMot0 < col0->m_ccdSquareMotionThreshold &&
|
||||
squareMot0 < col0->m_ccdSquareMotionThreshold)
|
||||
if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
|
||||
squareMot1 < col1->getCcdSquareMotionThreshold())
|
||||
return resultFraction;
|
||||
|
||||
|
||||
|
||||
if (disableCcd)
|
||||
return 1.f;
|
||||
|
||||
@ -212,26 +211,26 @@ float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC
|
||||
|
||||
/// Convex0 against sphere for Convex1
|
||||
{
|
||||
btConvexShape* convex0 = static_cast<btConvexShape*>(col0->m_collisionShape);
|
||||
btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
|
||||
|
||||
btSphereShape sphere1(col1->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
|
||||
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->m_worldTransform,col0->m_interpolationWorldTransform,
|
||||
col1->m_worldTransform,col1->m_interpolationWorldTransform,result))
|
||||
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
|
||||
col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
|
||||
{
|
||||
|
||||
//store result.m_fraction in both bodies
|
||||
|
||||
if (col0->m_hitFraction > result.m_fraction)
|
||||
col0->m_hitFraction = result.m_fraction;
|
||||
if (col0->getHitFraction()> result.m_fraction)
|
||||
col0->setHitFraction( result.m_fraction );
|
||||
|
||||
if (col1->m_hitFraction > result.m_fraction)
|
||||
col1->m_hitFraction = result.m_fraction;
|
||||
if (col1->getHitFraction() > result.m_fraction)
|
||||
col1->setHitFraction( result.m_fraction);
|
||||
|
||||
if (resultFraction > result.m_fraction)
|
||||
resultFraction = result.m_fraction;
|
||||
@ -245,26 +244,26 @@ float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC
|
||||
|
||||
/// Sphere (for convex0) against Convex1
|
||||
{
|
||||
btConvexShape* convex1 = static_cast<btConvexShape*>(col1->m_collisionShape);
|
||||
btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
|
||||
|
||||
btSphereShape sphere0(col0->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
|
||||
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->m_worldTransform,col0->m_interpolationWorldTransform,
|
||||
col1->m_worldTransform,col1->m_interpolationWorldTransform,result))
|
||||
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
|
||||
col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
|
||||
{
|
||||
|
||||
//store result.m_fraction in both bodies
|
||||
|
||||
if (col0->m_hitFraction > result.m_fraction)
|
||||
col0->m_hitFraction = result.m_fraction;
|
||||
if (col0->getHitFraction() > result.m_fraction)
|
||||
col0->setHitFraction( result.m_fraction);
|
||||
|
||||
if (col1->m_hitFraction > result.m_fraction)
|
||||
col1->m_hitFraction = result.m_fraction;
|
||||
if (col1->getHitFraction() > result.m_fraction)
|
||||
col1->setHitFraction( result.m_fraction);
|
||||
|
||||
if (resultFraction > result.m_fraction)
|
||||
resultFraction = result.m_fraction;
|
||||
|
@ -48,8 +48,8 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b
|
||||
m_body0(body0),
|
||||
m_body1(body1)
|
||||
{
|
||||
m_rootTransA = body0->m_worldTransform;
|
||||
m_rootTransB = body1->m_worldTransform;
|
||||
m_rootTransA = body0->getWorldTransform();
|
||||
m_rootTransB = body1->getWorldTransform();
|
||||
}
|
||||
|
||||
|
||||
@ -81,8 +81,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
|
||||
//User can override friction and/or restitution
|
||||
if (gContactAddedCallback &&
|
||||
//and if either of the two bodies requires custom material
|
||||
((m_body0->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
|
||||
(m_body1->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
|
||||
((m_body0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
|
||||
(m_body1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
|
||||
{
|
||||
//experimental feature info, for per-triangle material etc.
|
||||
btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
|
||||
|
@ -17,10 +17,12 @@ subject to the following restrictions:
|
||||
#ifndef MANIFOLD_RESULT_H
|
||||
#define MANIFOLD_RESULT_H
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
|
||||
struct btCollisionObject;
|
||||
class btCollisionObject;
|
||||
class btPersistentManifold;
|
||||
class btManifoldPoint;
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
|
||||
|
||||
#include "LinearMath/btTransform.h"
|
||||
|
||||
typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1);
|
||||
|
@ -42,8 +42,8 @@ void btSimulationIslandManager::findUnions(btDispatcher* dispatcher)
|
||||
((colObj1) && ((colObj1)->mergesSimulationIslands())))
|
||||
{
|
||||
|
||||
m_unionFind.unite((colObj0)->m_islandTag1,
|
||||
(colObj1)->m_islandTag1);
|
||||
m_unionFind.unite((colObj0)->getIslandTag(),
|
||||
(colObj1)->getIslandTag());
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -65,8 +65,8 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld
|
||||
{
|
||||
|
||||
btCollisionObject* collisionObject= (*i);
|
||||
collisionObject->m_islandTag1 = index;
|
||||
collisionObject->m_hitFraction = 1.f;
|
||||
collisionObject->setIslandTag(index);
|
||||
collisionObject->setHitFraction(1.f);
|
||||
index++;
|
||||
|
||||
}
|
||||
@ -98,10 +98,10 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col
|
||||
|
||||
if (collisionObject->mergesSimulationIslands())
|
||||
{
|
||||
collisionObject->m_islandTag1 = m_unionFind.find(index);
|
||||
collisionObject->setIslandTag( m_unionFind.find(index) );
|
||||
} else
|
||||
{
|
||||
collisionObject->m_islandTag1 = -1;
|
||||
collisionObject->setIslandTag(-1);
|
||||
}
|
||||
index++;
|
||||
}
|
||||
@ -113,7 +113,7 @@ inline int getIslandId(const btPersistentManifold* lhs)
|
||||
int islandId;
|
||||
const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
|
||||
const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
|
||||
islandId= rcolObj0->m_islandTag1>=0?rcolObj0->m_islandTag1:rcolObj1->m_islandTag1;
|
||||
islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
|
||||
return islandId;
|
||||
|
||||
}
|
||||
@ -158,19 +158,19 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
|
||||
int i = getUnionFind().getElement(idx).m_sz;
|
||||
|
||||
btCollisionObject* colObj0 = collisionObjects[i];
|
||||
if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1))
|
||||
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
|
||||
{
|
||||
printf("error in island management\n");
|
||||
}
|
||||
|
||||
assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1));
|
||||
if (colObj0->m_islandTag1 == islandId)
|
||||
assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
|
||||
if (colObj0->getIslandTag() == islandId)
|
||||
{
|
||||
if (colObj0->GetActivationState()== ACTIVE_TAG)
|
||||
if (colObj0->getActivationState()== ACTIVE_TAG)
|
||||
{
|
||||
allSleeping = false;
|
||||
}
|
||||
if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
|
||||
if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
|
||||
{
|
||||
allSleeping = false;
|
||||
}
|
||||
@ -184,16 +184,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
|
||||
{
|
||||
int i = getUnionFind().getElement(idx).m_sz;
|
||||
btCollisionObject* colObj0 = collisionObjects[i];
|
||||
if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1))
|
||||
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
|
||||
{
|
||||
printf("error in island management\n");
|
||||
}
|
||||
|
||||
assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1));
|
||||
assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
|
||||
|
||||
if (colObj0->m_islandTag1 == islandId)
|
||||
if (colObj0->getIslandTag() == islandId)
|
||||
{
|
||||
colObj0->SetActivationState( ISLAND_SLEEPING );
|
||||
colObj0->setActivationState( ISLAND_SLEEPING );
|
||||
}
|
||||
}
|
||||
} else
|
||||
@ -205,18 +205,18 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
|
||||
int i = getUnionFind().getElement(idx).m_sz;
|
||||
|
||||
btCollisionObject* colObj0 = collisionObjects[i];
|
||||
if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1))
|
||||
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
|
||||
{
|
||||
printf("error in island management\n");
|
||||
}
|
||||
|
||||
assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1));
|
||||
assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
|
||||
|
||||
if (colObj0->m_islandTag1 == islandId)
|
||||
if (colObj0->getIslandTag() == islandId)
|
||||
{
|
||||
if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
|
||||
if ( colObj0->getActivationState() == ISLAND_SLEEPING)
|
||||
{
|
||||
colObj0->SetActivationState( WANTS_DEACTIVATION);
|
||||
colObj0->setActivationState( WANTS_DEACTIVATION);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -236,17 +236,17 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
|
||||
btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
|
||||
|
||||
//todo: check sleeping conditions!
|
||||
if (((colObj0) && colObj0->GetActivationState() != ISLAND_SLEEPING) ||
|
||||
((colObj1) && colObj1->GetActivationState() != ISLAND_SLEEPING))
|
||||
if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
|
||||
((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
|
||||
{
|
||||
//kinematic objects don't merge islands, but wake up all connected objects
|
||||
if (colObj0->isKinematicObject() && colObj0->GetActivationState() != ISLAND_SLEEPING)
|
||||
if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
|
||||
{
|
||||
colObj1->SetActivationState(ACTIVE_TAG);
|
||||
colObj1->setActivationState(ACTIVE_TAG);
|
||||
}
|
||||
if (colObj1->isKinematicObject() && colObj1->GetActivationState() != ISLAND_SLEEPING)
|
||||
if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
|
||||
{
|
||||
colObj0->SetActivationState(ACTIVE_TAG);
|
||||
colObj0->setActivationState(ACTIVE_TAG);
|
||||
}
|
||||
|
||||
//filtering for response
|
||||
|
@ -57,11 +57,11 @@ void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,b
|
||||
btCollisionObject* boxObj = m_isSwapped? body0 : body1;
|
||||
|
||||
|
||||
btSphereShape* sphere0 = (btSphereShape*)sphereObj ->m_collisionShape;
|
||||
btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape();
|
||||
|
||||
btVector3 normalOnSurfaceB;
|
||||
btVector3 pOnBox,pOnSphere;
|
||||
btVector3 sphereCenter = sphereObj->m_worldTransform.getOrigin();
|
||||
btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin();
|
||||
btScalar radius = sphere0->getRadius();
|
||||
|
||||
float dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
|
||||
@ -93,14 +93,14 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box
|
||||
|
||||
btScalar margins;
|
||||
btVector3 bounds[2];
|
||||
btBoxShape* boxShape= (btBoxShape*)boxObj->m_collisionShape;
|
||||
btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
|
||||
|
||||
bounds[0] = -boxShape->getHalfExtents();
|
||||
bounds[1] = boxShape->getHalfExtents();
|
||||
|
||||
margins = boxShape->getMargin();//also add sphereShape margin?
|
||||
|
||||
const btTransform& m44T = boxObj->m_worldTransform;
|
||||
const btTransform& m44T = boxObj->getWorldTransform();
|
||||
|
||||
btVector3 boundsVec[2];
|
||||
btScalar fPenetration;
|
||||
@ -209,7 +209,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject*
|
||||
n[4].setValue( 0.0f, 1.0f, 0.0f );
|
||||
n[5].setValue( 0.0f, 0.0f, 1.0f );
|
||||
|
||||
const btTransform& m44T = boxObj->m_worldTransform;
|
||||
const btTransform& m44T = boxObj->getWorldTransform();
|
||||
|
||||
// convert point in local space
|
||||
prel = m44T.invXform( sphereCenter);
|
||||
|
10
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
vendored
10
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
vendored
@ -44,10 +44,10 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
|
||||
if (!m_manifoldPtr)
|
||||
return;
|
||||
|
||||
btSphereShape* sphere0 = (btSphereShape*)col0->m_collisionShape;
|
||||
btSphereShape* sphere1 = (btSphereShape*)col1->m_collisionShape;
|
||||
btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape();
|
||||
btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape();
|
||||
|
||||
btVector3 diff = col0->m_worldTransform.getOrigin()- col1->m_worldTransform.getOrigin();
|
||||
btVector3 diff = col0->getWorldTransform().getOrigin()- col1->getWorldTransform().getOrigin();
|
||||
float len = diff.length();
|
||||
btScalar radius0 = sphere0->getRadius();
|
||||
btScalar radius1 = sphere1->getRadius();
|
||||
@ -61,9 +61,9 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
|
||||
|
||||
btVector3 normalOnSurfaceB = diff / len;
|
||||
///point on A (worldspace)
|
||||
btVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB;
|
||||
btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
|
||||
///point on B (worldspace)
|
||||
btVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB;
|
||||
btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
|
||||
|
||||
/// report a contact. internally this will be kept persistent, and contact reduction is done
|
||||
resultOut->setPersistentManifold(m_manifoldPtr);
|
||||
|
8
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
vendored
8
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
vendored
@ -48,8 +48,8 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co
|
||||
if (!m_manifoldPtr)
|
||||
return;
|
||||
|
||||
btSphereShape* sphere = (btSphereShape*)col0->m_collisionShape;
|
||||
btTriangleShape* triangle = (btTriangleShape*)col1->m_collisionShape;
|
||||
btSphereShape* sphere = (btSphereShape*)col0->getCollisionShape();
|
||||
btTriangleShape* triangle = (btTriangleShape*)col1->getCollisionShape();
|
||||
|
||||
/// report a contact. internally this will be kept persistent, and contact reduction is done
|
||||
resultOut->setPersistentManifold(m_manifoldPtr);
|
||||
@ -57,8 +57,8 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co
|
||||
|
||||
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
|
||||
input.m_maximumDistanceSquared = 1e30f;//todo: tighter bounds
|
||||
input.m_transformA = col0->m_worldTransform;
|
||||
input.m_transformB = col1->m_worldTransform;
|
||||
input.m_transformA = col0->getWorldTransform();
|
||||
input.m_transformB = col1->getWorldTransform();
|
||||
|
||||
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
|
||||
|
||||
|
@ -255,6 +255,37 @@ public:
|
||||
return "Box";
|
||||
}
|
||||
|
||||
virtual int getNumPreferredPenetrationDirections() const
|
||||
{
|
||||
return 6;
|
||||
}
|
||||
|
||||
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
|
||||
{
|
||||
switch (index)
|
||||
{
|
||||
case 0:
|
||||
penetrationVector.setValue(1.f,0.f,0.f);
|
||||
break;
|
||||
case 1:
|
||||
penetrationVector.setValue(-1.f,0.f,0.f);
|
||||
break;
|
||||
case 2:
|
||||
penetrationVector.setValue(0.f,1.f,0.f);
|
||||
break;
|
||||
case 3:
|
||||
penetrationVector.setValue(0.f,-1.f,0.f);
|
||||
break;
|
||||
case 4:
|
||||
penetrationVector.setValue(0.f,0.f,1.f);
|
||||
break;
|
||||
case 5:
|
||||
penetrationVector.setValue(0.f,0.f,-1.f);
|
||||
break;
|
||||
default:
|
||||
assert(0);
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
@ -26,7 +26,7 @@ subject to the following restrictions:
|
||||
|
||||
//todo: get rid of this btConvexCastResult thing!
|
||||
struct btConvexCastResult;
|
||||
|
||||
#define MAX_PREFERRED_PENETRATION_DIRECTIONS 10
|
||||
|
||||
/// btConvexShape is an abstract shape interface.
|
||||
/// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface.
|
||||
@ -84,6 +84,17 @@ public:
|
||||
return m_collisionMargin;
|
||||
}
|
||||
|
||||
virtual int getNumPreferredPenetrationDirections() const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
|
||||
{
|
||||
assert(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
@ -20,17 +20,17 @@ subject to the following restrictions:
|
||||
btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres)
|
||||
:m_inertiaHalfExtents(inertiaHalfExtents)
|
||||
{
|
||||
m_minRadius = 1e30f;
|
||||
float startMargin = 1e30f;
|
||||
|
||||
m_numSpheres = numSpheres;
|
||||
for (int i=0;i<m_numSpheres;i++)
|
||||
{
|
||||
m_localPositions[i] = positions[i];
|
||||
m_radi[i] = radi[i];
|
||||
if (radi[i] < m_minRadius)
|
||||
m_minRadius = radi[i];
|
||||
if (radi[i] < startMargin)
|
||||
startMargin = radi[i];
|
||||
}
|
||||
setMargin(m_minRadius);
|
||||
setMargin(startMargin);
|
||||
|
||||
}
|
||||
|
||||
@ -64,7 +64,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons
|
||||
|
||||
for (i=0;i<m_numSpheres;i++)
|
||||
{
|
||||
vtx = (*pos) +vec*((*rad)-m_minRadius);
|
||||
vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
|
||||
pos++;
|
||||
rad++;
|
||||
newDot = vec.dot(vtx);
|
||||
@ -96,7 +96,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons
|
||||
|
||||
for (int i=0;i<m_numSpheres;i++)
|
||||
{
|
||||
vtx = (*pos) +vec*((*rad)-m_minRadius);
|
||||
vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
|
||||
pos++;
|
||||
rad++;
|
||||
newDot = vec.dot(vtx);
|
||||
|
@ -31,9 +31,7 @@ class btMultiSphereShape : public btConvexShape
|
||||
btVector3 m_inertiaHalfExtents;
|
||||
|
||||
int m_numSpheres;
|
||||
float m_minRadius;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
41
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
vendored
Normal file
41
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
vendored
Normal file
@ -0,0 +1,41 @@
|
||||
/*
|
||||
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 "btTriangleBuffer.h"
|
||||
|
||||
|
||||
///example usage of this class:
|
||||
// btTriangleBuffer triBuf;
|
||||
// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax);
|
||||
// for (int i=0;i<triBuf.getNumTriangles();i++)
|
||||
// {
|
||||
// const btTriangle& tri = triBuf.getTriangle(i);
|
||||
// //do something useful here with the triangle
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex)
|
||||
{
|
||||
btTriangle tri;
|
||||
tri.m_vertex0 = triangle[0];
|
||||
tri.m_vertex1 = triangle[1];
|
||||
tri.m_vertex2 = triangle[2];
|
||||
tri.m_partId = partId;
|
||||
tri.m_triangleIndex = triangleIndex;
|
||||
|
||||
m_triangleBuffer.push_back(tri);
|
||||
}
|
60
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
vendored
Normal file
60
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
vendored
Normal file
@ -0,0 +1,60 @@
|
||||
/*
|
||||
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 BT_TRIANGLE_BUFFER_H
|
||||
#define BT_TRIANGLE_BUFFER_H
|
||||
|
||||
#include "btTriangleCallback.h"
|
||||
#include <vector>
|
||||
|
||||
struct btTriangle
|
||||
{
|
||||
btVector3 m_vertex0;
|
||||
btVector3 m_vertex1;
|
||||
btVector3 m_vertex2;
|
||||
int m_partId;
|
||||
int m_triangleIndex;
|
||||
};
|
||||
|
||||
///btTriangleBuffer can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles'
|
||||
class btTriangleBuffer : public btTriangleCallback
|
||||
{
|
||||
|
||||
std::vector<btTriangle> m_triangleBuffer;
|
||||
|
||||
public:
|
||||
|
||||
|
||||
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
|
||||
|
||||
int getNumTriangles() const
|
||||
{
|
||||
return int(m_triangleBuffer.size());
|
||||
}
|
||||
|
||||
const btTriangle& getTriangle(int index) const
|
||||
{
|
||||
return m_triangleBuffer[index];
|
||||
}
|
||||
|
||||
void clearBuffer()
|
||||
{
|
||||
m_triangleBuffer.clear();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_TRIANGLE_BUFFER_H
|
@ -46,6 +46,15 @@ class btTriangleMesh : public btStridingMeshInterface
|
||||
m_triangles.push_back(tri);
|
||||
}
|
||||
|
||||
int getNumTriangles() const
|
||||
{
|
||||
return m_triangles.size();
|
||||
}
|
||||
|
||||
const btMyTriangle& getTriangle(int index) const
|
||||
{
|
||||
return m_triangles[index];
|
||||
}
|
||||
|
||||
//StridingMeshInterface interface implementation
|
||||
|
||||
|
@ -157,6 +157,18 @@ public:
|
||||
return "Triangle";
|
||||
}
|
||||
|
||||
virtual int getNumPreferredPenetrationDirections() const
|
||||
{
|
||||
return 2;
|
||||
}
|
||||
|
||||
virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
|
||||
{
|
||||
calcNormal(penetrationVector);
|
||||
if (index)
|
||||
penetrationVector *= -1.f;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
6
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
vendored
6
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
vendored
@ -14,8 +14,8 @@ subject to the following restrictions:
|
||||
*/
|
||||
|
||||
|
||||
#ifndef DISCRETE_COLLISION_DETECTOR_INTERFACE_H
|
||||
#define DISCRETE_COLLISION_DETECTOR_INTERFACE_H
|
||||
#ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
|
||||
#define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
@ -84,4 +84,4 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
|
||||
}
|
||||
};
|
||||
|
||||
#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE_H
|
||||
#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE1_H
|
||||
|
@ -22,12 +22,13 @@ subject to the following restrictions:
|
||||
#include <stdio.h> //for debug printf
|
||||
#endif
|
||||
|
||||
static const btScalar rel_error = btScalar(1.0e-5);
|
||||
btScalar rel_error2 = rel_error * rel_error;
|
||||
float maxdist2 = 1.e30f;
|
||||
//must be above the machine epsilon
|
||||
#define REL_ERROR2 1.0e-6f
|
||||
|
||||
|
||||
#ifdef __SPU__
|
||||
#include <spu_printf.h>
|
||||
#define printf spu_printf
|
||||
#endif //__SPU__
|
||||
|
||||
btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
|
||||
@ -36,7 +37,9 @@ m_penetrationDepthSolver(penetrationDepthSolver),
|
||||
m_simplexSolver(simplexSolver),
|
||||
m_minkowskiA(objectA),
|
||||
m_minkowskiB(objectB),
|
||||
m_ignoreMargin(false)
|
||||
m_ignoreMargin(false),
|
||||
m_lastUsedMethod(-1),
|
||||
m_catchDegeneracies(0)
|
||||
{
|
||||
}
|
||||
|
||||
@ -45,6 +48,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
|
||||
btScalar distance=0.f;
|
||||
btVector3 normalInB(0.f,0.f,0.f);
|
||||
btVector3 pointOnA,pointOnB;
|
||||
btTransform localTransA = input.m_transformA;
|
||||
btTransform localTransB = input.m_transformB;
|
||||
btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * 0.5f;
|
||||
localTransA.getOrigin() -= positionOffset;
|
||||
localTransB.getOrigin() -= positionOffset;
|
||||
|
||||
float marginA = m_minkowskiA->getMargin();
|
||||
float marginB = m_minkowskiB->getMargin();
|
||||
@ -56,12 +64,16 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
|
||||
marginB = 0.f;
|
||||
}
|
||||
|
||||
int curIter = 0;
|
||||
m_curIter = 0;
|
||||
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
|
||||
m_cachedSeparatingAxis.setValue(0,1,0);
|
||||
|
||||
bool isValid = false;
|
||||
bool checkSimplex = false;
|
||||
bool checkPenetration = true;
|
||||
m_degenerateSimplex = false;
|
||||
|
||||
m_lastUsedMethod = -1;
|
||||
|
||||
{
|
||||
btScalar squaredDistance = SIMD_INFINITY;
|
||||
@ -81,8 +93,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
|
||||
|
||||
btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
|
||||
btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
|
||||
btPoint3 pWorld = input.m_transformA(pInA);
|
||||
btPoint3 qWorld = input.m_transformB(qInB);
|
||||
btPoint3 pWorld = localTransA(pInA);
|
||||
btPoint3 qWorld = localTransB(qInB);
|
||||
|
||||
btVector3 w = pWorld - qWorld;
|
||||
delta = m_cachedSeparatingAxis.dot(w);
|
||||
@ -101,7 +113,16 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
|
||||
break;
|
||||
}
|
||||
// are we getting any closer ?
|
||||
if (squaredDistance - delta <= squaredDistance * rel_error2)
|
||||
float f0 = squaredDistance - delta;
|
||||
float f1 = squaredDistance * REL_ERROR2;
|
||||
|
||||
if (f0 <= 0.f)
|
||||
{
|
||||
m_degenerateSimplex = 2;
|
||||
}
|
||||
|
||||
if (f0 >= 0.f && (f0 <= f1))
|
||||
//if (f0 <= f1)
|
||||
{
|
||||
checkSimplex = true;
|
||||
break;
|
||||
@ -112,6 +133,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
|
||||
//calculate the closest point to the origin (update vector v)
|
||||
if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
|
||||
{
|
||||
m_degenerateSimplex = 1;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
@ -130,10 +152,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
|
||||
}
|
||||
|
||||
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
|
||||
if (curIter++ > gGjkMaxIter)
|
||||
if (m_curIter++ > gGjkMaxIter)
|
||||
{
|
||||
#if defined(DEBUG) || defined (_DEBUG)
|
||||
printf("btGjkPairDetector maxIter exceeded:%i\n",curIter);
|
||||
|
||||
printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
|
||||
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
|
||||
m_cachedSeparatingAxis.getX(),
|
||||
m_cachedSeparatingAxis.getY(),
|
||||
@ -141,6 +164,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
|
||||
squaredDistance,
|
||||
m_minkowskiA->getShapeType(),
|
||||
m_minkowskiB->getShapeType());
|
||||
|
||||
#endif
|
||||
break;
|
||||
|
||||
@ -164,20 +188,27 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
|
||||
normalInB = pointOnA-pointOnB;
|
||||
float lenSqr = m_cachedSeparatingAxis.length2();
|
||||
//valid normal
|
||||
if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
|
||||
//if (lenSqr > (0.1f*margin)) //SIMD_EPSILON*SIMD_EPSILON))
|
||||
if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
|
||||
{
|
||||
float rlen = 1.f / btSqrt(lenSqr );
|
||||
normalInB *= rlen; //normalize
|
||||
btScalar s = btSqrt(squaredDistance);
|
||||
|
||||
ASSERT(s > btScalar(0.0));
|
||||
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
|
||||
pointOnB += m_cachedSeparatingAxis * (marginB / s);
|
||||
distance = ((1.f/rlen) - margin);
|
||||
isValid = true;
|
||||
m_lastUsedMethod = 1;
|
||||
} else
|
||||
{
|
||||
m_lastUsedMethod = 2;
|
||||
}
|
||||
}
|
||||
|
||||
if (checkPenetration && !isValid)
|
||||
//if (checkPenetration && !isValid)
|
||||
if (checkPenetration && (!isValid || (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex) ))
|
||||
{
|
||||
//penetration case
|
||||
|
||||
@ -185,27 +216,46 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
|
||||
if (m_penetrationDepthSolver)
|
||||
{
|
||||
// Penetration depth case.
|
||||
isValid = m_penetrationDepthSolver->calcPenDepth(
|
||||
btVector3 tmpPointOnA,tmpPointOnB;
|
||||
bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
|
||||
*m_simplexSolver,
|
||||
m_minkowskiA,m_minkowskiB,
|
||||
input.m_transformA,input.m_transformB,
|
||||
m_cachedSeparatingAxis, pointOnA, pointOnB,
|
||||
localTransA,localTransB,
|
||||
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
|
||||
debugDraw
|
||||
);
|
||||
|
||||
if (isValid)
|
||||
if (isValid2)
|
||||
{
|
||||
normalInB = pointOnB-pointOnA;
|
||||
float lenSqr = normalInB.length2();
|
||||
btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
|
||||
float lenSqr = tmpNormalInB.length2();
|
||||
if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
|
||||
{
|
||||
normalInB /= btSqrt(lenSqr);
|
||||
distance = -(pointOnA-pointOnB).length();
|
||||
tmpNormalInB /= btSqrt(lenSqr);
|
||||
float distance2 = -(tmpPointOnA-tmpPointOnB).length();
|
||||
//only replace valid penetrations when the result is deeper (check)
|
||||
if (!isValid || (distance2 < distance))
|
||||
{
|
||||
distance = distance2;
|
||||
pointOnA = tmpPointOnA;
|
||||
pointOnB = tmpPointOnB;
|
||||
normalInB = tmpNormalInB;
|
||||
isValid = true;
|
||||
m_lastUsedMethod = 3;
|
||||
} else
|
||||
{
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
isValid = false;
|
||||
//isValid = false;
|
||||
m_lastUsedMethod = 4;
|
||||
}
|
||||
} else
|
||||
{
|
||||
m_lastUsedMethod = 5;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -219,7 +269,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
|
||||
|
||||
output.addContactPoint(
|
||||
normalInB,
|
||||
pointOnB,
|
||||
pointOnB+positionOffset,
|
||||
distance);
|
||||
//printf("gjk add:%f",distance);
|
||||
}
|
||||
|
@ -43,6 +43,12 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
|
||||
|
||||
public:
|
||||
|
||||
//some debugging to fix degeneracy problems
|
||||
int m_lastUsedMethod;
|
||||
int m_curIter;
|
||||
int m_degenerateSimplex;
|
||||
int m_catchDegeneracies;
|
||||
|
||||
|
||||
btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
|
||||
virtual ~btGjkPairDetector() {};
|
||||
@ -68,11 +74,13 @@ public:
|
||||
m_penetrationDepthSolver = penetrationDepthSolver;
|
||||
}
|
||||
|
||||
///don't use setIgnoreMargin, it's for Bullet's internal use
|
||||
void setIgnoreMargin(bool ignoreMargin)
|
||||
{
|
||||
m_ignoreMargin = ignoreMargin;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //GJK_PAIR_DETECTOR_H
|
||||
|
153
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
vendored
153
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
vendored
@ -20,32 +20,10 @@ subject to the following restrictions:
|
||||
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
|
||||
|
||||
|
||||
struct MyResult : public btDiscreteCollisionDetectorInterface::Result
|
||||
{
|
||||
|
||||
MyResult():m_hasResult(false)
|
||||
{
|
||||
}
|
||||
|
||||
btVector3 m_normalOnBInWorld;
|
||||
btVector3 m_pointInWorld;
|
||||
float m_depth;
|
||||
bool m_hasResult;
|
||||
|
||||
virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)
|
||||
{
|
||||
}
|
||||
void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
|
||||
{
|
||||
m_normalOnBInWorld = normalOnBInWorld;
|
||||
m_pointInWorld = pointInWorld;
|
||||
m_depth = depth;
|
||||
m_hasResult = true;
|
||||
}
|
||||
};
|
||||
|
||||
#define NUM_UNITSPHERE_POINTS 42
|
||||
static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS] =
|
||||
static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
|
||||
{
|
||||
btVector3(0.000000f , -0.000000f,-1.000000f),
|
||||
btVector3(0.723608f , -0.525725f,-0.447219f),
|
||||
@ -100,6 +78,31 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
)
|
||||
{
|
||||
|
||||
|
||||
struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
|
||||
{
|
||||
|
||||
btIntermediateResult():m_hasResult(false)
|
||||
{
|
||||
}
|
||||
|
||||
btVector3 m_normalOnBInWorld;
|
||||
btVector3 m_pointInWorld;
|
||||
float m_depth;
|
||||
bool m_hasResult;
|
||||
|
||||
virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)
|
||||
{
|
||||
}
|
||||
void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
|
||||
{
|
||||
m_normalOnBInWorld = normalOnBInWorld;
|
||||
m_pointInWorld = pointInWorld;
|
||||
m_depth = depth;
|
||||
m_hasResult = true;
|
||||
}
|
||||
};
|
||||
|
||||
//just take fixed number of orientation, and sample the penetration depth in that direction
|
||||
float minProj = 1e30f;
|
||||
btVector3 minNorm;
|
||||
@ -110,22 +113,62 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
|
||||
#define USE_BATCHED_SUPPORT 1
|
||||
#ifdef USE_BATCHED_SUPPORT
|
||||
btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS];
|
||||
btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS];
|
||||
btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS];
|
||||
btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS];
|
||||
|
||||
btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
|
||||
btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
|
||||
btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
|
||||
btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
|
||||
int i;
|
||||
|
||||
for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
|
||||
int numSampleDirections = NUM_UNITSPHERE_POINTS;
|
||||
|
||||
for (i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
seperatingAxisInABatch[i] = (-norm)* transA.getBasis();
|
||||
seperatingAxisInBBatch[i] = norm * transB.getBasis();
|
||||
seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
|
||||
seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
|
||||
}
|
||||
|
||||
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,NUM_UNITSPHERE_POINTS);
|
||||
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS);
|
||||
for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
|
||||
{
|
||||
int numPDA = convexA->getNumPreferredPenetrationDirections();
|
||||
if (numPDA)
|
||||
{
|
||||
for (int i=0;i<numPDA;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
convexA->getPreferredPenetrationDirection(i,norm);
|
||||
norm = transA.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
|
||||
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int numPDB = convexB->getNumPreferredPenetrationDirections();
|
||||
if (numPDB)
|
||||
{
|
||||
for (int i=0;i<numPDB;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
convexB->getPreferredPenetrationDirection(i,norm);
|
||||
norm = transB.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
|
||||
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
|
||||
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
|
||||
|
||||
for (i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
seperatingAxisInA = seperatingAxisInABatch[i];
|
||||
@ -148,7 +191,40 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
}
|
||||
}
|
||||
#else
|
||||
for (int i=0;i<NUM_UNITSPHERE_POINTS;i++)
|
||||
|
||||
int numSampleDirections = NUM_UNITSPHERE_POINTS;
|
||||
|
||||
{
|
||||
int numPDA = convexA->getNumPreferredPenetrationDirections();
|
||||
if (numPDA)
|
||||
{
|
||||
for (int i=0;i<numPDA;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
convexA->getPreferredPenetrationDirection(i,norm);
|
||||
norm = transA.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int numPDB = convexB->getNumPreferredPenetrationDirections();
|
||||
if (numPDB)
|
||||
{
|
||||
for (int i=0;i<numPDB;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
convexB->getPreferredPenetrationDirection(i,norm);
|
||||
norm = transB.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
seperatingAxisInA = (-norm)* transA.getBasis();
|
||||
@ -174,10 +250,15 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
|
||||
minA += minNorm*convexA->getMargin();
|
||||
minB -= minNorm*convexB->getMargin();
|
||||
//no penetration
|
||||
if (minProj < 0.f)
|
||||
return false;
|
||||
|
||||
minProj += (convexA->getMargin() + convexB->getMargin());
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//#define DEBUG_DRAW 1
|
||||
#ifdef DEBUG_DRAW
|
||||
@ -213,7 +294,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
|
||||
input.m_transformB = transB;
|
||||
input.m_maximumDistanceSquared = 1e30f;//minProj;
|
||||
|
||||
MyResult res;
|
||||
btIntermediateResult res;
|
||||
gjkdet.getClosestPoints(input,res,debugDraw);
|
||||
|
||||
float correctedMinNorm = minProj - res.m_depth;
|
||||
|
@ -164,7 +164,8 @@ float resolveSingleFriction(
|
||||
float combinedFriction = cpd->m_friction;
|
||||
|
||||
btScalar limit = cpd->m_appliedImpulse * combinedFriction;
|
||||
//if (contactPoint.m_appliedImpulse>0.f)
|
||||
|
||||
if (cpd->m_appliedImpulse>0.f)
|
||||
//friction
|
||||
{
|
||||
//apply friction in the 2 tangential directions
|
||||
@ -182,11 +183,12 @@ float resolveSingleFriction(
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j1 = -vrel * cpd->m_jacDiagABInvTangent0;
|
||||
float total = cpd->m_accumulatedTangentImpulse0 + j1;
|
||||
GEN_set_min(total, limit);
|
||||
GEN_set_max(total, -limit);
|
||||
j1 = total - cpd->m_accumulatedTangentImpulse0;
|
||||
cpd->m_accumulatedTangentImpulse0 = total;
|
||||
float oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
|
||||
cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
|
||||
GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit);
|
||||
GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit);
|
||||
j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
|
||||
|
||||
}
|
||||
{
|
||||
// 2nd tangent
|
||||
@ -195,11 +197,11 @@ float resolveSingleFriction(
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j2 = -vrel * cpd->m_jacDiagABInvTangent1;
|
||||
float total = cpd->m_accumulatedTangentImpulse1 + j2;
|
||||
GEN_set_min(total, limit);
|
||||
GEN_set_max(total, -limit);
|
||||
j2 = total - cpd->m_accumulatedTangentImpulse1;
|
||||
cpd->m_accumulatedTangentImpulse1 = total;
|
||||
float oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
|
||||
cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
|
||||
GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit);
|
||||
GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit);
|
||||
j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
|
||||
}
|
||||
|
||||
#ifdef USE_INTERNAL_APPLY_IMPULSE
|
||||
@ -395,4 +397,5 @@ float resolveSingleFrictionEmpty(
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
return 0.f;
|
||||
};
|
||||
};
|
||||
|
||||
|
@ -21,6 +21,7 @@ subject to the following restrictions:
|
||||
static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f };
|
||||
static const int kAxisA[] = { 1, 0, 0 };
|
||||
static const int kAxisB[] = { 2, 2, 1 };
|
||||
#define GENERIC_D6_DISABLE_WARMSTARTING 1
|
||||
|
||||
btGeneric6DofConstraint::btGeneric6DofConstraint()
|
||||
{
|
||||
@ -47,7 +48,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody&
|
||||
|
||||
void btGeneric6DofConstraint::buildJacobian()
|
||||
{
|
||||
btVector3 normal(0,0,0);
|
||||
btVector3 localNormalInA(0,0,0);
|
||||
|
||||
const btVector3& pivotInA = m_frameInA.getOrigin();
|
||||
const btVector3& pivotInB = m_frameInB.getOrigin();
|
||||
@ -64,7 +65,9 @@ void btGeneric6DofConstraint::buildJacobian()
|
||||
{
|
||||
if (isLimited(i))
|
||||
{
|
||||
normal[i] = 1;
|
||||
localNormalInA[i] = 1;
|
||||
btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
|
||||
|
||||
|
||||
// Create linear atom
|
||||
new (&m_jacLinear[i]) btJacobianEntry(
|
||||
@ -72,19 +75,24 @@ void btGeneric6DofConstraint::buildJacobian()
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
|
||||
m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
|
||||
normal,
|
||||
normalWorld,
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
|
||||
//optionally disable warmstarting
|
||||
#ifdef GENERIC_D6_DISABLE_WARMSTARTING
|
||||
m_accumulatedImpulse[i] = 0.f;
|
||||
#endif //GENERIC_D6_DISABLE_WARMSTARTING
|
||||
|
||||
// Apply accumulated impulse
|
||||
btVector3 impulse_vector = m_accumulatedImpulse[i] * normal;
|
||||
btVector3 impulse_vector = m_accumulatedImpulse[i] * normalWorld;
|
||||
|
||||
m_rbA.applyImpulse( impulse_vector, rel_pos1);
|
||||
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
|
||||
|
||||
normal[i] = 0;
|
||||
localNormalInA[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@ -106,6 +114,10 @@ void btGeneric6DofConstraint::buildJacobian()
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvInertiaDiagLocal());
|
||||
|
||||
#ifdef GENERIC_D6_DISABLE_WARMSTARTING
|
||||
m_accumulatedImpulse[i + 3] = 0.f;
|
||||
#endif //GENERIC_D6_DISABLE_WARMSTARTING
|
||||
|
||||
// Apply accumulated impulse
|
||||
btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
|
||||
|
||||
@ -115,6 +127,52 @@ void btGeneric6DofConstraint::buildJacobian()
|
||||
}
|
||||
}
|
||||
|
||||
float getMatrixElem(const btMatrix3x3& mat,int index)
|
||||
{
|
||||
int row = index%3;
|
||||
int col = index / 3;
|
||||
return mat[row][col];
|
||||
}
|
||||
|
||||
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
|
||||
bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
|
||||
{
|
||||
// rot = cy*cz -cy*sz sy
|
||||
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
|
||||
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
|
||||
|
||||
/// 0..8
|
||||
|
||||
if (getMatrixElem(mat,2) < 1.0f)
|
||||
{
|
||||
if (getMatrixElem(mat,2) > -1.0f)
|
||||
{
|
||||
xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8));
|
||||
xyz[1] = btAsin(getMatrixElem(mat,2));
|
||||
xyz[2] = btAtan2(-getMatrixElem(mat,1),getMatrixElem(mat,0));
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
|
||||
xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
|
||||
xyz[1] = -SIMD_HALF_PI;
|
||||
xyz[2] = 0.0f;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
|
||||
xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
|
||||
xyz[1] = SIMD_HALF_PI;
|
||||
xyz[2] = 0.0;
|
||||
return false;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
|
||||
{
|
||||
btScalar tau = 0.1f;
|
||||
@ -126,7 +184,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
btVector3 normal(0,0,0);
|
||||
btVector3 localNormalInA(0,0,0);
|
||||
int i;
|
||||
|
||||
// linear
|
||||
@ -137,8 +195,10 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
|
||||
btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
localNormalInA.setValue(0,0,0);
|
||||
localNormalInA[i] = 1;
|
||||
btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
|
||||
|
||||
normal[i] = 1;
|
||||
btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
|
||||
|
||||
//velocity error (first order error)
|
||||
@ -146,19 +206,59 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal);
|
||||
|
||||
btScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
|
||||
m_accumulatedImpulse[i] += impulse;
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld);
|
||||
btScalar lo = -1e30f;
|
||||
btScalar hi = 1e30f;
|
||||
|
||||
//handle the limits
|
||||
if (m_lowerLimit[i] < m_upperLimit[i])
|
||||
{
|
||||
{
|
||||
if (depth > m_upperLimit[i])
|
||||
{
|
||||
depth -= m_upperLimit[i];
|
||||
lo = 0.f;
|
||||
|
||||
} else
|
||||
{
|
||||
if (depth < m_lowerLimit[i])
|
||||
{
|
||||
depth -= m_lowerLimit[i];
|
||||
hi = 0.f;
|
||||
} else
|
||||
{
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 impulse_vector = normal * impulse;
|
||||
btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
|
||||
float oldNormalImpulse = m_accumulatedImpulse[i];
|
||||
float sum = oldNormalImpulse + normalImpulse;
|
||||
m_accumulatedImpulse[i] = sum > hi ? 0.f : sum < lo ? 0.f : sum;
|
||||
normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse;
|
||||
|
||||
btVector3 impulse_vector = normalWorld * normalImpulse;
|
||||
m_rbA.applyImpulse( impulse_vector, rel_pos1);
|
||||
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
|
||||
|
||||
normal[i] = 0;
|
||||
localNormalInA[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 axis;
|
||||
btScalar angle;
|
||||
btTransform frameAWorld = m_rbA.getCenterOfMassTransform() * m_frameInA;
|
||||
btTransform frameBWorld = m_rbB.getCenterOfMassTransform() * m_frameInB;
|
||||
|
||||
btTransformUtil::calculateDiffAxisAngle(frameAWorld,frameBWorld,axis,angle);
|
||||
btQuaternion diff(axis,angle);
|
||||
btMatrix3x3 diffMat (diff);
|
||||
btVector3 xyz;
|
||||
///this is not perfect, we can first check which axis are limited, and choose a more appropriate order
|
||||
MatrixToEulerXYZ(diffMat,xyz);
|
||||
|
||||
// angular
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
@ -179,13 +279,46 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
|
||||
|
||||
btScalar rel_pos = kSign[i] * axisA.dot(axisB);
|
||||
|
||||
btScalar lo = -1e30f;
|
||||
btScalar hi = 1e30f;
|
||||
|
||||
//handle the twist limit
|
||||
if (m_lowerLimit[i+3] < m_upperLimit[i+3])
|
||||
{
|
||||
//clamp the values
|
||||
btScalar loLimit = m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f;
|
||||
btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f;
|
||||
|
||||
float projAngle = -2.*xyz[i];
|
||||
|
||||
if (projAngle < loLimit)
|
||||
{
|
||||
hi = 0.f;
|
||||
rel_pos = (loLimit - projAngle);
|
||||
} else
|
||||
{
|
||||
if (projAngle > hiLimit)
|
||||
{
|
||||
lo = 0.f;
|
||||
rel_pos = (hiLimit - projAngle);
|
||||
} else
|
||||
{
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//impulse
|
||||
btScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
|
||||
m_accumulatedImpulse[i + 3] += impulse;
|
||||
|
||||
btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
|
||||
float oldNormalImpulse = m_accumulatedImpulse[i+3];
|
||||
float sum = oldNormalImpulse + normalImpulse;
|
||||
m_accumulatedImpulse[i+3] = sum > hi ? 0.f : sum < lo ? 0.f : sum;
|
||||
normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse;
|
||||
|
||||
// Dirk: Not needed - we could actually project onto Jacobian entry here (same as above)
|
||||
btVector3 axis = kSign[i] * axisA.cross(axisB);
|
||||
btVector3 impulse_vector = axis * impulse;
|
||||
btVector3 impulse_vector = axis * normalImpulse;
|
||||
|
||||
m_rbA.applyTorqueImpulse( impulse_vector);
|
||||
m_rbB.applyTorqueImpulse(-impulse_vector);
|
||||
|
131
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
vendored
131
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
vendored
@ -32,6 +32,27 @@ int totalCpd = 0;
|
||||
|
||||
int gTotalContactPoints = 0;
|
||||
|
||||
struct btOrderIndex
|
||||
{
|
||||
short int m_manifoldIndex;
|
||||
short int m_pointIndex;
|
||||
};
|
||||
|
||||
#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
|
||||
static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
|
||||
static unsigned long btSeed2 = 0;
|
||||
unsigned long btRand2()
|
||||
{
|
||||
btSeed2 = (1664525L*btSeed2 + 1013904223L) & 0xffffffff;
|
||||
return btSeed2;
|
||||
}
|
||||
|
||||
int btRandInt2 (int n)
|
||||
{
|
||||
float a = float(n) / 4294967296.0f;
|
||||
return (int) (float(btRand2()) * a);
|
||||
}
|
||||
|
||||
bool MyContactDestroyedCallback(void* userPersistentData)
|
||||
{
|
||||
assert (userPersistentData);
|
||||
@ -69,52 +90,62 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
|
||||
btProfiler::beginBlock("solve");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
int totalPoints = 0;
|
||||
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k=j;
|
||||
prepareConstraints(manifoldPtr[k],info,debugDrawer);
|
||||
solve(manifoldPtr[k],info,0,debugDrawer);
|
||||
prepareConstraints(manifoldPtr[j],info,debugDrawer);
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
|
||||
{
|
||||
gOrder[totalPoints].m_manifoldIndex = j;
|
||||
gOrder[totalPoints].m_pointIndex = p;
|
||||
totalPoints++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//should traverse the contacts random order...
|
||||
int i;
|
||||
for ( i = 0;i<numiter-1;i++)
|
||||
int iteration;
|
||||
for ( iteration = 0;iteration<numiter-1;iteration++)
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k=j;
|
||||
if (i&1)
|
||||
k=numManifolds-j-1;
|
||||
|
||||
solve(manifoldPtr[k],info,i,debugDrawer);
|
||||
if ((iteration & 7) == 0) {
|
||||
for (j=0; j<totalPoints; ++j) {
|
||||
btOrderIndex tmp = gOrder[j];
|
||||
int swapi = btRandInt2(j+1);
|
||||
gOrder[j] = gOrder[swapi];
|
||||
gOrder[swapi] = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
for (j=0;j<totalPoints;j++)
|
||||
{
|
||||
btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
|
||||
solve( (btRigidBody*)manifold->getBody0(),
|
||||
(btRigidBody*)manifold->getBody1()
|
||||
,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
|
||||
}
|
||||
|
||||
for (j=0;j<totalPoints;j++)
|
||||
{
|
||||
btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
|
||||
solveFriction((btRigidBody*)manifold->getBody0(),
|
||||
(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#ifdef USE_PROFILE
|
||||
btProfiler::endBlock("solve");
|
||||
|
||||
btProfiler::beginBlock("solveFriction");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
//now solve the friction
|
||||
for (i = 0;i<numiter-1;i++)
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k = j;
|
||||
if (i&1)
|
||||
k=numManifolds-j-1;
|
||||
solveFriction(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
}
|
||||
#ifdef USE_PROFILE
|
||||
btProfiler::endBlock("solveFriction");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
return 0.f;
|
||||
@ -225,7 +256,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
|
||||
|
||||
btScalar penVel = -cpd->m_penetration/info.m_timeStep;
|
||||
|
||||
if (cpd->m_restitution >= penVel)
|
||||
if (cpd->m_restitution > penVel)
|
||||
{
|
||||
cpd->m_penetration = 0.f;
|
||||
}
|
||||
@ -233,7 +264,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
|
||||
|
||||
|
||||
float relaxation = info.m_damping;
|
||||
cpd->m_appliedImpulse *= relaxation;
|
||||
cpd->m_appliedImpulse =0.f;//*= relaxation;
|
||||
//for friction
|
||||
cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
|
||||
|
||||
@ -260,8 +291,8 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
|
||||
|
||||
btVector3 totalImpulse =
|
||||
#ifndef NO_FRICTION_WARMSTART
|
||||
cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
|
||||
cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
|
||||
cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
|
||||
cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
|
||||
#endif //NO_FRICTION_WARMSTART
|
||||
cp.m_normalWorldOnB*cpd->m_appliedImpulse;
|
||||
|
||||
@ -304,29 +335,16 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
|
||||
}
|
||||
}
|
||||
|
||||
float btSequentialImpulseConstraintSolver::solve(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
|
||||
float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
|
||||
btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
|
||||
|
||||
float maxImpulse = 0.f;
|
||||
|
||||
{
|
||||
const int numpoints = manifoldPtr->getNumContacts();
|
||||
|
||||
btVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
{
|
||||
|
||||
int j=i;
|
||||
if (iter % 2)
|
||||
j = numpoints-1-i;
|
||||
else
|
||||
j=i;
|
||||
|
||||
btManifoldPoint& cp = manifoldPtr->getContactPoint(j);
|
||||
if (cp.getDistance() <= 0.f)
|
||||
if (cp.getDistance() <= 0.f)
|
||||
{
|
||||
|
||||
if (iter == 0)
|
||||
@ -353,24 +371,15 @@ float btSequentialImpulseConstraintSolver::solve(btPersistentManifold* manifoldP
|
||||
return maxImpulse;
|
||||
}
|
||||
|
||||
float btSequentialImpulseConstraintSolver::solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
|
||||
float btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
|
||||
btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
|
||||
|
||||
|
||||
{
|
||||
const int numpoints = manifoldPtr->getNumContacts();
|
||||
|
||||
btVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
{
|
||||
|
||||
int j=i;
|
||||
//if (iter % 2)
|
||||
// j = numpoints-1-i;
|
||||
|
||||
btManifoldPoint& cp = manifoldPtr->getContactPoint(j);
|
||||
|
||||
if (cp.getDistance() <= 0.f)
|
||||
{
|
||||
|
||||
|
@ -29,8 +29,9 @@ class btIDebugDraw;
|
||||
/// Applies impulses for combined restitution and penetration recovery and to simulate friction
|
||||
class btSequentialImpulseConstraintSolver : public btConstraintSolver
|
||||
{
|
||||
float solve(btPersistentManifold* manifold, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
float solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
|
||||
float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
|
||||
|
||||
ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
|
||||
|
@ -17,7 +17,7 @@ subject to the following restrictions:
|
||||
#include "btTypedConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
static btRigidBody s_fixed(0, btTransform::getIdentity(),0);
|
||||
static btRigidBody s_fixed(0, 0,0);
|
||||
|
||||
btTypedConstraint::btTypedConstraint()
|
||||
: m_userConstraintType(-1),
|
||||
|
@ -56,9 +56,10 @@ subject to the following restrictions:
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld()
|
||||
|
||||
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btConstraintSolver* constraintSolver)
|
||||
:btDynamicsWorld(),
|
||||
m_constraintSolver(new btSequentialImpulseConstraintSolver),
|
||||
m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
|
||||
m_debugDrawer(0),
|
||||
m_gravity(0,-10,0),
|
||||
m_localTime(1.f/60.f),
|
||||
@ -66,10 +67,10 @@ m_profileTimings(0)
|
||||
{
|
||||
m_islandManager = new btSimulationIslandManager();
|
||||
m_ownsIslandManager = true;
|
||||
m_ownsConstraintSolver = true;
|
||||
|
||||
m_ownsConstraintSolver = (constraintSolver==0);
|
||||
}
|
||||
|
||||
|
||||
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
|
||||
:btDynamicsWorld(dispatcher,pairCache),
|
||||
m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
|
||||
@ -103,7 +104,7 @@ void btDiscreteDynamicsWorld::saveKinematicState(float timeStep)
|
||||
if (body)
|
||||
{
|
||||
btTransform predictedTrans;
|
||||
if (body->GetActivationState() != ISLAND_SLEEPING)
|
||||
if (body->getActivationState() != ISLAND_SLEEPING)
|
||||
{
|
||||
if (body->isKinematicObject())
|
||||
{
|
||||
@ -117,42 +118,82 @@ void btDiscreteDynamicsWorld::saveKinematicState(float timeStep)
|
||||
|
||||
void btDiscreteDynamicsWorld::synchronizeMotionStates()
|
||||
{
|
||||
//todo: iterate over awake simulation islands!
|
||||
for (unsigned int i=0;i<m_collisionObjects.size();i++)
|
||||
//debug vehicle wheels
|
||||
|
||||
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
|
||||
//todo: iterate over awake simulation islands!
|
||||
for (unsigned int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btVector3 color(255.f,255.f,255.f);
|
||||
switch(colObj->GetActivationState())
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
|
||||
{
|
||||
case ACTIVE_TAG:
|
||||
color = btVector3(255.f,255.f,255.f); break;
|
||||
case ISLAND_SLEEPING:
|
||||
color = btVector3(0.f,255.f,0.f);break;
|
||||
case WANTS_DEACTIVATION:
|
||||
color = btVector3(0.f,255.f,255.f);break;
|
||||
case DISABLE_DEACTIVATION:
|
||||
color = btVector3(255.f,0.f,0.f);break;
|
||||
case DISABLE_SIMULATION:
|
||||
color = btVector3(255.f,255.f,0.f);break;
|
||||
default:
|
||||
btVector3 color(255.f,255.f,255.f);
|
||||
switch(colObj->getActivationState())
|
||||
{
|
||||
color = btVector3(255.f,0.f,0.f);
|
||||
}
|
||||
};
|
||||
case ACTIVE_TAG:
|
||||
color = btVector3(255.f,255.f,255.f); break;
|
||||
case ISLAND_SLEEPING:
|
||||
color = btVector3(0.f,255.f,0.f);break;
|
||||
case WANTS_DEACTIVATION:
|
||||
color = btVector3(0.f,255.f,255.f);break;
|
||||
case DISABLE_DEACTIVATION:
|
||||
color = btVector3(255.f,0.f,0.f);break;
|
||||
case DISABLE_SIMULATION:
|
||||
color = btVector3(255.f,255.f,0.f);break;
|
||||
default:
|
||||
{
|
||||
color = btVector3(255.f,0.f,0.f);
|
||||
}
|
||||
};
|
||||
|
||||
debugDrawObject(colObj->m_worldTransform,colObj->m_collisionShape,color);
|
||||
}
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
|
||||
{
|
||||
if (body->GetActivationState() != ISLAND_SLEEPING)
|
||||
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
|
||||
}
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
|
||||
{
|
||||
btTransform interpolatedTransform;
|
||||
btTransformUtil::integrateTransform(body->m_interpolationWorldTransform,
|
||||
body->m_interpolationLinearVelocity,body->m_interpolationAngularVelocity,m_localTime,interpolatedTransform);
|
||||
body->getMotionState()->setWorldTransform(interpolatedTransform);
|
||||
if (body->getActivationState() != ISLAND_SLEEPING)
|
||||
{
|
||||
btTransform interpolatedTransform;
|
||||
btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
|
||||
body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
|
||||
body->getMotionState()->setWorldTransform(interpolatedTransform);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
|
||||
{
|
||||
for (unsigned int i=0;i<this->m_vehicles.size();i++)
|
||||
{
|
||||
for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
|
||||
{
|
||||
btVector3 wheelColor(0,255,255);
|
||||
if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
|
||||
{
|
||||
wheelColor.setValue(0,0,255);
|
||||
} else
|
||||
{
|
||||
wheelColor.setValue(255,0,255);
|
||||
}
|
||||
|
||||
//synchronize the wheels with the (interpolated) chassis worldtransform
|
||||
m_vehicles[i]->updateWheelTransform(v,true);
|
||||
|
||||
btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
|
||||
|
||||
btVector3 axle = btVector3(
|
||||
m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
|
||||
m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
|
||||
m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
|
||||
|
||||
|
||||
//m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
|
||||
//debug wheels (cylinders)
|
||||
m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
|
||||
m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -178,8 +219,15 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo
|
||||
//variable timestep
|
||||
fixedTimeStep = timeStep;
|
||||
m_localTime = timeStep;
|
||||
numSimulationSubSteps = 1;
|
||||
maxSubSteps = 1;
|
||||
if (btFuzzyZero(timeStep))
|
||||
{
|
||||
numSimulationSubSteps = 0;
|
||||
maxSubSteps = 0;
|
||||
} else
|
||||
{
|
||||
numSimulationSubSteps = 1;
|
||||
maxSubSteps = 1;
|
||||
}
|
||||
}
|
||||
|
||||
//process some debugging flags
|
||||
@ -187,7 +235,7 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo
|
||||
{
|
||||
gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
|
||||
}
|
||||
if (!btFuzzyZero(timeStep) && numSimulationSubSteps)
|
||||
if (numSimulationSubSteps)
|
||||
{
|
||||
|
||||
saveKinematicState(fixedTimeStep);
|
||||
@ -198,6 +246,7 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo
|
||||
for (int i=0;i<clampedSimulationSteps;i++)
|
||||
{
|
||||
internalSingleStepSimulation(fixedTimeStep);
|
||||
synchronizeMotionStates();
|
||||
}
|
||||
|
||||
}
|
||||
@ -226,20 +275,23 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
|
||||
btContactSolverInfo infoGlobal;
|
||||
infoGlobal.m_timeStep = timeStep;
|
||||
|
||||
|
||||
|
||||
///solve non-contact constraints
|
||||
solveNoncontactConstraints(infoGlobal);
|
||||
|
||||
///solve contact constraints
|
||||
solveContactConstraints(infoGlobal);
|
||||
|
||||
///update vehicle simulation
|
||||
updateVehicles(timeStep);
|
||||
|
||||
///CallbackTriggers();
|
||||
|
||||
///integrate transforms
|
||||
integrateTransforms(timeStep);
|
||||
|
||||
|
||||
///update vehicle simulation
|
||||
updateVehicles(timeStep);
|
||||
|
||||
|
||||
updateActivationState( timeStep );
|
||||
|
||||
|
||||
@ -268,12 +320,19 @@ void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
|
||||
|
||||
void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
|
||||
{
|
||||
body->setGravity(m_gravity);
|
||||
bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
|
||||
short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter;
|
||||
short collisionFilterMask = isDynamic? btBroadphaseProxy::AllFilter : btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
|
||||
if (!body->isStaticOrKinematicObject())
|
||||
{
|
||||
body->setGravity(m_gravity);
|
||||
}
|
||||
|
||||
addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
|
||||
if (body->getCollisionShape())
|
||||
{
|
||||
bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
|
||||
short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter;
|
||||
short collisionFilterMask = isDynamic? btBroadphaseProxy::AllFilter : btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
|
||||
|
||||
addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -303,12 +362,12 @@ void btDiscreteDynamicsWorld::updateActivationState(float timeStep)
|
||||
|
||||
if (body->wantsSleeping())
|
||||
{
|
||||
if (body->GetActivationState() == ACTIVE_TAG)
|
||||
body->SetActivationState( WANTS_DEACTIVATION );
|
||||
if (body->getActivationState() == ACTIVE_TAG)
|
||||
body->setActivationState( WANTS_DEACTIVATION );
|
||||
} else
|
||||
{
|
||||
if (body->GetActivationState() != DISABLE_DEACTIVATION)
|
||||
body->SetActivationState( ACTIVE_TAG );
|
||||
if (body->getActivationState() != DISABLE_DEACTIVATION)
|
||||
body->setActivationState( ACTIVE_TAG );
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -436,11 +495,11 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
|
||||
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
|
||||
((colObj1) && ((colObj1)->mergesSimulationIslands())))
|
||||
{
|
||||
if (colObj0->IsActive() || colObj1->IsActive())
|
||||
if (colObj0->isActive() || colObj1->isActive())
|
||||
{
|
||||
|
||||
getSimulationIslandManager()->getUnionFind().unite((colObj0)->m_islandTag1,
|
||||
(colObj1)->m_islandTag1);
|
||||
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
|
||||
(colObj1)->getIslandTag());
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -501,19 +560,19 @@ void btDiscreteDynamicsWorld::updateAabbs()
|
||||
// if (body->IsActive() && (!body->IsStatic()))
|
||||
{
|
||||
btPoint3 minAabb,maxAabb;
|
||||
colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb);
|
||||
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
|
||||
btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache;
|
||||
|
||||
//moving objects should be moderately sized, probably something wrong if not
|
||||
if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < 1e12f))
|
||||
{
|
||||
bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
|
||||
bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
|
||||
} else
|
||||
{
|
||||
//something went wrong, investigate
|
||||
//this assert is unwanted in 3D modelers (danger of loosing work)
|
||||
assert(0);
|
||||
body->SetActivationState(DISABLE_SIMULATION);
|
||||
body->setActivationState(DISABLE_SIMULATION);
|
||||
|
||||
static bool reportMe = true;
|
||||
if (reportMe)
|
||||
@ -548,7 +607,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(float timeStep)
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
if (body->IsActive() && (!body->isStaticOrKinematicObject()))
|
||||
if (body->isActive() && (!body->isStaticOrKinematicObject()))
|
||||
{
|
||||
body->predictIntegratedTransform(timeStep, predictedTrans);
|
||||
body->proceedToTransform( predictedTrans);
|
||||
@ -571,11 +630,11 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep)
|
||||
{
|
||||
if (!body->isStaticOrKinematicObject())
|
||||
{
|
||||
if (body->IsActive())
|
||||
if (body->isActive())
|
||||
{
|
||||
body->applyForces( timeStep);
|
||||
body->integrateVelocities( timeStep);
|
||||
body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
|
||||
body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -768,3 +827,14 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
|
||||
{
|
||||
if (m_ownsConstraintSolver)
|
||||
{
|
||||
delete m_constraintSolver;
|
||||
}
|
||||
m_ownsConstraintSolver = false;
|
||||
m_constraintSolver = solver;
|
||||
}
|
||||
|
||||
|
@ -86,7 +86,7 @@ public:
|
||||
btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver=0);
|
||||
|
||||
///this btDiscreteDynamicsWorld will create and own dispatcher, pairCache and constraintSolver, and deletes it in the destructor.
|
||||
btDiscreteDynamicsWorld();
|
||||
btDiscreteDynamicsWorld(btConstraintSolver* constraintSolver = 0);
|
||||
|
||||
virtual ~btDiscreteDynamicsWorld();
|
||||
|
||||
@ -136,6 +136,7 @@ public:
|
||||
|
||||
void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
|
||||
|
||||
virtual void setConstraintSolver(btConstraintSolver* solver);
|
||||
|
||||
};
|
||||
|
||||
|
@ -19,6 +19,8 @@ subject to the following restrictions:
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
|
||||
class btTypedConstraint;
|
||||
class btRaycastVehicle;
|
||||
class btConstraintSolver;
|
||||
|
||||
|
||||
///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous
|
||||
class btDynamicsWorld : public btCollisionWorld
|
||||
@ -65,6 +67,8 @@ class btDynamicsWorld : public btCollisionWorld
|
||||
|
||||
virtual void removeRigidBody(btRigidBody* body) = 0;
|
||||
|
||||
virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_DYNAMICS_WORLD_H
|
||||
|
@ -42,7 +42,13 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap
|
||||
m_frictionSolverType(0)
|
||||
{
|
||||
|
||||
motionState->getWorldTransform(m_worldTransform);
|
||||
if (motionState)
|
||||
{
|
||||
motionState->getWorldTransform(m_worldTransform);
|
||||
} else
|
||||
{
|
||||
m_worldTransform = btTransform::getIdentity();
|
||||
}
|
||||
|
||||
m_interpolationWorldTransform = m_worldTransform;
|
||||
m_interpolationLinearVelocity.setValue(0,0,0);
|
||||
@ -64,7 +70,7 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap
|
||||
|
||||
}
|
||||
|
||||
|
||||
#ifdef OBSOLETE_MOTIONSTATE_LESS
|
||||
btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
|
||||
:
|
||||
m_gravity(0.0f, 0.0f, 0.0f),
|
||||
@ -100,30 +106,34 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi
|
||||
|
||||
}
|
||||
|
||||
#endif //OBSOLETE_MOTIONSTATE_LESS
|
||||
|
||||
|
||||
#define EXPERIMENTAL_JITTER_REMOVAL 1
|
||||
#ifdef EXPERIMENTAL_JITTER_REMOVAL
|
||||
//Bullet 2.20b has experimental code to reduce jitter just before objects fall asleep/deactivate
|
||||
//doesn't work very well yet (value 0 only reduces performance a bit, no difference in functionality)
|
||||
float gClippedAngvelThresholdSqr = 0.f;
|
||||
float gClippedLinearThresholdSqr = 0.f;
|
||||
//Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
|
||||
//doesn't work very well yet (value 0 disabled this damping)
|
||||
//note there this influences deactivation thresholds!
|
||||
float gClippedAngvelThresholdSqr = 0.01f;
|
||||
float gClippedLinearThresholdSqr = 0.01f;
|
||||
float gJitterVelocityDampingFactor = 1.f;
|
||||
#endif //EXPERIMENTAL_JITTER_REMOVAL
|
||||
|
||||
void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform)
|
||||
{
|
||||
|
||||
#ifdef EXPERIMENTAL_JITTER_REMOVAL
|
||||
//clip to avoid jitter
|
||||
if (m_angularVelocity.length2() < gClippedAngvelThresholdSqr)
|
||||
//if (wantsSleeping())
|
||||
{
|
||||
m_angularVelocity.setValue(0,0,0);
|
||||
printf("clipped!\n");
|
||||
//clip to avoid jitter
|
||||
if ((m_angularVelocity.length2() < gClippedAngvelThresholdSqr) &&
|
||||
(m_linearVelocity.length2() < gClippedLinearThresholdSqr))
|
||||
{
|
||||
m_angularVelocity *= gJitterVelocityDampingFactor;
|
||||
m_linearVelocity *= gJitterVelocityDampingFactor;
|
||||
}
|
||||
}
|
||||
|
||||
if (m_linearVelocity.length2() < gClippedLinearThresholdSqr)
|
||||
{
|
||||
m_linearVelocity.setValue(0,0,0);
|
||||
printf("clipped!\n");
|
||||
}
|
||||
#endif //EXPERIMENTAL_JITTER_REMOVAL
|
||||
|
||||
btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
|
||||
|
@ -62,7 +62,11 @@ class btRigidBody : public btCollisionObject
|
||||
|
||||
public:
|
||||
|
||||
#ifdef OBSOLETE_MOTIONSTATE_LESS
|
||||
//not supported, please use btMotionState
|
||||
btRigidBody(float mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f);
|
||||
#endif //OBSOLETE_MOTIONSTATE_LESS
|
||||
|
||||
btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f);
|
||||
|
||||
void proceedToTransform(const btTransform& newTrans);
|
||||
@ -71,11 +75,11 @@ public:
|
||||
///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
|
||||
static const btRigidBody* upcast(const btCollisionObject* colObj)
|
||||
{
|
||||
return (const btRigidBody*)colObj->m_internalOwner;
|
||||
return (const btRigidBody*)colObj->getInternalOwner();
|
||||
}
|
||||
static btRigidBody* upcast(btCollisionObject* colObj)
|
||||
{
|
||||
return (btRigidBody*)colObj->m_internalOwner;
|
||||
return (btRigidBody*)colObj->getInternalOwner();
|
||||
}
|
||||
|
||||
/// continuous collision detection needs prediction
|
||||
@ -247,7 +251,7 @@ public:
|
||||
|
||||
inline void updateDeactivation(float timeStep)
|
||||
{
|
||||
if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == DISABLE_DEACTIVATION))
|
||||
if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
|
||||
return;
|
||||
|
||||
if ((getLinearVelocity().length2() < gLinearSleepingThreshold*gLinearSleepingThreshold) &&
|
||||
@ -257,7 +261,7 @@ public:
|
||||
} else
|
||||
{
|
||||
m_deactivationTime=0.f;
|
||||
SetActivationState(0);
|
||||
setActivationState(0);
|
||||
}
|
||||
|
||||
}
|
||||
@ -265,14 +269,14 @@ public:
|
||||
inline bool wantsSleeping()
|
||||
{
|
||||
|
||||
if (GetActivationState() == DISABLE_DEACTIVATION)
|
||||
if (getActivationState() == DISABLE_DEACTIVATION)
|
||||
return false;
|
||||
|
||||
//disable deactivation
|
||||
if (gDisableDeactivation || (gDeactivationTime == 0.f))
|
||||
return false;
|
||||
|
||||
if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == WANTS_DEACTIVATION))
|
||||
if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
|
||||
return true;
|
||||
|
||||
if (m_deactivationTime> gDeactivationTime)
|
||||
@ -309,6 +313,8 @@ public:
|
||||
void setMotionState(btMotionState* motionState)
|
||||
{
|
||||
m_optionalMotionState = motionState;
|
||||
if (m_optionalMotionState)
|
||||
motionState->getWorldTransform(m_worldTransform);
|
||||
}
|
||||
|
||||
//for experimental overriding of friction/contact solver func
|
||||
|
@ -114,12 +114,12 @@ void btSimpleDynamicsWorld::updateAabbs()
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
if (body->IsActive() && (!body->isStaticObject()))
|
||||
if (body->isActive() && (!body->isStaticObject()))
|
||||
{
|
||||
btPoint3 minAabb,maxAabb;
|
||||
colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb);
|
||||
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
|
||||
btBroadphaseInterface* bp = getBroadphase();
|
||||
bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
|
||||
bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -134,7 +134,7 @@ void btSimpleDynamicsWorld::integrateTransforms(float timeStep)
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
{
|
||||
if (body->IsActive() && (!body->isStaticObject()))
|
||||
if (body->isActive() && (!body->isStaticObject()))
|
||||
{
|
||||
body->predictIntegratedTransform(timeStep, predictedTrans);
|
||||
body->proceedToTransform( predictedTrans);
|
||||
@ -155,11 +155,11 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(float timeStep)
|
||||
{
|
||||
if (!body->isStaticObject())
|
||||
{
|
||||
if (body->IsActive())
|
||||
if (body->isActive())
|
||||
{
|
||||
body->applyForces( timeStep);
|
||||
body->integrateVelocities( timeStep);
|
||||
body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
|
||||
body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -176,11 +176,22 @@ void btSimpleDynamicsWorld::synchronizeMotionStates()
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body && body->getMotionState())
|
||||
{
|
||||
if (body->GetActivationState() != ISLAND_SLEEPING)
|
||||
if (body->getActivationState() != ISLAND_SLEEPING)
|
||||
{
|
||||
body->getMotionState()->setWorldTransform(body->m_worldTransform);
|
||||
body->getMotionState()->setWorldTransform(body->getWorldTransform());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
|
||||
{
|
||||
if (m_ownsConstraintSolver)
|
||||
{
|
||||
delete m_constraintSolver;
|
||||
}
|
||||
m_ownsConstraintSolver = false;
|
||||
m_constraintSolver = solver;
|
||||
}
|
||||
|
@ -77,6 +77,8 @@ public:
|
||||
|
||||
void synchronizeMotionStates();
|
||||
|
||||
virtual void setConstraintSolver(btConstraintSolver* solver);
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_SIMPLE_DYNAMICS_WORLD_H
|
||||
|
@ -11,6 +11,7 @@
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btRaycastVehicle.h"
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
|
||||
#include "LinearMath/btQuaternion.h"
|
||||
@ -23,7 +24,7 @@
|
||||
|
||||
|
||||
|
||||
static btRigidBody s_fixedObject( 0,btTransform(btQuaternion(0,0,0,1)),0);
|
||||
static btRigidBody s_fixedObject( 0,0,0);
|
||||
|
||||
btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
|
||||
:m_vehicleRaycaster(raycaster),
|
||||
@ -75,8 +76,8 @@ btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con
|
||||
|
||||
btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];
|
||||
|
||||
updateWheelTransformsWS( wheel );
|
||||
updateWheelTransform(getNumWheels()-1);
|
||||
updateWheelTransformsWS( wheel , false );
|
||||
updateWheelTransform(getNumWheels()-1,false);
|
||||
return wheel;
|
||||
}
|
||||
|
||||
@ -91,15 +92,18 @@ const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
|
||||
|
||||
}
|
||||
|
||||
void btRaycastVehicle::updateWheelTransform( int wheelIndex )
|
||||
void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
|
||||
{
|
||||
|
||||
btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
|
||||
updateWheelTransformsWS(wheel);
|
||||
updateWheelTransformsWS(wheel,interpolatedTransform);
|
||||
btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
|
||||
const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
|
||||
btVector3 fwd = up.cross(right);
|
||||
fwd = fwd.normalize();
|
||||
// up = right.cross(fwd);
|
||||
// up.normalize();
|
||||
|
||||
//rotate around steering over de wheelAxleWS
|
||||
float steering = wheel.m_steering;
|
||||
|
||||
@ -138,16 +142,16 @@ void btRaycastVehicle::resetSuspension()
|
||||
}
|
||||
}
|
||||
|
||||
void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel )
|
||||
void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
|
||||
{
|
||||
wheel.m_raycastInfo.m_isInContact = false;
|
||||
|
||||
btTransform chassisTrans;
|
||||
if (getRigidBody()->getMotionState())
|
||||
btTransform chassisTrans = getChassisWorldTransform();
|
||||
if (interpolatedTransform && (getRigidBody()->getMotionState()))
|
||||
{
|
||||
getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
|
||||
else
|
||||
chassisTrans = getRigidBody()->getCenterOfMassTransform();
|
||||
|
||||
}
|
||||
|
||||
wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
|
||||
wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ;
|
||||
wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
|
||||
@ -155,7 +159,7 @@ void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel )
|
||||
|
||||
btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
|
||||
{
|
||||
updateWheelTransformsWS( wheel );
|
||||
updateWheelTransformsWS( wheel,false);
|
||||
|
||||
|
||||
btScalar depth = -1;
|
||||
@ -239,12 +243,35 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
|
||||
}
|
||||
|
||||
|
||||
const btTransform& btRaycastVehicle::getChassisWorldTransform() const
|
||||
{
|
||||
/*if (getRigidBody()->getMotionState())
|
||||
{
|
||||
btTransform chassisWorldTrans;
|
||||
getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
|
||||
return chassisWorldTrans;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
return getRigidBody()->getCenterOfMassTransform();
|
||||
}
|
||||
|
||||
|
||||
void btRaycastVehicle::updateVehicle( btScalar step )
|
||||
{
|
||||
{
|
||||
for (int i=0;i<getNumWheels();i++)
|
||||
{
|
||||
updateWheelTransform(i,false);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
m_currentVehicleSpeedKmHour = 3.6f * getRigidBody()->getLinearVelocity().length();
|
||||
|
||||
const btTransform& chassisTrans = getRigidBody()->getCenterOfMassTransform();
|
||||
const btTransform& chassisTrans = getChassisWorldTransform();
|
||||
|
||||
btVector3 forwardW (
|
||||
chassisTrans.getBasis()[0][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[1][m_indexForwardAxis],
|
||||
@ -304,10 +331,12 @@ void btRaycastVehicle::updateVehicle( btScalar step )
|
||||
|
||||
if (wheel.m_raycastInfo.m_isInContact)
|
||||
{
|
||||
const btTransform& chassisWorldTransform = getChassisWorldTransform();
|
||||
|
||||
btVector3 fwd (
|
||||
getRigidBody()->getCenterOfMassTransform().getBasis()[0][m_indexForwardAxis],
|
||||
getRigidBody()->getCenterOfMassTransform().getBasis()[1][m_indexForwardAxis],
|
||||
getRigidBody()->getCenterOfMassTransform().getBasis()[2][m_indexForwardAxis]);
|
||||
chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
|
||||
chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
|
||||
chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
|
||||
|
||||
btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
|
||||
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
|
||||
|
@ -68,9 +68,8 @@ public:
|
||||
virtual ~btRaycastVehicle() ;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
const btTransform& getChassisWorldTransform() const;
|
||||
|
||||
btScalar rayCast(btWheelInfo& wheel);
|
||||
|
||||
virtual void updateVehicle(btScalar step);
|
||||
@ -86,7 +85,7 @@ public:
|
||||
|
||||
const btTransform& getWheelTransformWS( int wheelIndex ) const;
|
||||
|
||||
void updateWheelTransform( int wheelIndex );
|
||||
void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true );
|
||||
|
||||
void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
|
||||
|
||||
@ -103,7 +102,7 @@ public:
|
||||
|
||||
btWheelInfo& getWheelInfo(int index);
|
||||
|
||||
void updateWheelTransformsWS(btWheelInfo& wheel );
|
||||
void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true);
|
||||
|
||||
|
||||
void setBrake(float brake,int wheelIndex);
|
||||
|
1
extern/bullet2/src/LinearMath/CMakeLists.txt
vendored
1
extern/bullet2/src/LinearMath/CMakeLists.txt
vendored
@ -5,5 +5,6 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src }
|
||||
|
||||
ADD_LIBRARY(LibLinearMath
|
||||
btQuickprof.cpp
|
||||
btGeometryUtil.cpp
|
||||
)
|
||||
|
||||
|
70
extern/bullet2/src/LinearMath/btAabbUtil2.h
vendored
70
extern/bullet2/src/LinearMath/btAabbUtil2.h
vendored
@ -18,6 +18,8 @@ subject to the following restrictions:
|
||||
#define AABB_UTIL2
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btSimdMinMax.h"
|
||||
|
||||
|
||||
#define btMin(a,b) ((a < b ? a : b))
|
||||
#define btMax(a,b) ((a > b ? a : b))
|
||||
@ -53,5 +55,73 @@ SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent)
|
||||
{
|
||||
return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
|
||||
(p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
|
||||
(p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
|
||||
(p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
|
||||
(p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
|
||||
(p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
|
||||
const btVector3& rayTo,
|
||||
const btVector3& aabbMin,
|
||||
const btVector3& aabbMax,
|
||||
btScalar& param, btVector3& normal)
|
||||
{
|
||||
btVector3 aabbHalfExtent = (aabbMax-aabbMin)* 0.5f;
|
||||
btVector3 aabbCenter = (aabbMax+aabbMin)* 0.5f;
|
||||
btVector3 source = rayFrom - aabbCenter;
|
||||
btVector3 target = rayTo - aabbCenter;
|
||||
int sourceOutcode = btOutcode(source,aabbHalfExtent);
|
||||
int targetOutcode = btOutcode(target,aabbHalfExtent);
|
||||
if ((sourceOutcode & targetOutcode) == 0x0)
|
||||
{
|
||||
btScalar lambda_enter = btScalar(0.0);
|
||||
btScalar lambda_exit = param;
|
||||
btVector3 r = target - source;
|
||||
int i;
|
||||
btScalar normSign = 1;
|
||||
btVector3 hitNormal(0,0,0);
|
||||
int bit=1;
|
||||
|
||||
for (int j=0;j<2;j++)
|
||||
{
|
||||
for (i = 0; i != 3; ++i)
|
||||
{
|
||||
if (sourceOutcode & bit)
|
||||
{
|
||||
btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
|
||||
if (lambda_enter <= lambda)
|
||||
{
|
||||
lambda_enter = lambda;
|
||||
hitNormal.setValue(0,0,0);
|
||||
hitNormal[i] = normSign;
|
||||
}
|
||||
}
|
||||
else if (targetOutcode & bit)
|
||||
{
|
||||
btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
|
||||
btSetMin(lambda_exit, lambda);
|
||||
}
|
||||
bit<<=1;
|
||||
}
|
||||
normSign = -1.f;
|
||||
}
|
||||
if (lambda_enter <= lambda_exit)
|
||||
{
|
||||
param = lambda_enter;
|
||||
normal = hitNormal;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
@ -19,16 +19,25 @@ struct btDefaultMotionState : public btMotionState
|
||||
}
|
||||
|
||||
///synchronizes world transform from user to physics
|
||||
virtual void getWorldTransform(btTransform& centerOfMassWorldTrans )
|
||||
virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const
|
||||
{
|
||||
centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ;
|
||||
}
|
||||
|
||||
///synchronizes world transform from physics to user
|
||||
///Bullet only calls the update of worldtransform for active objects
|
||||
virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans)
|
||||
{
|
||||
m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ;
|
||||
}
|
||||
|
||||
///Bullet gives a callback for objects that are about to be deactivated (put asleep)
|
||||
/// You can intercept this callback for your own bookkeeping.
|
||||
///Also you can return false to disable deactivation for this object this frame.
|
||||
virtual bool deactivationCallback(void* userPointer) {
|
||||
return true;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //DEFAULT_MOTION_STATE_H
|
||||
|
168
extern/bullet2/src/LinearMath/btGeometryUtil.cpp
vendored
Normal file
168
extern/bullet2/src/LinearMath/btGeometryUtil.cpp
vendored
Normal file
@ -0,0 +1,168 @@
|
||||
/*
|
||||
Copyright (c) 2003-2006 Gino van den Bergen / 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 "btGeometryUtil.h"
|
||||
|
||||
bool btGeometryUtil::isPointInsidePlanes(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin)
|
||||
{
|
||||
int numbrushes = planeEquations.size();
|
||||
for (int i=0;i<numbrushes;i++)
|
||||
{
|
||||
const btVector3& N1 = planeEquations[i];
|
||||
float dist = float(N1.dot(point))+float(N1[3])-margin;
|
||||
if (dist>0.f)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const std::vector<btVector3>& vertices, float margin)
|
||||
{
|
||||
int numvertices = vertices.size();
|
||||
for (int i=0;i<numvertices;i++)
|
||||
{
|
||||
const btVector3& N1 = vertices[i];
|
||||
float dist = float(planeNormal.dot(N1))+float(planeNormal[3])-margin;
|
||||
if (dist>0.f)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool notExist(const btVector3& planeEquation,const std::vector<btVector3>& planeEquations)
|
||||
{
|
||||
int numbrushes = planeEquations.size();
|
||||
for (int i=0;i<numbrushes;i++)
|
||||
{
|
||||
const btVector3& N1 = planeEquations[i];
|
||||
if (planeEquation.dot(N1) > 0.999f)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void btGeometryUtil::getPlaneEquationsFromVertices(std::vector<btVector3>& vertices, std::vector<btVector3>& planeEquationsOut )
|
||||
{
|
||||
const int numvertices = vertices.size();
|
||||
// brute force:
|
||||
for (int i=0;i<numvertices;i++)
|
||||
{
|
||||
const btVector3& N1 = vertices[i];
|
||||
|
||||
|
||||
for (int j=i+1;j<numvertices;j++)
|
||||
{
|
||||
const btVector3& N2 = vertices[j];
|
||||
|
||||
for (int k=j+1;k<numvertices;k++)
|
||||
{
|
||||
|
||||
const btVector3& N3 = vertices[k];
|
||||
|
||||
btVector3 planeEquation,edge0,edge1;
|
||||
edge0 = N2-N1;
|
||||
edge1 = N3-N1;
|
||||
float normalSign = 1.f;
|
||||
for (int ww=0;ww<2;ww++)
|
||||
{
|
||||
planeEquation = normalSign * edge0.cross(edge1);
|
||||
if (planeEquation.length2() > 0.0001f)
|
||||
{
|
||||
planeEquation.normalize();
|
||||
if (notExist(planeEquation,planeEquationsOut))
|
||||
{
|
||||
planeEquation[3] = -planeEquation.dot(N1);
|
||||
|
||||
//check if inside, and replace supportingVertexOut if needed
|
||||
if (areVerticesBehindPlane(planeEquation,vertices,0.01f))
|
||||
{
|
||||
planeEquationsOut.push_back(planeEquation);
|
||||
}
|
||||
}
|
||||
}
|
||||
normalSign = -1.f;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void btGeometryUtil::getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut )
|
||||
{
|
||||
const int numbrushes = planeEquations.size();
|
||||
// brute force:
|
||||
for (int i=0;i<numbrushes;i++)
|
||||
{
|
||||
const btVector3& N1 = planeEquations[i];
|
||||
|
||||
|
||||
for (int j=i+1;j<numbrushes;j++)
|
||||
{
|
||||
const btVector3& N2 = planeEquations[j];
|
||||
|
||||
for (int k=j+1;k<numbrushes;k++)
|
||||
{
|
||||
|
||||
const btVector3& N3 = planeEquations[k];
|
||||
|
||||
btVector3 n2n3; n2n3 = N2.cross(N3);
|
||||
btVector3 n3n1; n3n1 = N3.cross(N1);
|
||||
btVector3 n1n2; n1n2 = N1.cross(N2);
|
||||
|
||||
if ( ( n2n3.length2() > 0.0001f ) &&
|
||||
( n3n1.length2() > 0.0001f ) &&
|
||||
( n1n2.length2() > 0.0001f ) )
|
||||
{
|
||||
//point P out of 3 plane equations:
|
||||
|
||||
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
|
||||
//P = -------------------------------------------------------------------------
|
||||
// N1 . ( N2 * N3 )
|
||||
|
||||
|
||||
float quotient = (N1.dot(n2n3));
|
||||
if (btFabs(quotient) > 0.000001f)
|
||||
{
|
||||
quotient = -1.f / quotient;
|
||||
n2n3 *= N1[3];
|
||||
n3n1 *= N2[3];
|
||||
n1n2 *= N3[3];
|
||||
btVector3 potentialVertex = n2n3;
|
||||
potentialVertex += n3n1;
|
||||
potentialVertex += n1n2;
|
||||
potentialVertex *= quotient;
|
||||
|
||||
//check if inside, and replace supportingVertexOut if needed
|
||||
if (isPointInsidePlanes(planeEquations,potentialVertex,0.01f))
|
||||
{
|
||||
verticesOut.push_back(potentialVertex);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
39
extern/bullet2/src/LinearMath/btGeometryUtil.h
vendored
Normal file
39
extern/bullet2/src/LinearMath/btGeometryUtil.h
vendored
Normal file
@ -0,0 +1,39 @@
|
||||
/*
|
||||
Copyright (c) 2003-2006 Gino van den Bergen / 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 BT_GEOMETRY_UTIL_H
|
||||
#define BT_GEOMETRY_UTIL_H
|
||||
#include <vector>
|
||||
#include "btVector3.h"
|
||||
|
||||
class btGeometryUtil
|
||||
{
|
||||
public:
|
||||
|
||||
|
||||
static void getPlaneEquationsFromVertices(std::vector<btVector3>& vertices, std::vector<btVector3>& planeEquationsOut );
|
||||
|
||||
static void getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut );
|
||||
|
||||
static bool isInside(const std::vector<btVector3>& vertices, const btVector3& planeNormal, float margin);
|
||||
|
||||
static bool isPointInsidePlanes(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin);
|
||||
|
||||
static bool areVerticesBehindPlane(const btVector3& planeNormal, const std::vector<btVector3>& vertices, float margin);
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_GEOMETRY_UTIL_H
|
@ -29,10 +29,15 @@ class btMotionState
|
||||
|
||||
}
|
||||
|
||||
virtual void getWorldTransform(btTransform& worldTrans )=0;
|
||||
virtual void getWorldTransform(btTransform& worldTrans ) const =0;
|
||||
|
||||
//Bullet only calls the update of worldtransform for active objects
|
||||
virtual void setWorldTransform(const btTransform& worldTrans)=0;
|
||||
|
||||
|
||||
//future: when Bullet makes attempt to deactivate object, you can intercept this callback (return false to disable deactivation for this object this frame)
|
||||
virtual bool deactivationCallback(void* userPointer) {
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_MOTIONSTATE_H
|
||||
|
37
extern/bullet2/src/LinearMath/btQuickprof.cpp
vendored
37
extern/bullet2/src/LinearMath/btQuickprof.cpp
vendored
@ -1,25 +1,18 @@
|
||||
/************************************************************************
|
||||
* QuickProf *
|
||||
* Copyright (C) 2006 *
|
||||
* Tyler Streeter tylerstreeter@gmail.com *
|
||||
* All rights reserved. *
|
||||
* Web: http://quickprof.sourceforge.net *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser bteral Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* bteral Public License is included with this library in the *
|
||||
* file license-LGPL.txt. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file license-BSD.txt. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* license-LGPL.txt and license-BSD.txt for more details. *
|
||||
************************************************************************/
|
||||
/*
|
||||
Copyright (c) 2006 Tyler Streeter
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
|
||||
// Please visit the project website (http://quickprof.sourceforge.net)
|
||||
// for usage instructions.
|
||||
|
40
extern/bullet2/src/LinearMath/btQuickprof.h
vendored
40
extern/bullet2/src/LinearMath/btQuickprof.h
vendored
@ -1,26 +1,18 @@
|
||||
/************************************************************************
|
||||
* QuickProf *
|
||||
* Copyright (C) 2006 *
|
||||
* Tyler Streeter tylerstreeter@gmail.com *
|
||||
* All rights reserved. *
|
||||
* Web: http://quickprof.sourceforge.net *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser bteral Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* bteral Public License is included with this library in the *
|
||||
* file license-LGPL.txt. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file license-BSD.txt. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* license-LGPL.txt and license-BSD.txt for more details. *
|
||||
************************************************************************/
|
||||
/*
|
||||
Copyright (c) 2006 Tyler Streeter
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
// Please visit the project website (http://quickprof.sourceforge.net)
|
||||
// for usage instructions.
|
||||
|
||||
@ -46,6 +38,10 @@
|
||||
typedef uint64_t __int64;
|
||||
#endif
|
||||
|
||||
#if defined (SUNOS) || defined (__SUNOS__)
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
|
||||
#if defined(WIN32) || defined(_WIN32)
|
||||
#define USE_WINDOWS_TIMERS
|
||||
#include <windows.h>
|
||||
|
19
extern/bullet2/src/LinearMath/btScalar.h
vendored
19
extern/bullet2/src/LinearMath/btScalar.h
vendored
@ -88,13 +88,13 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
|
||||
#endif
|
||||
|
||||
|
||||
const btScalar SIMD_2_PI = 6.283185307179586232f;
|
||||
const btScalar SIMD_PI = SIMD_2_PI * btScalar(0.5f);
|
||||
const btScalar SIMD_HALF_PI = SIMD_2_PI * btScalar(0.25f);
|
||||
const btScalar SIMD_RADS_PER_DEG = SIMD_2_PI / btScalar(360.0f);
|
||||
const btScalar SIMD_DEGS_PER_RAD = btScalar(360.0f) / SIMD_2_PI;
|
||||
const btScalar SIMD_EPSILON = FLT_EPSILON;
|
||||
const btScalar SIMD_INFINITY = FLT_MAX;
|
||||
#define SIMD_2_PI 6.283185307179586232f
|
||||
#define SIMD_PI (SIMD_2_PI * btScalar(0.5f))
|
||||
#define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25f))
|
||||
#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0f))
|
||||
#define SIMD_DEGS_PER_RAD (btScalar(360.0f) / SIMD_2_PI)
|
||||
#define SIMD_EPSILON FLT_EPSILON
|
||||
#define SIMD_INFINITY FLT_MAX
|
||||
|
||||
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
|
||||
|
||||
@ -114,13 +114,14 @@ SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
|
||||
*/
|
||||
|
||||
SIMD_FORCE_INLINE int btSign(btScalar x) {
|
||||
return x < 0.0f ? -1 : x > 0.0f ? 1 : 0;
|
||||
SIMD_FORCE_INLINE int btIsNegative(btScalar x) {
|
||||
return x < 0.0f ? 1 : 0;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
|
||||
SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
|
||||
|
||||
#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
|
||||
|
||||
|
||||
#endif //SIMD___SCALAR_H
|
||||
|
1
extern/bullet2/src/LinearMath/btSimdMinMax.h
vendored
1
extern/bullet2/src/LinearMath/btSimdMinMax.h
vendored
@ -16,6 +16,7 @@ subject to the following restrictions:
|
||||
|
||||
#ifndef SIMD_MINMAX_H
|
||||
#define SIMD_MINMAX_H
|
||||
#include "LinearMath/btScalar.h"
|
||||
|
||||
template <class T>
|
||||
SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) {
|
||||
|
20
extern/bullet2/src/LinearMath/btTransformUtil.h
vendored
20
extern/bullet2/src/LinearMath/btTransformUtil.h
vendored
@ -108,7 +108,16 @@ public:
|
||||
static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
|
||||
{
|
||||
linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
|
||||
#ifdef USE_QUATERNION_DIFF
|
||||
btVector3 axis;
|
||||
btScalar angle;
|
||||
calculateDiffAxisAngle(transform0,transform1,axis,angle);
|
||||
angVel = axis * angle / timeStep;
|
||||
}
|
||||
|
||||
static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
|
||||
{
|
||||
|
||||
#ifdef USE_QUATERNION_DIFF
|
||||
btQuaternion orn0 = transform0.getRotation();
|
||||
btQuaternion orn1a = transform1.getRotation();
|
||||
btQuaternion orn1 = orn0.farthest(orn1a);
|
||||
@ -118,9 +127,7 @@ public:
|
||||
btQuaternion dorn;
|
||||
dmat.getRotation(dorn);
|
||||
#endif//USE_QUATERNION_DIFF
|
||||
|
||||
btVector3 axis;
|
||||
btScalar angle;
|
||||
|
||||
angle = dorn.getAngle();
|
||||
axis = btVector3(dorn.x(),dorn.y(),dorn.z());
|
||||
axis[3] = 0.f;
|
||||
@ -130,13 +137,8 @@ public:
|
||||
axis = btVector3(1.f,0.f,0.f);
|
||||
else
|
||||
axis /= btSqrt(len);
|
||||
|
||||
|
||||
angVel = axis * angle / timeStep;
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //SIMD_TRANSFORM_UTIL_H
|
||||
|
Loading…
Reference in New Issue
Block a user