merged most of the changes from the branch into trunk, except for COLLADA, libxml and glut glitches.

Still need to verify to make sure no unwanted renaming is introduced.
This commit is contained in:
ejcoumans
2006-09-27 20:43:51 +00:00
parent d1e9a885f3
commit eb23bb5c0c
263 changed files with 7528 additions and 6714 deletions

View File

@@ -18,7 +18,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "LinearMath/SimdTransformUtil.h"
#include "LinearMath/btTransformUtil.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "btGjkPairDetector.h"
@@ -26,7 +26,7 @@ subject to the following restrictions:
ContinuousConvexCollision::ContinuousConvexCollision ( ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver, ConvexPenetrationDepthSolver* penetrationDepthSolver)
btContinuousConvexCollision::btContinuousConvexCollision ( btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_simplexSolver(simplexSolver),
m_penetrationDepthSolver(penetrationDepthSolver),
m_convexA(convexA),m_convexB(convexB)
@@ -37,37 +37,37 @@ m_convexA(convexA),m_convexB(convexB)
/// You don't want your game ever to lock-up.
#define MAX_ITERATIONS 1000
bool ContinuousConvexCollision::calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
bool btContinuousConvexCollision::calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
{
m_simplexSolver->reset();
/// compute linear and angular velocity for this interval, to interpolate
SimdVector3 linVelA,angVelA,linVelB,angVelB;
SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linVelA,angVelA);
SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linVelB,angVelB);
btVector3 linVelA,angVelA,linVelB,angVelB;
btTransformUtil::CalculateVelocity(fromA,toA,1.f,linVelA,angVelA);
btTransformUtil::CalculateVelocity(fromB,toB,1.f,linVelB,angVelB);
SimdScalar boundingRadiusA = m_convexA->GetAngularMotionDisc();
SimdScalar boundingRadiusB = m_convexB->GetAngularMotionDisc();
btScalar boundingRadiusA = m_convexA->GetAngularMotionDisc();
btScalar boundingRadiusB = m_convexB->GetAngularMotionDisc();
SimdScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
float radius = 0.001f;
SimdScalar lambda = 0.f;
SimdVector3 v(1,0,0);
btScalar lambda = 0.f;
btVector3 v(1,0,0);
int maxIter = MAX_ITERATIONS;
SimdVector3 n;
btVector3 n;
n.setValue(0.f,0.f,0.f);
bool hasResult = false;
SimdVector3 c;
btVector3 c;
float lastLambda = lambda;
//float epsilon = 0.001f;
@@ -76,21 +76,21 @@ bool ContinuousConvexCollision::calcTimeOfImpact(
//first solution, using GJK
SimdTransform identityTrans;
btTransform identityTrans;
identityTrans.setIdentity();
SphereShape raySphere(0.0f);
btSphereShape raySphere(0.0f);
raySphere.SetMargin(0.f);
// result.DrawCoordSystem(sphereTr);
PointCollector pointCollector1;
btPointCollector pointCollector1;
{
GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
GjkPairDetector::ClosestPointInput input;
btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
//we don't use margins during CCD
gjk.SetIgnoreMargin(true);
@@ -105,7 +105,7 @@ bool ContinuousConvexCollision::calcTimeOfImpact(
if (hasResult)
{
SimdScalar dist;
btScalar dist;
dist = pointCollector1.m_distance;
n = pointCollector1.m_normalOnBInWorld;
@@ -143,17 +143,17 @@ bool ContinuousConvexCollision::calcTimeOfImpact(
//interpolate to next lambda
SimdTransform interpolatedTransA,interpolatedTransB,relativeTrans;
btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
SimdTransformUtil::IntegrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
SimdTransformUtil::IntegrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
btTransformUtil::IntegrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
btTransformUtil::IntegrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
result.DebugDraw( lambda );
PointCollector pointCollector;
GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
GjkPairDetector::ClosestPointInput input;
btPointCollector pointCollector;
btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = interpolatedTransA;
input.m_transformB = interpolatedTransB;
gjk.GetClosestPoints(input,pointCollector,0);
@@ -187,7 +187,7 @@ bool ContinuousConvexCollision::calcTimeOfImpact(
/*
//todo:
//if movement away from normal, discard result
SimdVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
if (result.m_fraction < 1.f)
{
if (move.dot(result.m_normal) <= 0.f)

View File

@@ -19,30 +19,30 @@ subject to the following restrictions:
#include "btConvexCast.h"
#include "btSimplexSolverInterface.h"
class ConvexPenetrationDepthSolver;
class ConvexShape;
class btConvexPenetrationDepthSolver;
class btConvexShape;
/// ContinuousConvexCollision implements angular and linear time of impact for convex objects.
/// btContinuousConvexCollision implements angular and linear time of impact for convex objects.
/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
class ContinuousConvexCollision : public ConvexCast
class btContinuousConvexCollision : public btConvexCast
{
SimplexSolverInterface* m_simplexSolver;
ConvexPenetrationDepthSolver* m_penetrationDepthSolver;
ConvexShape* m_convexA;
ConvexShape* m_convexB;
btSimplexSolverInterface* m_simplexSolver;
btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
btConvexShape* m_convexA;
btConvexShape* m_convexB;
public:
ContinuousConvexCollision (ConvexShape* shapeA,ConvexShape* shapeB ,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver);
btContinuousConvexCollision (btConvexShape* shapeA,btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
virtual bool calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result);

View File

@@ -15,6 +15,6 @@ subject to the following restrictions:
#include "btConvexCast.h"
ConvexCast::~ConvexCast()
btConvexCast::~btConvexCast()
{
}

View File

@@ -17,28 +17,28 @@ subject to the following restrictions:
#ifndef CONVEX_CAST_H
#define CONVEX_CAST_H
#include <LinearMath/SimdTransform.h>
#include <LinearMath/SimdVector3.h>
#include <LinearMath/SimdScalar.h>
class MinkowskiSumShape;
#include "LinearMath/GenIDebugDraw.h"
#include <LinearMath/btTransform.h>
#include <LinearMath/btVector3.h>
#include <LinearMath/btScalar.h>
class btMinkowskiSumShape;
#include "LinearMath/btIDebugDraw.h"
/// ConvexCast is an interface for Casting
class ConvexCast
/// btConvexCast is an interface for Casting
class btConvexCast
{
public:
virtual ~ConvexCast();
virtual ~btConvexCast();
///RayResult stores the closest result
/// alternatively, add a callback method to decide about closest/all results
struct CastResult
{
//virtual bool addRayResult(const SimdVector3& normal,SimdScalar fraction) = 0;
//virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0;
virtual void DebugDraw(SimdScalar fraction) {}
virtual void DrawCoordSystem(const SimdTransform& trans) {}
virtual void DebugDraw(btScalar fraction) {}
virtual void DrawCoordSystem(const btTransform& trans) {}
CastResult()
:m_fraction(1e30f),
@@ -49,22 +49,22 @@ public:
virtual ~CastResult() {};
SimdVector3 m_normal;
SimdScalar m_fraction;
SimdTransform m_hitTransformA;
SimdTransform m_hitTransformB;
btVector3 m_normal;
btScalar m_fraction;
btTransform m_hitTransformA;
btTransform m_hitTransformB;
IDebugDraw* m_debugDrawer;
btIDebugDraw* m_debugDrawer;
};
/// cast a convex against another convex object
virtual bool calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result) = 0;
};

View File

@@ -17,23 +17,23 @@ subject to the following restrictions:
#ifndef CONVEX_PENETRATION_DEPTH_H
#define CONVEX_PENETRATION_DEPTH_H
class SimdVector3;
class btVector3;
#include "btSimplexSolverInterface.h"
class ConvexShape;
#include "LinearMath/SimdPoint3.h"
class SimdTransform;
class btConvexShape;
#include "LinearMath/btPoint3.h"
class btTransform;
///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
class ConvexPenetrationDepthSolver
class btConvexPenetrationDepthSolver
{
public:
virtual ~ConvexPenetrationDepthSolver() {};
virtual bool CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transA,const SimdTransform& transB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb,
class IDebugDraw* debugDraw
virtual ~btConvexPenetrationDepthSolver() {};
virtual bool CalcPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transA,const btTransform& transB,
btVector3& v, btPoint3& pa, btPoint3& pb,
class btIDebugDraw* debugDraw
) = 0;

View File

@@ -16,8 +16,8 @@ subject to the following restrictions:
#ifndef DISCRETE_COLLISION_DETECTOR_INTERFACE_H
#define DISCRETE_COLLISION_DETECTOR_INTERFACE_H
#include "LinearMath/SimdTransform.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"
/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
@@ -25,7 +25,7 @@ subject to the following restrictions:
/// the closest point is on the second object (B), and the normal points from the surface on B towards A.
/// distance is between closest points on B and closest point on A. So you can calculate closest point on A
/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
struct DiscreteCollisionDetectorInterface
struct btDiscreteCollisionDetectorInterface
{
void operator delete(void* ptr) {};
@@ -37,7 +37,7 @@ struct DiscreteCollisionDetectorInterface
///SetShapeIdentifiers provides experimental support for per-triangle material / custom material combiner
virtual void SetShapeIdentifiers(int partId0,int index0, int partId1,int index1)=0;
virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)=0;
virtual void AddContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)=0;
};
struct ClosestPointInput
@@ -47,35 +47,35 @@ struct DiscreteCollisionDetectorInterface
{
}
SimdTransform m_transformA;
SimdTransform m_transformB;
SimdScalar m_maximumDistanceSquared;
btTransform m_transformA;
btTransform m_transformB;
btScalar m_maximumDistanceSquared;
};
virtual ~DiscreteCollisionDetectorInterface() {};
virtual ~btDiscreteCollisionDetectorInterface() {};
//
// give either closest points (distance > 0) or penetration (distance)
// the normal always points from B towards A
//
virtual void GetClosestPoints(const ClosestPointInput& input,Result& output,class IDebugDraw* debugDraw) = 0;
virtual void GetClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) = 0;
SimdScalar getCollisionMargin() { return 0.2f;}
btScalar getCollisionMargin() { return 0.2f;}
};
struct StorageResult : public DiscreteCollisionDetectorInterface::Result
struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
{
SimdVector3 m_normalOnSurfaceB;
SimdVector3 m_closestPointInB;
SimdScalar m_distance; //negative means penetration !
btVector3 m_normalOnSurfaceB;
btVector3 m_closestPointInB;
btScalar m_distance; //negative means penetration !
StorageResult() : m_distance(1e30f)
btStorageResult() : m_distance(1e30f)
{
}
virtual ~StorageResult() {};
virtual ~btStorageResult() {};
virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
virtual void AddContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
{
if (depth < m_distance)
{

View File

@@ -22,37 +22,37 @@ subject to the following restrictions:
#include "btPointCollector.h"
GjkConvexCast::GjkConvexCast(ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver)
btGjkConvexCast::btGjkConvexCast(btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
:m_simplexSolver(simplexSolver),
m_convexA(convexA),
m_convexB(convexB)
{
}
bool GjkConvexCast::calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
bool btGjkConvexCast::calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
{
MinkowskiSumShape combi(m_convexA,m_convexB);
MinkowskiSumShape* convex = &combi;
btMinkowskiSumShape combi(m_convexA,m_convexB);
btMinkowskiSumShape* convex = &combi;
SimdTransform rayFromLocalA;
SimdTransform rayToLocalA;
btTransform rayFromLocalA;
btTransform rayToLocalA;
rayFromLocalA = fromA.inverse()* fromB;
rayToLocalA = toA.inverse()* toB;
SimdTransform trA,trB;
trA = SimdTransform(fromA);
trB = SimdTransform(fromB);
trA.setOrigin(SimdPoint3(0,0,0));
trB.setOrigin(SimdPoint3(0,0,0));
btTransform trA,trB;
trA = btTransform(fromA);
trB = btTransform(fromB);
trA.setOrigin(btPoint3(0,0,0));
trB.setOrigin(btPoint3(0,0,0));
convex->SetTransformA(trA);
convex->SetTransformB(trB);
@@ -62,38 +62,38 @@ bool GjkConvexCast::calcTimeOfImpact(
float radius = 0.01f;
SimdScalar lambda = 0.f;
SimdVector3 s = rayFromLocalA.getOrigin();
SimdVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin();
SimdVector3 x = s;
SimdVector3 n;
btScalar lambda = 0.f;
btVector3 s = rayFromLocalA.getOrigin();
btVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin();
btVector3 x = s;
btVector3 n;
n.setValue(0,0,0);
bool hasResult = false;
SimdVector3 c;
btVector3 c;
float lastLambda = lambda;
//first solution, using GJK
//no penetration support for now, perhaps pass a pointer when we really want it
ConvexPenetrationDepthSolver* penSolverPtr = 0;
btConvexPenetrationDepthSolver* penSolverPtr = 0;
SimdTransform identityTrans;
btTransform identityTrans;
identityTrans.setIdentity();
SphereShape raySphere(0.0f);
btSphereShape raySphere(0.0f);
raySphere.SetMargin(0.f);
SimdTransform sphereTr;
btTransform sphereTr;
sphereTr.setIdentity();
sphereTr.setOrigin( rayFromLocalA.getOrigin());
result.DrawCoordSystem(sphereTr);
{
PointCollector pointCollector1;
GjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);
btPointCollector pointCollector1;
btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);
GjkPairDetector::ClosestPointInput input;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = sphereTr;
input.m_transformB = identityTrans;
gjk.GetClosestPoints(input,pointCollector1,0);
@@ -107,7 +107,7 @@ bool GjkConvexCast::calcTimeOfImpact(
if (hasResult)
{
SimdScalar dist;
btScalar dist;
dist = (c-x).length();
if (dist < radius)
{
@@ -120,7 +120,7 @@ bool GjkConvexCast::calcTimeOfImpact(
{
n = x - c;
SimdScalar nDotr = n.dot(r);
btScalar nDotr = n.dot(r);
if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON))
return false;
@@ -135,9 +135,9 @@ bool GjkConvexCast::calcTimeOfImpact(
sphereTr.setOrigin( x );
result.DrawCoordSystem(sphereTr);
PointCollector pointCollector;
GjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);
GjkPairDetector::ClosestPointInput input;
btPointCollector pointCollector;
btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = sphereTr;
input.m_transformB = identityTrans;
gjk.GetClosestPoints(input,pointCollector,0);

View File

@@ -20,29 +20,29 @@ subject to the following restrictions:
#include <BulletCollision/CollisionShapes/btCollisionMargin.h>
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
#include "btConvexCast.h"
class ConvexShape;
class MinkowskiSumShape;
class btConvexShape;
class btMinkowskiSumShape;
#include "btSimplexSolverInterface.h"
///GjkConvexCast performs a raycast on a convex object using support mapping.
class GjkConvexCast : public ConvexCast
class btGjkConvexCast : public btConvexCast
{
SimplexSolverInterface* m_simplexSolver;
ConvexShape* m_convexA;
ConvexShape* m_convexB;
btSimplexSolverInterface* m_simplexSolver;
btConvexShape* m_convexA;
btConvexShape* m_convexB;
public:
GjkConvexCast(ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver);
btGjkConvexCast(btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);
/// cast a convex against another convex object
virtual bool calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result);
};

View File

@@ -22,14 +22,14 @@ subject to the following restrictions:
#include <stdio.h> //for debug printf
#endif
static const SimdScalar rel_error = SimdScalar(1.0e-5);
SimdScalar rel_error2 = rel_error * rel_error;
static const btScalar rel_error = btScalar(1.0e-5);
btScalar rel_error2 = rel_error * rel_error;
float maxdist2 = 1.e30f;
int gGjkMaxIter=1000;
GjkPairDetector::GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver)
btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_cachedSeparatingAxis(0.f,0.f,1.f),
m_penetrationDepthSolver(penetrationDepthSolver),
m_simplexSolver(simplexSolver),
@@ -43,11 +43,11 @@ m_index1(-1)
{
}
void GjkPairDetector::GetClosestPoints(const ClosestPointInput& input,Result& output,class IDebugDraw* debugDraw)
void btGjkPairDetector::GetClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
{
SimdScalar distance=0.f;
SimdVector3 normalInB(0.f,0.f,0.f);
SimdVector3 pointOnA,pointOnB;
btScalar distance=0.f;
btVector3 normalInB(0.f,0.f,0.f);
btVector3 pointOnA,pointOnB;
float marginA = m_minkowskiA->GetMargin();
float marginB = m_minkowskiB->GetMargin();
@@ -66,10 +66,10 @@ int curIter = 0;
bool checkPenetration = true;
{
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
btScalar squaredDistance = SIMD_INFINITY;
btScalar delta = 0.f;
SimdScalar margin = marginA + marginB;
btScalar margin = marginA + marginB;
@@ -77,11 +77,11 @@ int curIter = 0;
while (true)
{
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a CollisionObject
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (curIter++ > gGjkMaxIter)
{
#if defined(DEBUG) || defined (_DEBUG)
printf("GjkPairDetector maxIter exceeded:%i\n",curIter);
printf("btGjkPairDetector maxIter exceeded:%i\n",curIter);
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
m_cachedSeparatingAxis.getX(),
m_cachedSeparatingAxis.getY(),
@@ -94,19 +94,19 @@ int curIter = 0;
}
SimdVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
SimdVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
SimdVector3 pInA = m_minkowskiA->LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
SimdVector3 qInB = m_minkowskiB->LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
SimdPoint3 pWorld = input.m_transformA(pInA);
SimdPoint3 qWorld = input.m_transformB(qInB);
btVector3 pInA = m_minkowskiA->LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
btPoint3 pWorld = input.m_transformA(pInA);
btPoint3 qWorld = input.m_transformB(qInB);
SimdVector3 w = pWorld - qWorld;
btVector3 w = pWorld - qWorld;
delta = m_cachedSeparatingAxis.dot(w);
// potential exit, they don't overlap
if ((delta > SimdScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{
checkPenetration = false;
break;
@@ -134,7 +134,7 @@ int curIter = 0;
break;
}
SimdScalar previousSquaredDistance = squaredDistance;
btScalar previousSquaredDistance = squaredDistance;
squaredDistance = m_cachedSeparatingAxis.length2();
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
@@ -165,10 +165,10 @@ int curIter = 0;
//valid normal
if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
{
float rlen = 1.f / SimdSqrt(lenSqr );
float rlen = 1.f / btSqrt(lenSqr );
normalInB *= rlen; //normalize
SimdScalar s = SimdSqrt(squaredDistance);
ASSERT(s > SimdScalar(0.0));
btScalar s = btSqrt(squaredDistance);
ASSERT(s > btScalar(0.0));
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((1.f/rlen) - margin);
@@ -198,7 +198,7 @@ int curIter = 0;
float lenSqr = normalInB.length2();
if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
{
normalInB /= SimdSqrt(lenSqr);
normalInB /= btSqrt(lenSqr);
distance = -(pointOnA-pointOnB).length();
} else
{

View File

@@ -20,24 +20,24 @@ subject to the following restrictions:
#define GJK_PAIR_DETECTOR_H
#include "btDiscreteCollisionDetectorInterface.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/btPoint3.h"
#include <BulletCollision/CollisionShapes/btCollisionMargin.h>
class ConvexShape;
class btConvexShape;
#include "btSimplexSolverInterface.h"
class ConvexPenetrationDepthSolver;
class btConvexPenetrationDepthSolver;
/// GjkPairDetector uses GJK to implement the DiscreteCollisionDetectorInterface
class GjkPairDetector : public DiscreteCollisionDetectorInterface
/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
{
SimdVector3 m_cachedSeparatingAxis;
ConvexPenetrationDepthSolver* m_penetrationDepthSolver;
SimplexSolverInterface* m_simplexSolver;
ConvexShape* m_minkowskiA;
ConvexShape* m_minkowskiB;
btVector3 m_cachedSeparatingAxis;
btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
btSimplexSolverInterface* m_simplexSolver;
btConvexShape* m_minkowskiA;
btConvexShape* m_minkowskiB;
bool m_ignoreMargin;
@@ -50,26 +50,26 @@ public:
int m_partId1;
int m_index1;
GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver);
virtual ~GjkPairDetector() {};
btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
virtual ~btGjkPairDetector() {};
virtual void GetClosestPoints(const ClosestPointInput& input,Result& output,class IDebugDraw* debugDraw);
virtual void GetClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);
void SetMinkowskiA(ConvexShape* minkA)
void SetMinkowskiA(btConvexShape* minkA)
{
m_minkowskiA = minkA;
}
void SetMinkowskiB(ConvexShape* minkB)
void SetMinkowskiB(btConvexShape* minkB)
{
m_minkowskiB = minkB;
}
void SetCachedSeperatingAxis(const SimdVector3& seperatingAxis)
void SetCachedSeperatingAxis(const btVector3& seperatingAxis)
{
m_cachedSeparatingAxis = seperatingAxis;
}
void SetPenetrationDepthSolver(ConvexPenetrationDepthSolver* penetrationDepthSolver)
void SetPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver)
{
m_penetrationDepthSolver = penetrationDepthSolver;
}

View File

@@ -16,7 +16,7 @@ subject to the following restrictions:
#include "btManifoldContactAddResult.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
ManifoldContactAddResult::ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr)
btManifoldContactAddResult::btManifoldContactAddResult(btTransform transA,btTransform transB,btPersistentManifold* manifoldPtr)
:m_manifoldPtr(manifoldPtr)
{
m_transAInv = transA.inverse();
@@ -25,16 +25,16 @@ ManifoldContactAddResult::ManifoldContactAddResult(SimdTransform transA,SimdTran
}
void ManifoldContactAddResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
void btManifoldContactAddResult::AddContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
{
if (depth > m_manifoldPtr->GetContactBreakingTreshold())
return;
SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth;
SimdVector3 localA = m_transAInv(pointA );
SimdVector3 localB = m_transBInv(pointInWorld);
ManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
btVector3 localA = m_transAInv(pointA );
btVector3 localB = m_transBInv(pointInWorld);
btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
int insertIndex = m_manifoldPtr->GetCacheEntry(newPt);
if (insertIndex >= 0)

View File

@@ -18,19 +18,19 @@ subject to the following restrictions:
#define MANIFOLD_CONTACT_ADD_RESULT_H
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
class PersistentManifold;
class btPersistentManifold;
class ManifoldContactAddResult : public DiscreteCollisionDetectorInterface::Result
class btManifoldContactAddResult : public btDiscreteCollisionDetectorInterface::Result
{
PersistentManifold* m_manifoldPtr;
SimdTransform m_transAInv;
SimdTransform m_transBInv;
btPersistentManifold* m_manifoldPtr;
btTransform m_transAInv;
btTransform m_transBInv;
public:
ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr);
btManifoldContactAddResult(btTransform transA,btTransform transB,btPersistentManifold* manifoldPtr);
virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth);
virtual void AddContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth);
};

View File

@@ -16,8 +16,8 @@ subject to the following restrictions:
#ifndef MANIFOLD_CONTACT_POINT_H
#define MANIFOLD_CONTACT_POINT_H
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdTransformUtil.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransformUtil.h"
@@ -25,17 +25,17 @@ subject to the following restrictions:
/// ManifoldContactPoint collects and maintains persistent contactpoints.
/// used to improve stability and performance of rigidbody dynamics response.
class ManifoldPoint
class btManifoldPoint
{
public:
ManifoldPoint()
btManifoldPoint()
:m_userPersistentData(0)
{
}
ManifoldPoint( const SimdVector3 &pointA, const SimdVector3 &pointB,
const SimdVector3 &normal,
SimdScalar distance ) :
btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB,
const btVector3 &normal,
btScalar distance ) :
m_localPointA( pointA ),
m_localPointB( pointB ),
m_normalWorldOnB( normal ),
@@ -51,12 +51,12 @@ class ManifoldPoint
SimdVector3 m_localPointA;
SimdVector3 m_localPointB;
SimdVector3 m_positionWorldOnB;
btVector3 m_localPointA;
btVector3 m_localPointB;
btVector3 m_positionWorldOnB;
///m_positionWorldOnA is redundant information, see GetPositionWorldOnA(), but for clarity
SimdVector3 m_positionWorldOnA;
SimdVector3 m_normalWorldOnB;
btVector3 m_positionWorldOnA;
btVector3 m_normalWorldOnB;
float m_distance1;
float m_combinedFriction;
@@ -76,12 +76,12 @@ class ManifoldPoint
return m_lifeTime;
}
SimdVector3 GetPositionWorldOnA() {
btVector3 GetPositionWorldOnA() {
return m_positionWorldOnA;
// return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
}
const SimdVector3& GetPositionWorldOnB()
const btVector3& GetPositionWorldOnB()
{
return m_positionWorldOnB;
}

View File

@@ -20,22 +20,22 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
struct MyResult : public DiscreteCollisionDetectorInterface::Result
struct MyResult : public btDiscreteCollisionDetectorInterface::Result
{
MyResult():m_hasResult(false)
{
}
SimdVector3 m_normalOnBInWorld;
SimdVector3 m_pointInWorld;
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 SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
void AddContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
{
m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld;
@@ -45,80 +45,80 @@ struct MyResult : public DiscreteCollisionDetectorInterface::Result
};
#define NUM_UNITSPHERE_POINTS 42
static SimdVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS] =
static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS] =
{
SimdVector3(0.000000f , -0.000000f,-1.000000f),
SimdVector3(0.723608f , -0.525725f,-0.447219f),
SimdVector3(-0.276388f , -0.850649f,-0.447219f),
SimdVector3(-0.894426f , -0.000000f,-0.447216f),
SimdVector3(-0.276388f , 0.850649f,-0.447220f),
SimdVector3(0.723608f , 0.525725f,-0.447219f),
SimdVector3(0.276388f , -0.850649f,0.447220f),
SimdVector3(-0.723608f , -0.525725f,0.447219f),
SimdVector3(-0.723608f , 0.525725f,0.447219f),
SimdVector3(0.276388f , 0.850649f,0.447219f),
SimdVector3(0.894426f , 0.000000f,0.447216f),
SimdVector3(-0.000000f , 0.000000f,1.000000f),
SimdVector3(0.425323f , -0.309011f,-0.850654f),
SimdVector3(-0.162456f , -0.499995f,-0.850654f),
SimdVector3(0.262869f , -0.809012f,-0.525738f),
SimdVector3(0.425323f , 0.309011f,-0.850654f),
SimdVector3(0.850648f , -0.000000f,-0.525736f),
SimdVector3(-0.525730f , -0.000000f,-0.850652f),
SimdVector3(-0.688190f , -0.499997f,-0.525736f),
SimdVector3(-0.162456f , 0.499995f,-0.850654f),
SimdVector3(-0.688190f , 0.499997f,-0.525736f),
SimdVector3(0.262869f , 0.809012f,-0.525738f),
SimdVector3(0.951058f , 0.309013f,0.000000f),
SimdVector3(0.951058f , -0.309013f,0.000000f),
SimdVector3(0.587786f , -0.809017f,0.000000f),
SimdVector3(0.000000f , -1.000000f,0.000000f),
SimdVector3(-0.587786f , -0.809017f,0.000000f),
SimdVector3(-0.951058f , -0.309013f,-0.000000f),
SimdVector3(-0.951058f , 0.309013f,-0.000000f),
SimdVector3(-0.587786f , 0.809017f,-0.000000f),
SimdVector3(-0.000000f , 1.000000f,-0.000000f),
SimdVector3(0.587786f , 0.809017f,-0.000000f),
SimdVector3(0.688190f , -0.499997f,0.525736f),
SimdVector3(-0.262869f , -0.809012f,0.525738f),
SimdVector3(-0.850648f , 0.000000f,0.525736f),
SimdVector3(-0.262869f , 0.809012f,0.525738f),
SimdVector3(0.688190f , 0.499997f,0.525736f),
SimdVector3(0.525730f , 0.000000f,0.850652f),
SimdVector3(0.162456f , -0.499995f,0.850654f),
SimdVector3(-0.425323f , -0.309011f,0.850654f),
SimdVector3(-0.425323f , 0.309011f,0.850654f),
SimdVector3(0.162456f , 0.499995f,0.850654f)
btVector3(0.000000f , -0.000000f,-1.000000f),
btVector3(0.723608f , -0.525725f,-0.447219f),
btVector3(-0.276388f , -0.850649f,-0.447219f),
btVector3(-0.894426f , -0.000000f,-0.447216f),
btVector3(-0.276388f , 0.850649f,-0.447220f),
btVector3(0.723608f , 0.525725f,-0.447219f),
btVector3(0.276388f , -0.850649f,0.447220f),
btVector3(-0.723608f , -0.525725f,0.447219f),
btVector3(-0.723608f , 0.525725f,0.447219f),
btVector3(0.276388f , 0.850649f,0.447219f),
btVector3(0.894426f , 0.000000f,0.447216f),
btVector3(-0.000000f , 0.000000f,1.000000f),
btVector3(0.425323f , -0.309011f,-0.850654f),
btVector3(-0.162456f , -0.499995f,-0.850654f),
btVector3(0.262869f , -0.809012f,-0.525738f),
btVector3(0.425323f , 0.309011f,-0.850654f),
btVector3(0.850648f , -0.000000f,-0.525736f),
btVector3(-0.525730f , -0.000000f,-0.850652f),
btVector3(-0.688190f , -0.499997f,-0.525736f),
btVector3(-0.162456f , 0.499995f,-0.850654f),
btVector3(-0.688190f , 0.499997f,-0.525736f),
btVector3(0.262869f , 0.809012f,-0.525738f),
btVector3(0.951058f , 0.309013f,0.000000f),
btVector3(0.951058f , -0.309013f,0.000000f),
btVector3(0.587786f , -0.809017f,0.000000f),
btVector3(0.000000f , -1.000000f,0.000000f),
btVector3(-0.587786f , -0.809017f,0.000000f),
btVector3(-0.951058f , -0.309013f,-0.000000f),
btVector3(-0.951058f , 0.309013f,-0.000000f),
btVector3(-0.587786f , 0.809017f,-0.000000f),
btVector3(-0.000000f , 1.000000f,-0.000000f),
btVector3(0.587786f , 0.809017f,-0.000000f),
btVector3(0.688190f , -0.499997f,0.525736f),
btVector3(-0.262869f , -0.809012f,0.525738f),
btVector3(-0.850648f , 0.000000f,0.525736f),
btVector3(-0.262869f , 0.809012f,0.525738f),
btVector3(0.688190f , 0.499997f,0.525736f),
btVector3(0.525730f , 0.000000f,0.850652f),
btVector3(0.162456f , -0.499995f,0.850654f),
btVector3(-0.425323f , -0.309011f,0.850654f),
btVector3(-0.425323f , 0.309011f,0.850654f),
btVector3(0.162456f , 0.499995f,0.850654f)
};
bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transA,const SimdTransform& transB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb,
class IDebugDraw* debugDraw
bool btMinkowskiPenetrationDepthSolver::CalcPenDepth(btSimplexSolverInterface& simplexSolver,
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transA,const btTransform& transB,
btVector3& v, btPoint3& pa, btPoint3& pb,
class btIDebugDraw* debugDraw
)
{
//just take fixed number of orientation, and sample the penetration depth in that direction
float minProj = 1e30f;
SimdVector3 minNorm;
SimdVector3 minVertex;
SimdVector3 minA,minB;
SimdVector3 seperatingAxisInA,seperatingAxisInB;
SimdVector3 pInA,qInB,pWorld,qWorld,w;
btVector3 minNorm;
btVector3 minVertex;
btVector3 minA,minB;
btVector3 seperatingAxisInA,seperatingAxisInB;
btVector3 pInA,qInB,pWorld,qWorld,w;
#define USE_BATCHED_SUPPORT 1
#ifdef USE_BATCHED_SUPPORT
SimdVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS];
SimdVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS];
SimdVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS];
SimdVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS];
btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS];
btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS];
btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS];
btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS];
int i;
for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
{
const SimdVector3& norm = sPenetrationDirections[i];
const btVector3& norm = sPenetrationDirections[i];
seperatingAxisInABatch[i] = (-norm)* transA.getBasis();
seperatingAxisInBBatch[i] = norm * transB.getBasis();
}
@@ -127,7 +127,7 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
convexB->BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS);
for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
{
const SimdVector3& norm = sPenetrationDirections[i];
const btVector3& norm = sPenetrationDirections[i];
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
@@ -150,7 +150,7 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
#else
for (int i=0;i<NUM_UNITSPHERE_POINTS;i++)
{
const SimdVector3& norm = sPenetrationDirections[i];
const btVector3& norm = sPenetrationDirections[i];
seperatingAxisInA = (-norm)* transA.getBasis();
seperatingAxisInB = norm* transB.getBasis();
pInA = convexA->LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
@@ -183,10 +183,10 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
#ifdef DEBUG_DRAW
if (debugDraw)
{
SimdVector3 color(0,1,0);
btVector3 color(0,1,0);
debugDraw->DrawLine(minA,minB,color);
color = SimdVector3 (1,1,1);
SimdVector3 vec = minB-minA;
color = btVector3 (1,1,1);
btVector3 vec = minB-minA;
float prj2 = minNorm.dot(vec);
debugDraw->DrawLine(minA,minA+(minNorm*minProj),color);
@@ -195,18 +195,18 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
GjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
SimdScalar offsetDist = minProj;
SimdVector3 offset = minNorm * offsetDist;
btScalar offsetDist = minProj;
btVector3 offset = minNorm * offsetDist;
GjkPairDetector::ClosestPointInput input;
btGjkPairDetector::ClosestPointInput input;
SimdVector3 newOrg = transA.getOrigin() + offset;
btVector3 newOrg = transA.getOrigin() + offset;
SimdTransform displacedTrans = transA;
btTransform displacedTrans = transA;
displacedTrans.setOrigin(newOrg);
input.m_transformA = displacedTrans;
@@ -232,7 +232,7 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
#ifdef DEBUG_DRAW
if (debugDraw)
{
SimdVector3 color(1,0,0);
btVector3 color(1,0,0);
debugDraw->DrawLine(pa,pb,color);
}
#endif//DEBUG_DRAW

View File

@@ -20,15 +20,15 @@ subject to the following restrictions:
///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
class MinkowskiPenetrationDepthSolver : public ConvexPenetrationDepthSolver
class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
{
public:
virtual bool CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transA,const SimdTransform& transB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb,
class IDebugDraw* debugDraw
virtual bool CalcPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transA,const btTransform& transB,
btVector3& v, btPoint3& pa, btPoint3& pb,
class btIDebugDraw* debugDraw
);
};

View File

@@ -15,7 +15,7 @@ subject to the following restrictions:
#include "btPersistentManifold.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btTransform.h"
#include <assert.h>
float gContactBreakingTreshold = 0.02f;
@@ -23,7 +23,7 @@ ContactDestroyedCallback gContactDestroyedCallback = 0;
PersistentManifold::PersistentManifold()
btPersistentManifold::btPersistentManifold()
:m_body0(0),
m_body1(0),
m_cachedPoints (0),
@@ -32,7 +32,7 @@ m_index1(0)
}
void PersistentManifold::ClearManifold()
void btPersistentManifold::ClearManifold()
{
int i;
for (i=0;i<m_cachedPoints;i++)
@@ -44,7 +44,7 @@ void PersistentManifold::ClearManifold()
#ifdef DEBUG_PERSISTENCY
#include <stdio.h>
void PersistentManifold::DebugPersistency()
void btPersistentManifold::DebugPersistency()
{
int i;
printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
@@ -55,7 +55,7 @@ void PersistentManifold::DebugPersistency()
}
#endif //DEBUG_PERSISTENCY
void PersistentManifold::ClearUserCache(ManifoldPoint& pt)
void btPersistentManifold::ClearUserCache(btManifoldPoint& pt)
{
void* oldPtr = pt.m_userPersistentData;
@@ -91,7 +91,7 @@ void PersistentManifold::ClearUserCache(ManifoldPoint& pt)
}
int PersistentManifold::SortCachedPoints(const ManifoldPoint& pt)
int btPersistentManifold::SortCachedPoints(const btManifoldPoint& pt)
{
//calculate 4 possible cases areas, and take biggest area
@@ -111,55 +111,55 @@ int PersistentManifold::SortCachedPoints(const ManifoldPoint& pt)
}
#endif //KEEP_DEEPEST_POINT
SimdScalar res0(0.f),res1(0.f),res2(0.f),res3(0.f);
btScalar res0(0.f),res1(0.f),res2(0.f),res3(0.f);
if (maxPenetrationIndex != 0)
{
SimdVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
SimdVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
SimdVector3 cross = a0.cross(b0);
btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
btVector3 cross = a0.cross(b0);
res0 = cross.length2();
}
if (maxPenetrationIndex != 1)
{
SimdVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
SimdVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
SimdVector3 cross = a1.cross(b1);
btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
btVector3 cross = a1.cross(b1);
res1 = cross.length2();
}
if (maxPenetrationIndex != 2)
{
SimdVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
SimdVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
SimdVector3 cross = a2.cross(b2);
btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
btVector3 cross = a2.cross(b2);
res2 = cross.length2();
}
if (maxPenetrationIndex != 3)
{
SimdVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
SimdVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
SimdVector3 cross = a3.cross(b3);
btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
btVector3 cross = a3.cross(b3);
res3 = cross.length2();
}
SimdVector4 maxvec(res0,res1,res2,res3);
btVector4 maxvec(res0,res1,res2,res3);
int biggestarea = maxvec.closestAxis4();
return biggestarea;
}
int PersistentManifold::GetCacheEntry(const ManifoldPoint& newPoint) const
int btPersistentManifold::GetCacheEntry(const btManifoldPoint& newPoint) const
{
SimdScalar shortestDist = GetContactBreakingTreshold() * GetContactBreakingTreshold();
btScalar shortestDist = GetContactBreakingTreshold() * GetContactBreakingTreshold();
int size = GetNumContacts();
int nearestPoint = -1;
for( int i = 0; i < size; i++ )
{
const ManifoldPoint &mp = m_pointCache[i];
const btManifoldPoint &mp = m_pointCache[i];
SimdVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
const SimdScalar distToManiPoint = diffA.dot(diffA);
btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
const btScalar distToManiPoint = diffA.dot(diffA);
if( distToManiPoint < shortestDist )
{
shortestDist = distToManiPoint;
@@ -169,7 +169,7 @@ int PersistentManifold::GetCacheEntry(const ManifoldPoint& newPoint) const
return nearestPoint;
}
void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint)
void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint)
{
assert(ValidContactDistance(newPoint));
@@ -193,19 +193,19 @@ void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint)
ReplaceContactPoint(newPoint,insertIndex);
}
float PersistentManifold::GetContactBreakingTreshold() const
float btPersistentManifold::GetContactBreakingTreshold() const
{
return gContactBreakingTreshold;
}
void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const SimdTransform& trB)
void btPersistentManifold::RefreshContactPoints(const btTransform& trA,const btTransform& trB)
{
int i;
/// first refresh worldspace positions and distance
for (i=GetNumContacts()-1;i>=0;i--)
{
ManifoldPoint &manifoldPoint = m_pointCache[i];
btManifoldPoint &manifoldPoint = m_pointCache[i];
manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
@@ -213,12 +213,12 @@ void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const Sim
}
/// then
SimdScalar distance2d;
SimdVector3 projectedDifference,projectedPoint;
btScalar distance2d;
btVector3 projectedDifference,projectedPoint;
for (i=GetNumContacts()-1;i>=0;i--)
{
ManifoldPoint &manifoldPoint = m_pointCache[i];
btManifoldPoint &manifoldPoint = m_pointCache[i];
//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
if (!ValidContactDistance(manifoldPoint))
{

View File

@@ -17,11 +17,11 @@ subject to the following restrictions:
#define PERSISTENT_MANIFOLD_H
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
#include "btManifoldPoint.h"
struct CollisionResult;
struct btCollisionResult;
///contact breaking and merging treshold
extern float gContactBreakingTreshold;
@@ -36,10 +36,10 @@ extern ContactDestroyedCallback gContactDestroyedCallback;
///PersistentManifold maintains contact points, and reduces them to 4.
///It does contact filtering/contact reduction.
class PersistentManifold
class btPersistentManifold
{
ManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
/// this two body pointers can point to the physics rigidbody class.
/// void* will allow any rigidbody class
@@ -49,17 +49,17 @@ class PersistentManifold
/// sort cached points so most isolated points come first
int SortCachedPoints(const ManifoldPoint& pt);
int SortCachedPoints(const btManifoldPoint& pt);
int FindContactPoint(const ManifoldPoint* unUsed, int numUnused,const ManifoldPoint& pt);
int FindContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt);
public:
int m_index1;
PersistentManifold();
btPersistentManifold();
PersistentManifold(void* body0,void* body1)
btPersistentManifold(void* body0,void* body1)
: m_body0(body0),m_body1(body1),m_cachedPoints(0)
{
}
@@ -76,7 +76,7 @@ public:
m_body1 = body1;
}
void ClearUserCache(ManifoldPoint& pt);
void ClearUserCache(btManifoldPoint& pt);
#ifdef DEBUG_PERSISTENCY
void DebugPersistency();
@@ -84,13 +84,13 @@ public:
inline int GetNumContacts() const { return m_cachedPoints;}
inline const ManifoldPoint& GetContactPoint(int index) const
inline const btManifoldPoint& GetContactPoint(int index) const
{
ASSERT(index < m_cachedPoints);
return m_pointCache[index];
}
inline ManifoldPoint& GetContactPoint(int index)
inline btManifoldPoint& GetContactPoint(int index)
{
ASSERT(index < m_cachedPoints);
return m_pointCache[index];
@@ -99,9 +99,9 @@ public:
/// todo: get this margin from the current physics / collision environment
float GetContactBreakingTreshold() const;
int GetCacheEntry(const ManifoldPoint& newPoint) const;
int GetCacheEntry(const btManifoldPoint& newPoint) const;
void AddManifoldPoint( const ManifoldPoint& newPoint);
void AddManifoldPoint( const btManifoldPoint& newPoint);
void RemoveContactPoint (int index)
{
@@ -113,7 +113,7 @@ public:
m_pointCache[lastUsedIndex].m_userPersistentData = 0;
m_cachedPoints--;
}
void ReplaceContactPoint(const ManifoldPoint& newPoint,int insertIndex)
void ReplaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
{
assert(ValidContactDistance(newPoint));
@@ -122,12 +122,12 @@ public:
m_pointCache[insertIndex] = newPoint;
}
bool ValidContactDistance(const ManifoldPoint& pt) const
bool ValidContactDistance(const btManifoldPoint& pt) const
{
return pt.m_distance1 <= GetContactBreakingTreshold();
}
/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
void RefreshContactPoints( const SimdTransform& trA,const SimdTransform& trB);
void RefreshContactPoints( const btTransform& trA,const btTransform& trB);
void ClearManifold();

View File

@@ -20,17 +20,17 @@ subject to the following restrictions:
struct PointCollector : public DiscreteCollisionDetectorInterface::Result
struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
{
SimdVector3 m_normalOnBInWorld;
SimdVector3 m_pointInWorld;
SimdScalar m_distance;//negative means penetration
btVector3 m_normalOnBInWorld;
btVector3 m_pointInWorld;
btScalar m_distance;//negative means penetration
bool m_hasResult;
PointCollector ()
btPointCollector ()
: m_distance(1e30f),m_hasResult(false)
{
}
@@ -40,7 +40,7 @@ struct PointCollector : public DiscreteCollisionDetectorInterface::Result
//??
}
virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
virtual void AddContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
{
if (depth< m_distance)
{

View File

@@ -16,7 +16,7 @@ subject to the following restrictions:
#include "btRaycastCallback.h"
TriangleRaycastCallback::TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to)
btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to)
:
m_from(from),
m_to(to),
@@ -27,18 +27,18 @@ TriangleRaycastCallback::TriangleRaycastCallback(const SimdVector3& from,const S
void TriangleRaycastCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex)
void btTriangleRaycastCallback::ProcessTriangle(btVector3* triangle,int partId, int triangleIndex)
{
const SimdVector3 &vert0=triangle[0];
const SimdVector3 &vert1=triangle[1];
const SimdVector3 &vert2=triangle[2];
const btVector3 &vert0=triangle[0];
const btVector3 &vert1=triangle[1];
const btVector3 &vert2=triangle[2];
SimdVector3 v10; v10 = vert1 - vert0 ;
SimdVector3 v20; v20 = vert2 - vert0 ;
btVector3 v10; v10 = vert1 - vert0 ;
btVector3 v20; v20 = vert2 - vert0 ;
SimdVector3 triangleNormal; triangleNormal = v10.cross( v20 );
btVector3 triangleNormal; triangleNormal = v10.cross( v20 );
const float dist = vert0.dot(triangleNormal);
float dist_a = triangleNormal.dot(m_from) ;
@@ -64,22 +64,22 @@ void TriangleRaycastCallback::ProcessTriangle(SimdVector3* triangle,int partId,
float edge_tolerance =triangleNormal.length2();
edge_tolerance *= -0.0001f;
SimdVector3 point; point.setInterpolate3( m_from, m_to, distance);
btVector3 point; point.setInterpolate3( m_from, m_to, distance);
{
SimdVector3 v0p; v0p = vert0 - point;
SimdVector3 v1p; v1p = vert1 - point;
SimdVector3 cp0; cp0 = v0p.cross( v1p );
btVector3 v0p; v0p = vert0 - point;
btVector3 v1p; v1p = vert1 - point;
btVector3 cp0; cp0 = v0p.cross( v1p );
if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance)
{
SimdVector3 v2p; v2p = vert2 - point;
SimdVector3 cp1;
btVector3 v2p; v2p = vert2 - point;
btVector3 cp1;
cp1 = v1p.cross( v2p);
if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance)
{
SimdVector3 cp2;
btVector3 cp2;
cp2 = v2p.cross(v0p);
if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance)

View File

@@ -17,24 +17,24 @@ subject to the following restrictions:
#define RAYCAST_TRI_CALLBACK_H
#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
struct BroadphaseProxy;
struct btBroadphaseProxy;
class TriangleRaycastCallback: public TriangleCallback
class btTriangleRaycastCallback: public btTriangleCallback
{
public:
//input
SimdVector3 m_from;
SimdVector3 m_to;
btVector3 m_from;
btVector3 m_to;
float m_hitFraction;
TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to);
btTriangleRaycastCallback(const btVector3& from,const btVector3& to);
virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex);
virtual void ProcessTriangle(btVector3* triangle, int partId, int triangleIndex);
virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0;
virtual float ReportHit(const btVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0;
};

View File

@@ -18,42 +18,42 @@ subject to the following restrictions:
#ifndef SIMPLEX_SOLVER_INTERFACE_H
#define SIMPLEX_SOLVER_INTERFACE_H
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btPoint3.h"
#define NO_VIRTUAL_INTERFACE 1
#ifdef NO_VIRTUAL_INTERFACE
#include "btVoronoiSimplexSolver.h"
#define SimplexSolverInterface VoronoiSimplexSolver
#define btSimplexSolverInterface btVoronoiSimplexSolver
#else
/// SimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
/// btSimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
/// voronoi regions or barycentric coordinates
class SimplexSolverInterface
class btSimplexSolverInterface
{
public:
virtual ~SimplexSolverInterface() {};
virtual ~btSimplexSolverInterface() {};
virtual void reset() = 0;
virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q) = 0;
virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) = 0;
virtual bool closest(SimdVector3& v) = 0;
virtual bool closest(btVector3& v) = 0;
virtual SimdScalar maxVertex() = 0;
virtual btScalar maxVertex() = 0;
virtual bool fullSimplex() const = 0;
virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const = 0;
virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const = 0;
virtual bool inSimplex(const SimdVector3& w) = 0;
virtual bool inSimplex(const btVector3& w) = 0;
virtual void backup_closest(SimdVector3& v) = 0;
virtual void backup_closest(btVector3& v) = 0;
virtual bool emptySimplex() const = 0;
virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2) = 0;
virtual void compute_points(btPoint3& p1, btPoint3& p2) = 0;
virtual int numVertices() const =0;

View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
SubsimplexConvexCast::SubsimplexConvexCast (ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver)
btSubsimplexConvexCast::btSubsimplexConvexCast (btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
:m_simplexSolver(simplexSolver),
m_convexA(convexA),m_convexB(convexB)
{
@@ -29,19 +29,19 @@ m_convexA(convexA),m_convexB(convexB)
#define MAX_ITERATIONS 1000
bool SubsimplexConvexCast::calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
bool btSubsimplexConvexCast::calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
{
MinkowskiSumShape combi(m_convexA,m_convexB);
MinkowskiSumShape* convex = &combi;
btMinkowskiSumShape combi(m_convexA,m_convexB);
btMinkowskiSumShape* convex = &combi;
SimdTransform rayFromLocalA;
SimdTransform rayToLocalA;
btTransform rayFromLocalA;
btTransform rayToLocalA;
rayFromLocalA = fromA.inverse()* fromB;
rayToLocalA = toA.inverse()* toB;
@@ -49,28 +49,28 @@ bool SubsimplexConvexCast::calcTimeOfImpact(
m_simplexSolver->reset();
convex->SetTransformB(SimdTransform(rayFromLocalA.getBasis()));
convex->SetTransformB(btTransform(rayFromLocalA.getBasis()));
//float radius = 0.01f;
SimdScalar lambda = 0.f;
btScalar lambda = 0.f;
//todo: need to verify this:
//because of minkowski difference, we need the inverse direction
SimdVector3 s = -rayFromLocalA.getOrigin();
SimdVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin());
SimdVector3 x = s;
SimdVector3 v;
SimdVector3 arbitraryPoint = convex->LocalGetSupportingVertex(r);
btVector3 s = -rayFromLocalA.getOrigin();
btVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin());
btVector3 x = s;
btVector3 v;
btVector3 arbitraryPoint = convex->LocalGetSupportingVertex(r);
v = x - arbitraryPoint;
int maxIter = MAX_ITERATIONS;
SimdVector3 n;
btVector3 n;
n.setValue(0.f,0.f,0.f);
bool hasResult = false;
SimdVector3 c;
btVector3 c;
float lastLambda = lambda;
@@ -78,7 +78,7 @@ bool SubsimplexConvexCast::calcTimeOfImpact(
float dist2 = v.length2();
float epsilon = 0.0001f;
SimdVector3 w,p;
btVector3 w,p;
float VdotR;
while ( (dist2 > epsilon) && maxIter--)

View File

@@ -19,30 +19,30 @@ subject to the following restrictions:
#include "btConvexCast.h"
#include "btSimplexSolverInterface.h"
class ConvexShape;
class btConvexShape;
/// SubsimplexConvexCast implements Gino van den Bergens' paper
///"Ray Casting against General Convex Objects with Application to Continuous Collision Detection"
/// btSubsimplexConvexCast implements Gino van den Bergens' paper
///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
/// GJK based Ray Cast, optimized version
/// Objects should not start in overlap, otherwise results are not defined.
class SubsimplexConvexCast : public ConvexCast
class btSubsimplexConvexCast : public btConvexCast
{
SimplexSolverInterface* m_simplexSolver;
ConvexShape* m_convexA;
ConvexShape* m_convexB;
btSimplexSolverInterface* m_simplexSolver;
btConvexShape* m_convexA;
btConvexShape* m_convexB;
public:
SubsimplexConvexCast (ConvexShape* shapeA,ConvexShape* shapeB,SimplexSolverInterface* simplexSolver);
btSubsimplexConvexCast (btConvexShape* shapeA,btConvexShape* shapeB,btSimplexSolverInterface* simplexSolver);
//virtual ~SubsimplexConvexCast();
//virtual ~btSubsimplexConvexCast();
///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector.
///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
virtual bool calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result);
};

View File

@@ -34,7 +34,7 @@ subject to the following restrictions:
#define VERTD 3
#define CATCH_DEGENERATE_TETRAHEDRON 1
void VoronoiSimplexSolver::removeVertex(int index)
void btVoronoiSimplexSolver::removeVertex(int index)
{
assert(m_numVertices>0);
@@ -44,7 +44,7 @@ void VoronoiSimplexSolver::removeVertex(int index)
m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
}
void VoronoiSimplexSolver::ReduceVertices (const UsageBitfield& usedVerts)
void btVoronoiSimplexSolver::ReduceVertices (const btUsageBitfield& usedVerts)
{
if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
removeVertex(3);
@@ -65,19 +65,19 @@ void VoronoiSimplexSolver::ReduceVertices (const UsageBitfield& usedVerts)
//clear the simplex, remove all the vertices
void VoronoiSimplexSolver::reset()
void btVoronoiSimplexSolver::reset()
{
m_cachedValidClosest = false;
m_numVertices = 0;
m_needsUpdate = true;
m_lastW = SimdVector3(1e30f,1e30f,1e30f);
m_lastW = btVector3(1e30f,1e30f,1e30f);
m_cachedBC.Reset();
}
//add a vertex
void VoronoiSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q)
void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
{
m_lastW = w;
m_needsUpdate = true;
@@ -89,7 +89,7 @@ void VoronoiSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p,
m_numVertices++;
}
bool VoronoiSimplexSolver::UpdateClosestVectorAndPoints()
bool btVoronoiSimplexSolver::UpdateClosestVectorAndPoints()
{
if (m_needsUpdate)
@@ -116,13 +116,13 @@ bool VoronoiSimplexSolver::UpdateClosestVectorAndPoints()
case 2:
{
//closest point origin from line segment
const SimdVector3& from = m_simplexVectorW[0];
const SimdVector3& to = m_simplexVectorW[1];
SimdVector3 nearest;
const btVector3& from = m_simplexVectorW[0];
const btVector3& to = m_simplexVectorW[1];
btVector3 nearest;
SimdVector3 p (0.f,0.f,0.f);
SimdVector3 diff = p - from;
SimdVector3 v = to - from;
btVector3 p (0.f,0.f,0.f);
btVector3 diff = p - from;
btVector3 v = to - from;
float t = v.dot(diff);
if (t > 0) {
@@ -159,11 +159,11 @@ bool VoronoiSimplexSolver::UpdateClosestVectorAndPoints()
case 3:
{
//closest point origin from triangle
SimdVector3 p (0.f,0.f,0.f);
btVector3 p (0.f,0.f,0.f);
const SimdVector3& a = m_simplexVectorW[0];
const SimdVector3& b = m_simplexVectorW[1];
const SimdVector3& c = m_simplexVectorW[2];
const btVector3& a = m_simplexVectorW[0];
const btVector3& b = m_simplexVectorW[1];
const btVector3& c = m_simplexVectorW[2];
ClosestPtPointTriangle(p,a,b,c,m_cachedBC);
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
@@ -187,12 +187,12 @@ bool VoronoiSimplexSolver::UpdateClosestVectorAndPoints()
{
SimdVector3 p (0.f,0.f,0.f);
btVector3 p (0.f,0.f,0.f);
const SimdVector3& a = m_simplexVectorW[0];
const SimdVector3& b = m_simplexVectorW[1];
const SimdVector3& c = m_simplexVectorW[2];
const SimdVector3& d = m_simplexVectorW[3];
const btVector3& a = m_simplexVectorW[0];
const btVector3& b = m_simplexVectorW[1];
const btVector3& c = m_simplexVectorW[2];
const btVector3& d = m_simplexVectorW[3];
bool hasSeperation = ClosestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
@@ -244,7 +244,7 @@ bool VoronoiSimplexSolver::UpdateClosestVectorAndPoints()
}
//return/calculate the closest vertex
bool VoronoiSimplexSolver::closest(SimdVector3& v)
bool btVoronoiSimplexSolver::closest(btVector3& v)
{
bool succes = UpdateClosestVectorAndPoints();
v = m_cachedV;
@@ -253,13 +253,13 @@ bool VoronoiSimplexSolver::closest(SimdVector3& v)
SimdScalar VoronoiSimplexSolver::maxVertex()
btScalar btVoronoiSimplexSolver::maxVertex()
{
int i, numverts = numVertices();
SimdScalar maxV = 0.f;
btScalar maxV = 0.f;
for (i=0;i<numverts;i++)
{
SimdScalar curLen2 = m_simplexVectorW[i].length2();
btScalar curLen2 = m_simplexVectorW[i].length2();
if (maxV < curLen2)
maxV = curLen2;
}
@@ -269,7 +269,7 @@ SimdScalar VoronoiSimplexSolver::maxVertex()
//return the current simplex
int VoronoiSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const
int btVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
{
int i;
for (i=0;i<numVertices();i++)
@@ -284,11 +284,11 @@ int VoronoiSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVec
bool VoronoiSimplexSolver::inSimplex(const SimdVector3& w)
bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
{
bool found = false;
int i, numverts = numVertices();
//SimdScalar maxV = 0.f;
//btScalar maxV = 0.f;
//w is in the current (reduced) simplex
for (i=0;i<numverts;i++)
@@ -304,19 +304,19 @@ bool VoronoiSimplexSolver::inSimplex(const SimdVector3& w)
return found;
}
void VoronoiSimplexSolver::backup_closest(SimdVector3& v)
void btVoronoiSimplexSolver::backup_closest(btVector3& v)
{
v = m_cachedV;
}
bool VoronoiSimplexSolver::emptySimplex() const
bool btVoronoiSimplexSolver::emptySimplex() const
{
return (numVertices() == 0);
}
void VoronoiSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
void btVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
{
UpdateClosestVectorAndPoints();
p1 = m_cachedP1;
@@ -327,14 +327,14 @@ void VoronoiSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
bool VoronoiSimplexSolver::ClosestPtPointTriangle(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c,SubSimplexClosestResult& result)
bool btVoronoiSimplexSolver::ClosestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result)
{
result.m_usedVertices.reset();
// Check if P in vertex region outside A
SimdVector3 ab = b - a;
SimdVector3 ac = c - a;
SimdVector3 ap = p - a;
btVector3 ab = b - a;
btVector3 ac = c - a;
btVector3 ap = p - a;
float d1 = ab.dot(ap);
float d2 = ac.dot(ap);
if (d1 <= 0.0f && d2 <= 0.0f)
@@ -346,7 +346,7 @@ bool VoronoiSimplexSolver::ClosestPtPointTriangle(const SimdPoint3& p, const Sim
}
// Check if P in vertex region outside B
SimdVector3 bp = p - b;
btVector3 bp = p - b;
float d3 = ab.dot(bp);
float d4 = ac.dot(bp);
if (d3 >= 0.0f && d4 <= d3)
@@ -370,7 +370,7 @@ bool VoronoiSimplexSolver::ClosestPtPointTriangle(const SimdPoint3& p, const Sim
}
// Check if P in vertex region outside C
SimdVector3 cp = p - c;
btVector3 cp = p - c;
float d5 = ab.dot(cp);
float d6 = ac.dot(cp);
if (d6 >= 0.0f && d5 <= d6)
@@ -427,9 +427,9 @@ bool VoronoiSimplexSolver::ClosestPtPointTriangle(const SimdPoint3& p, const Sim
/// Test if point p and d lie on opposite sides of plane through abc
int VoronoiSimplexSolver::PointOutsideOfPlane(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d)
int btVoronoiSimplexSolver::PointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d)
{
SimdVector3 normal = (b-a).cross(c-a);
btVector3 normal = (b-a).cross(c-a);
float signp = (p - a).dot(normal); // [AP AB AC]
float signd = (d - a).dot( normal); // [AD AB AC]
@@ -446,9 +446,9 @@ int VoronoiSimplexSolver::PointOutsideOfPlane(const SimdPoint3& p, const SimdPoi
}
bool VoronoiSimplexSolver::ClosestPtPointTetrahedron(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d, SubSimplexClosestResult& finalResult)
bool btVoronoiSimplexSolver::ClosestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult)
{
SubSimplexClosestResult tempResult;
btSubSimplexClosestResult tempResult;
// Start out assuming point inside all halfspaces, so closest to itself
finalResult.m_closestPointOnSimplex = p;
@@ -480,7 +480,7 @@ bool VoronoiSimplexSolver::ClosestPtPointTetrahedron(const SimdPoint3& p, const
if (pointOutsideABC)
{
ClosestPtPointTriangle(p, a, b, c,tempResult);
SimdPoint3 q = tempResult.m_closestPointOnSimplex;
btPoint3 q = tempResult.m_closestPointOnSimplex;
float sqDist = (q - p).dot( q - p);
// Update best closest point if (squared) distance is less than current best
@@ -507,7 +507,7 @@ bool VoronoiSimplexSolver::ClosestPtPointTetrahedron(const SimdPoint3& p, const
if (pointOutsideACD)
{
ClosestPtPointTriangle(p, a, c, d,tempResult);
SimdPoint3 q = tempResult.m_closestPointOnSimplex;
btPoint3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
float sqDist = (q - p).dot( q - p);
@@ -534,7 +534,7 @@ bool VoronoiSimplexSolver::ClosestPtPointTetrahedron(const SimdPoint3& p, const
if (pointOutsideADB)
{
ClosestPtPointTriangle(p, a, d, b,tempResult);
SimdPoint3 q = tempResult.m_closestPointOnSimplex;
btPoint3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
float sqDist = (q - p).dot( q - p);
@@ -561,7 +561,7 @@ bool VoronoiSimplexSolver::ClosestPtPointTetrahedron(const SimdPoint3& p, const
if (pointOutsideBDC)
{
ClosestPtPointTriangle(p, b, d, c,tempResult);
SimdPoint3 q = tempResult.m_closestPointOnSimplex;
btPoint3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
float sqDist = (q - p).dot( q - p);
if (sqDist < bestSqDist)

View File

@@ -15,8 +15,8 @@ subject to the following restrictions:
#ifndef VoronoiSimplexSolver_H
#define VoronoiSimplexSolver_H
#ifndef btVoronoiSimplexSolver_H
#define btVoronoiSimplexSolver_H
#include "btSimplexSolverInterface.h"
@@ -24,8 +24,8 @@ subject to the following restrictions:
#define VORONOI_SIMPLEX_MAX_VERTS 5
struct UsageBitfield{
UsageBitfield()
struct btUsageBitfield{
btUsageBitfield()
{
reset();
}
@@ -48,13 +48,13 @@ struct UsageBitfield{
};
struct SubSimplexClosestResult
struct btSubSimplexClosestResult
{
SimdPoint3 m_closestPointOnSimplex;
btPoint3 m_closestPointOnSimplex;
//MASK for m_usedVertices
//stores the simplex vertex-usage, using the MASK,
// if m_usedVertices & MASK then the related vertex is used
UsageBitfield m_usedVertices;
btUsageBitfield m_usedVertices;
float m_barycentricCoords[4];
bool m_degenerate;
@@ -84,67 +84,67 @@ struct SubSimplexClosestResult
};
/// VoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
/// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
/// Can be used with GJK, as an alternative to Johnson distance algorithm.
#ifdef NO_VIRTUAL_INTERFACE
class VoronoiSimplexSolver
class btVoronoiSimplexSolver
#else
class VoronoiSimplexSolver : public SimplexSolverInterface
class btVoronoiSimplexSolver : public btSimplexSolverInterface
#endif
{
public:
int m_numVertices;
SimdVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
SimdPoint3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
SimdPoint3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
btPoint3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
btPoint3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
SimdPoint3 m_cachedP1;
SimdPoint3 m_cachedP2;
SimdVector3 m_cachedV;
SimdVector3 m_lastW;
btPoint3 m_cachedP1;
btPoint3 m_cachedP2;
btVector3 m_cachedV;
btVector3 m_lastW;
bool m_cachedValidClosest;
SubSimplexClosestResult m_cachedBC;
btSubSimplexClosestResult m_cachedBC;
bool m_needsUpdate;
void removeVertex(int index);
void ReduceVertices (const UsageBitfield& usedVerts);
void ReduceVertices (const btUsageBitfield& usedVerts);
bool UpdateClosestVectorAndPoints();
bool ClosestPtPointTetrahedron(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d, SubSimplexClosestResult& finalResult);
int PointOutsideOfPlane(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d);
bool ClosestPtPointTriangle(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c,SubSimplexClosestResult& result);
bool ClosestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult);
int PointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d);
bool ClosestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result);
public:
void reset();
void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q);
void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
bool closest(SimdVector3& v);
bool closest(btVector3& v);
SimdScalar maxVertex();
btScalar maxVertex();
bool fullSimplex() const
{
return (m_numVertices == 4);
}
int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const;
int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;
bool inSimplex(const SimdVector3& w);
bool inSimplex(const btVector3& w);
void backup_closest(SimdVector3& v) ;
void backup_closest(btVector3& v) ;
bool emptySimplex() const ;
void compute_points(SimdPoint3& p1, SimdPoint3& p2) ;
void compute_points(btPoint3& p1, btPoint3& p2) ;
int numVertices() const
{