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

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

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files

42
Extras/ExtraSolid35/CombinedSimplexSolver.cpp
View File

@@ -30,7 +30,7 @@ void CombinedSimplexSolver::reset()
}
void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q)
void CombinedSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
{
printf("addVertex (%f %f %f)\n",w[0],w[1],w[2]);
m_voronoiSolver.addVertex(w,p,q);
@@ -38,8 +38,8 @@ void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p,
int i;
i=0;
SimdPoint3 vp1,vp2;
SimdPoint3 jp1,jp2;
btPoint3 vp1,vp2;
btPoint3 jp1,jp2;
/*
bool isClosest0 = m_voronoiSolver.closest(vp1);
bool isClosest1 = m_johnsonSolver.closest(vp1);
@@ -52,12 +52,12 @@ void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p,
bool CombinedSimplexSolver::closest(SimdVector3& v)
bool CombinedSimplexSolver::closest(btVector3& v)
{
bool result0 = 0;
bool result1 = 0;
SimdVector3 v0,v1;
btVector3 v0,v1;
result0 = m_voronoiSolver.closest(v0);
result1 = m_johnsonSolver.closest(v1);
@@ -79,10 +79,10 @@ bool CombinedSimplexSolver::closest(SimdVector3& v)
return result1;
}
SimdScalar CombinedSimplexSolver::maxVertex()
btScalar CombinedSimplexSolver::maxVertex()
{
SimdScalar maxv0 = m_voronoiSolver.maxVertex();
SimdScalar maxv1 = m_johnsonSolver.maxVertex();
btScalar maxv0 = m_voronoiSolver.maxVertex();
btScalar maxv1 = m_johnsonSolver.maxVertex();
MY_ASSERT(maxv0 = maxv1);
if (m_useVoronoiSolver)
return maxv0;
@@ -102,7 +102,7 @@ bool CombinedSimplexSolver::fullSimplex() const
return fullSimplex1;
}
int CombinedSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const
int CombinedSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
{
@@ -119,12 +119,12 @@ int CombinedSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVe
void CombinedSimplexSolver::debugPrint()
{
SimdPoint3 pBuf0[4];
SimdPoint3 qBuf0[4];
SimdPoint3 yBuf0[4];
SimdPoint3 pBuf1[4];
SimdPoint3 qBuf1[4];
SimdPoint3 yBuf1[4];
btPoint3 pBuf0[4];
btPoint3 qBuf0[4];
btPoint3 yBuf0[4];
btPoint3 pBuf1[4];
btPoint3 qBuf1[4];
btPoint3 yBuf1[4];
int verts0,verts1;
verts0 = m_voronoiSolver.getSimplex(&pBuf0[0], &qBuf0[0], &yBuf0[0]);
@@ -146,7 +146,7 @@ void CombinedSimplexSolver::debugPrint()
}
bool CombinedSimplexSolver::inSimplex(const SimdVector3& w)
bool CombinedSimplexSolver::inSimplex(const btVector3& w)
{
bool insimplex0 = m_voronoiSolver.inSimplex(w);
bool insimplex1 = m_johnsonSolver.inSimplex(w);
@@ -163,9 +163,9 @@ bool CombinedSimplexSolver::inSimplex(const SimdVector3& w)
return insimplex1;
}
void CombinedSimplexSolver::backup_closest(SimdVector3& v)
void CombinedSimplexSolver::backup_closest(btVector3& v)
{
SimdVector3 v0,v1;
btVector3 v0,v1;
m_voronoiSolver.backup_closest(v0);
m_johnsonSolver.backup_closest(v1);
@@ -189,10 +189,10 @@ bool CombinedSimplexSolver::emptySimplex() const
return empty1;
}
void CombinedSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
void CombinedSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
{
SimdPoint3 tmpP1,tmpP2;
SimdPoint3 tmpJP1,tmpJP2;
btPoint3 tmpP1,tmpP2;
btPoint3 tmpJP1,tmpJP2;
m_voronoiSolver.compute_points(tmpP1,tmpP2);
m_johnsonSolver.compute_points(tmpJP1,tmpJP2);

20
Extras/ExtraSolid35/CombinedSimplexSolver.h
View File

@@ -17,10 +17,10 @@
#include "Solid3JohnsonSimplexSolver.h"
/// CombinedSimplexSolver runs both Solid and Voronoi Simplex Solver for comparison
class CombinedSimplexSolver: public SimplexSolverInterface
class CombinedSimplexSolver: public btSimplexSolverInterface
{
VoronoiSimplexSolver m_voronoiSolver;
// VoronoiSimplexSolver m_johnsonSolver;
btVoronoiSimplexSolver m_voronoiSolver;
// btVoronoiSimplexSolver m_johnsonSolver;
Solid3JohnsonSimplexSolver m_johnsonSolver;
@@ -35,23 +35,23 @@ class CombinedSimplexSolver: public SimplexSolverInterface
virtual void reset();
virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q);
virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
virtual bool closest(SimdVector3& v);
virtual bool closest(btVector3& v);
virtual SimdScalar maxVertex();
virtual btScalar maxVertex();
virtual bool fullSimplex() const;
virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const;
virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;
virtual bool inSimplex(const SimdVector3& w);
virtual bool inSimplex(const btVector3& w);
virtual void backup_closest(SimdVector3& v) ;
virtual void backup_closest(btVector3& v) ;
virtual bool emptySimplex() const;
virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2);
virtual void compute_points(btPoint3& p1, btPoint3& p2);
virtual int numVertices() const;

154
Extras/ExtraSolid35/Solid3EpaPenetrationDepth.cpp
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
@@ -26,34 +26,34 @@
#include <vector>
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "LinearMath/GenMinMax.h"
#include "LinearMath/btMinMax.h"
#define ASSERT_MESSAGE
class ReplaceMeAccuracy {
public:
static SimdScalar rel_error2; // squared relative error in the computed distance
static SimdScalar depth_tolerance; // terminate EPA if upper_bound <= depth_tolerance * dist2
static SimdScalar tol_error; // error tolerance if the distance is almost zero
static btScalar rel_error2; // squared relative error in the computed distance
static btScalar depth_tolerance; // terminate EPA if upper_bound <= depth_tolerance * dist2
static btScalar tol_error; // error tolerance if the distance is almost zero
static void setAccuracy(SimdScalar rel_error)
static void setAccuracy(btScalar rel_error)
{
rel_error2 = rel_error * rel_error;
depth_tolerance = SimdScalar(1.0f) + SimdScalar(2.0f) * rel_error;
depth_tolerance = btScalar(1.0f) + btScalar(2.0f) * rel_error;
}
static void setTolerance(SimdScalar epsilon)
static void setTolerance(btScalar epsilon)
{
tol_error = epsilon;
}
};
static const SimdScalar rel_error = SimdScalar(1.0e-3);
static const btScalar rel_error = btScalar(1.0e-3);
SimdScalar ReplaceMeAccuracy::rel_error2 = rel_error * rel_error;
SimdScalar ReplaceMeAccuracy::depth_tolerance = SimdScalar(1.0) + SimdScalar(2.0) * rel_error;
SimdScalar ReplaceMeAccuracy::tol_error = SIMD_EPSILON;
btScalar ReplaceMeAccuracy::rel_error2 = rel_error * rel_error;
btScalar ReplaceMeAccuracy::depth_tolerance = btScalar(1.0) + btScalar(2.0) * rel_error;
btScalar ReplaceMeAccuracy::tol_error = SIMD_EPSILON;
@@ -101,28 +101,28 @@ public:
bool isObsolete() const { return m_obsolete; }
bool computeClosest(const SimdVector3 *verts);
bool computeClosest(const btVector3 *verts);
const SimdVector3& getClosest() const { return m_closest; }
const btVector3& getClosest() const { return m_closest; }
bool isClosestInternal() const
{
return m_lambda1 >= SimdScalar(0.0) &&
m_lambda2 >= SimdScalar(0.0) &&
return m_lambda1 >= btScalar(0.0) &&
m_lambda2 >= btScalar(0.0) &&
m_lambda1 + m_lambda2 <= m_det;
}
SimdScalar getDist2() const { return m_dist2; }
btScalar getDist2() const { return m_dist2; }
SimdPoint3 getClosestPoint(const SimdPoint3 *points) const
btPoint3 getClosestPoint(const btPoint3 *points) const
{
const SimdPoint3& p0 = points[m_indices[0]];
const btPoint3& p0 = points[m_indices[0]];
return p0 + (m_lambda1 * (points[m_indices[1]] - p0) +
m_lambda2 * (points[m_indices[2]] - p0)) / m_det;
}
void silhouette(const SimdVector3& w, ReplaceMeEdgeBuffer& edgeBuffer)
void silhouette(const btVector3& w, ReplaceMeEdgeBuffer& edgeBuffer)
{
edgeBuffer.clear();
m_obsolete = true;
@@ -132,18 +132,18 @@ public:
}
private:
void silhouette(int index, const SimdVector3& w, ReplaceMeEdgeBuffer& edgeBuffer);
void silhouette(int index, const btVector3& w, ReplaceMeEdgeBuffer& edgeBuffer);
int m_indices[3];
bool m_obsolete;
ReplaceMeFacet *m_adjFacets[3];
int m_adjEdges[3];
SimdScalar m_det;
SimdScalar m_lambda1;
SimdScalar m_lambda2;
SimdVector3 m_closest;
SimdScalar m_dist2;
btScalar m_det;
btScalar m_lambda1;
btScalar m_lambda2;
btVector3 m_closest;
btScalar m_dist2;
};
@@ -163,17 +163,17 @@ bool ReplaceMeFacet::link(int edge0, ReplaceMeFacet *facet, int edge1)
}
bool ReplaceMeFacet::computeClosest(const SimdVector3 *verts)
bool ReplaceMeFacet::computeClosest(const btVector3 *verts)
{
const SimdVector3& p0 = verts[m_indices[0]];
const btVector3& p0 = verts[m_indices[0]];
SimdVector3 v1 = verts[m_indices[1]] - p0;
SimdVector3 v2 = verts[m_indices[2]] - p0;
SimdScalar v1dv1 = v1.length2();
SimdScalar v1dv2 = v1.dot(v2);
SimdScalar v2dv2 = v2.length2();
SimdScalar p0dv1 = p0.dot(v1);
SimdScalar p0dv2 = p0.dot(v2);
btVector3 v1 = verts[m_indices[1]] - p0;
btVector3 v2 = verts[m_indices[2]] - p0;
btScalar v1dv1 = v1.length2();
btScalar v1dv2 = v1.dot(v2);
btScalar v2dv2 = v2.length2();
btScalar p0dv1 = p0.dot(v1);
btScalar p0dv2 = p0.dot(v2);
m_det = v1dv1 * v2dv2 - v1dv2 * v1dv2; // non-negative
//printf("m_det = %f\n",m_det);
@@ -192,7 +192,7 @@ bool ReplaceMeFacet::computeClosest(const SimdVector3 *verts)
return false;
}
void ReplaceMeFacet::silhouette(int index, const SimdVector3& w,
void ReplaceMeFacet::silhouette(int index, const btVector3& w,
ReplaceMeEdgeBuffer& edgeBuffer)
{
if (!m_obsolete) {
@@ -227,9 +227,9 @@ inline int ReplaceMeEdge::getTarget() const
const int MaxSupportPoints = 100;//1000;
const int MaxFacets = 200;//b2000;
static SimdPoint3 pBuf[MaxSupportPoints];
static SimdPoint3 qBuf[MaxSupportPoints];
static SimdVector3 yBuf[MaxSupportPoints];
static btPoint3 pBuf[MaxSupportPoints];
static btPoint3 qBuf[MaxSupportPoints];
static btVector3 yBuf[MaxSupportPoints];
static ReplaceMeFacet facetBuf[MaxFacets];
static int freeFacet = 0;
@@ -249,7 +249,7 @@ public:
ReplaceMeFacetComp myFacetComp;
inline ReplaceMeFacet *addFacet(int i0, int i1, int i2,
SimdScalar lower2, SimdScalar upper2)
btScalar lower2, btScalar upper2)
{
assert(i0 != i1 && i0 != i2 && i1 != i2);
if (freeFacet < MaxFacets)
@@ -300,27 +300,27 @@ inline ReplaceMeFacet *addFacet(int i0, int i1, int i2,
inline bool originInTetrahedron(const SimdVector3& p1, const SimdVector3& p2,
const SimdVector3& p3, const SimdVector3& p4)
inline bool originInTetrahedron(const btVector3& p1, const btVector3& p2,
const btVector3& p3, const btVector3& p4)
{
SimdVector3 normal1 = (p2 - p1).cross(p3 - p1);
SimdVector3 normal2 = (p3 - p2).cross(p4 - p2);
SimdVector3 normal3 = (p4 - p3).cross(p1 - p3);
SimdVector3 normal4 = (p1 - p4).cross(p2 - p4);
btVector3 normal1 = (p2 - p1).cross(p3 - p1);
btVector3 normal2 = (p3 - p2).cross(p4 - p2);
btVector3 normal3 = (p4 - p3).cross(p1 - p3);
btVector3 normal4 = (p1 - p4).cross(p2 - p4);
return
(normal1.dot(p1) > SimdScalar(0.0)) != (normal1.dot(p4) > SimdScalar(0.0)) &&
(normal2.dot(p2) > SimdScalar(0.0)) != (normal2.dot(p1) > SimdScalar(0.0)) &&
(normal3.dot(p3) > SimdScalar(0.0)) != (normal3.dot(p2) > SimdScalar(0.0)) &&
(normal4.dot(p4) > SimdScalar(0.0)) != (normal4.dot(p3) > SimdScalar(0.0));
(normal1.dot(p1) > btScalar(0.0)) != (normal1.dot(p4) > btScalar(0.0)) &&
(normal2.dot(p2) > btScalar(0.0)) != (normal2.dot(p1) > btScalar(0.0)) &&
(normal3.dot(p3) > btScalar(0.0)) != (normal3.dot(p2) > btScalar(0.0)) &&
(normal4.dot(p4) > btScalar(0.0)) != (normal4.dot(p3) > btScalar(0.0));
}
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb)
bool Solid3EpaPenetrationDepth::CalcPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transformA,const btTransform& transformB,
btVector3& v, btPoint3& pa, btPoint3& pb)
{
int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf);
@@ -336,17 +336,17 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
// We have a line segment inside the Minkowski sum containing the
// origin. Blow it up by adding three additional support points.
SimdVector3 dir = (yBuf[1] - yBuf[0]).normalized();
btVector3 dir = (yBuf[1] - yBuf[0]).normalized();
int axis = dir.furthestAxis();
static SimdScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;//
static btScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;//
SimdQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, SimdScalar(0.5));
SimdMatrix3x3 rot_mat(rot);
btQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, btScalar(0.5));
btMatrix3x3 rot_mat(rot);
SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2));
SimdVector3 aux2 = rot_mat * aux1;
SimdVector3 aux3 = rot_mat * aux2;
btVector3 aux1 = dir.cross(btVector3(axis == 0, axis == 1, axis == 2));
btVector3 aux2 = rot_mat * aux1;
btVector3 aux3 = rot_mat * aux2;
pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis()));
qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis()));
@@ -387,9 +387,9 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
// We have a triangle inside the Minkowski sum containing
// the origin. First blow it up.
SimdVector3 v1 = yBuf[1] - yBuf[0];
SimdVector3 v2 = yBuf[2] - yBuf[0];
SimdVector3 vv = v1.cross(v2);
btVector3 v1 = yBuf[1] - yBuf[0];
btVector3 v2 = yBuf[2] - yBuf[0];
btVector3 vv = v1.cross(v2);
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis()));
@@ -430,15 +430,15 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
num_facets = 0;
freeFacet = 0;
ReplaceMeFacet *f0 = addFacet(0, 1, 2, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f1 = addFacet(0, 3, 1, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f2 = addFacet(0, 2, 3, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f3 = addFacet(1, 3, 2, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f0 = addFacet(0, 1, 2, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f1 = addFacet(0, 3, 1, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f2 = addFacet(0, 2, 3, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f3 = addFacet(1, 3, 2, btScalar(0.0), SIMD_INFINITY);
if (!f0 || f0->getDist2() == SimdScalar(0.0) ||
!f1 || f1->getDist2() == SimdScalar(0.0) ||
!f2 || f2->getDist2() == SimdScalar(0.0) ||
!f3 || f3->getDist2() == SimdScalar(0.0))
if (!f0 || f0->getDist2() == btScalar(0.0) ||
!f1 || f1->getDist2() == btScalar(0.0) ||
!f2 || f2->getDist2() == btScalar(0.0) ||
!f3 || f3->getDist2() == btScalar(0.0))
{
return false;
}
@@ -461,7 +461,7 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
ReplaceMeFacet *facet = 0;
SimdScalar upper_bound2 = SIMD_INFINITY;
btScalar upper_bound2 = SIMD_INFINITY;
do {
facet = facetHeap[0];
@@ -470,7 +470,7 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
if (!facet->isObsolete())
{
assert(facet->getDist2() > SimdScalar(0.0));
assert(facet->getDist2() > btScalar(0.0));
if (num_verts == MaxSupportPoints)
{
@@ -487,16 +487,16 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
int index = num_verts++;
SimdScalar far_dist2 = yBuf[index].dot(facet->getClosest());
btScalar far_dist2 = yBuf[index].dot(facet->getClosest());
// Make sure the support mapping is OK.
//assert(far_dist2 > SimdScalar(0.0));
//assert(far_dist2 > btScalar(0.0));
//
// this is to avoid problems with implicit-sphere-touching contact
//
if (far_dist2 < SimdScalar(0.0))
if (far_dist2 < btScalar(0.0))
{
return false;
}

12
Extras/ExtraSolid35/Solid3EpaPenetrationDepth.h
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
@@ -28,14 +28,14 @@
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
/// Solid3EpaPenetrationDepth contains the 'Expanding Polytope Algorithm' from Solid 3.5
class Solid3EpaPenetrationDepth : public ConvexPenetrationDepthSolver
class Solid3EpaPenetrationDepth : public btConvexPenetrationDepthSolver
{
public:
virtual bool CalcPenDepth(SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb);
virtual bool CalcPenDepth(btSimplexSolverInterface& simplexSolver,
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transformA,const btTransform& transformB,
btVector3& v, btPoint3& pa, btPoint3& pb);
};

54
Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.cpp
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
@@ -22,7 +22,7 @@
*/
#include "Solid3JohnsonSimplexSolver.h"
#include "LinearMath/GenMinMax.h"
#include "LinearMath/btMinMax.h"
//#define USE_BACKUP_PROCEDURE
//#define FAST_CLOSEST
@@ -48,7 +48,7 @@ void Solid3JohnsonSimplexSolver::reset()
void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w)
void Solid3JohnsonSimplexSolver::addVertex(const btVector3& w)
{
assert(!fullSimplex());
m_last = 0;
@@ -66,7 +66,7 @@ void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w)
compute_det();
}
void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q)
void Solid3JohnsonSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
{
addVertex(w);
m_p[m_last] = p;
@@ -83,12 +83,12 @@ bool Solid3JohnsonSimplexSolver::fullSimplex() const
return m_bits1 == 0xf;
}
SimdScalar Solid3JohnsonSimplexSolver::maxVertex()
btScalar Solid3JohnsonSimplexSolver::maxVertex()
{
return m_maxlen2;
}
bool Solid3JohnsonSimplexSolver::closest(SimdVector3& v)
bool Solid3JohnsonSimplexSolver::closest(btVector3& v)
{
#ifdef FAST_CLOSEST
T_Bits s;
@@ -125,14 +125,14 @@ bool Solid3JohnsonSimplexSolver::closest(SimdVector3& v)
// Original GJK calls the backup procedure at this point.
#ifdef USE_BACKUP_PROCEDURE
backup_closest(SimdVector3& v);
backup_closest(btVector3& v);
#endif
return false;
}
int Solid3JohnsonSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const
int Solid3JohnsonSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
{
int num_verts = 0;
int i;
@@ -155,7 +155,7 @@ int Solid3JohnsonSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, S
return num_verts;
}
bool Solid3JohnsonSimplexSolver::inSimplex(const SimdVector3& w)
bool Solid3JohnsonSimplexSolver::inSimplex(const btVector3& w)
{
int i;
T_Bits bit;
@@ -171,18 +171,18 @@ bool Solid3JohnsonSimplexSolver::inSimplex(const SimdVector3& w)
void Solid3JohnsonSimplexSolver::backup_closest(SimdVector3& v)
void Solid3JohnsonSimplexSolver::backup_closest(btVector3& v)
{
SimdScalar min_dist2 = SIMD_INFINITY;
btScalar min_dist2 = SIMD_INFINITY;
T_Bits s;
for (s = m_all_bits; s != 0x0; --s)
{
if (subseteq(s, m_all_bits) && proper(s))
{
SimdVector3 u;
btVector3 u;
compute_vector(s, u);
SimdScalar dist2 = u.length2();
btScalar dist2 = u.length2();
if (dist2 < min_dist2)
{
min_dist2 = dist2;
@@ -195,11 +195,11 @@ void Solid3JohnsonSimplexSolver::backup_closest(SimdVector3& v)
}
void Solid3JohnsonSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
void Solid3JohnsonSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
{
SimdScalar sum = SimdScalar(0.0);
p1.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
p2.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
btScalar sum = btScalar(0.0);
p1.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
p2.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
int i;
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
@@ -212,8 +212,8 @@ void Solid3JohnsonSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
}
}
assert(sum > SimdScalar(0.0));
SimdScalar s = SimdScalar(1.0) / sum;
assert(sum > btScalar(0.0));
btScalar s = btScalar(1.0) / sum;
p1 *= s;
p2 *= s;
}
@@ -272,12 +272,12 @@ bool Solid3JohnsonSimplexSolver::valid(T_Bits s)
{
if (contains(s, bit))
{
if (m_det[s][i] <= SimdScalar(0.0))
if (m_det[s][i] <= btScalar(0.0))
{
return false;
}
}
else if (m_det[s | bit][i] > SimdScalar(0.0))
else if (m_det[s | bit][i] > btScalar(0.0))
{
return false;
}
@@ -292,7 +292,7 @@ bool Solid3JohnsonSimplexSolver::proper(T_Bits s)
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{
if (contains(s, bit) && m_det[s][i] <= SimdScalar(0.0))
if (contains(s, bit) && m_det[s][i] <= btScalar(0.0))
{
return false;
}
@@ -300,11 +300,11 @@ bool Solid3JohnsonSimplexSolver::proper(T_Bits s)
return true;
}
void Solid3JohnsonSimplexSolver::compute_vector(T_Bits s, SimdVector3& v)
void Solid3JohnsonSimplexSolver::compute_vector(T_Bits s, btVector3& v)
{
m_maxlen2 = SimdScalar(0.0);
SimdScalar sum = SimdScalar(0.0);
v .setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
m_maxlen2 = btScalar(0.0);
btScalar sum = btScalar(0.0);
v .setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
int i;
T_Bits bit;
@@ -318,7 +318,7 @@ void Solid3JohnsonSimplexSolver::compute_vector(T_Bits s, SimdVector3& v)
}
}
assert(sum > SimdScalar(0.0));
assert(sum > btScalar(0.0));
v /= sum;
}

38
Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.h
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
@@ -30,7 +30,7 @@
/// Solid3JohnsonSimplexSolver contains Johnson subdistance algorithm from Solid 3.5 library
class Solid3JohnsonSimplexSolver : public SimplexSolverInterface
class Solid3JohnsonSimplexSolver : public btSimplexSolverInterface
{
private:
@@ -42,22 +42,22 @@ private:
void compute_det();
bool valid(T_Bits s);
bool proper(T_Bits s);
void compute_vector(T_Bits s, SimdVector3& v);
void compute_vector(T_Bits s, btVector3& v);
SimdScalar m_det[16][4]; // cached sub-determinants
SimdVector3 m_edge[4][4];
btScalar m_det[16][4]; // cached sub-determinants
btVector3 m_edge[4][4];
#ifdef JOHNSON_ROBUST
SimdScalar m_norm[4][4];
btScalar m_norm[4][4];
#endif
SimdPoint3 m_p[4]; // support points of object A in local coordinates
SimdPoint3 m_q[4]; // support points of object B in local coordinates
SimdVector3 m_y[4]; // support points of A - B in world coordinates
SimdScalar m_ylen2[4]; // Squared lengths support points y
btPoint3 m_p[4]; // support points of object A in local coordinates
btPoint3 m_q[4]; // support points of object B in local coordinates
btVector3 m_y[4]; // support points of A - B in world coordinates
btScalar m_ylen2[4]; // Squared lengths support points y
SimdScalar m_maxlen2; // Maximum squared length to a vertex of the current
btScalar m_maxlen2; // Maximum squared length to a vertex of the current
// simplex
T_Bits m_bits1; // identifies current simplex
T_Bits m_last; // identifies last found support point
@@ -69,7 +69,7 @@ private:
private:
void addVertex(const SimdVector3& w);
void addVertex(const btVector3& w);
public:
Solid3JohnsonSimplexSolver();
@@ -78,23 +78,23 @@ public:
virtual void reset();
virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q);
virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
virtual bool closest(SimdVector3& v);
virtual bool closest(btVector3& v);
virtual SimdScalar maxVertex();
virtual btScalar maxVertex();
virtual bool fullSimplex() const;
virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const;
virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;
virtual bool inSimplex(const SimdVector3& w);
virtual bool inSimplex(const btVector3& w);
virtual void backup_closest(SimdVector3& v) ;
virtual void backup_closest(btVector3& v) ;
virtual bool emptySimplex() const ;
virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2) ;
virtual void compute_points(btPoint3& p1, btPoint3& p2) ;
virtual int numVertices() const ;