mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-16 14:40:05 +00:00
removed obsolete files
This commit is contained in:
ejcoumans
parent
6f26aa1591
commit
a424207b73
@ -1,332 +0,0 @@
|
||||
/*
|
||||
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "SpuGjkPairDetector.h"
|
||||
#include "SpuConvexPenetrationDepthSolver.h"
|
||||
#include "SpuLocalSupport.h"
|
||||
|
||||
//#include "BulletCollision/CollisionShapes/btConvexShape.h"
|
||||
|
||||
#if defined(DEBUG) || defined (_DEBUG)
|
||||
#include <stdio.h> //for debug printf
|
||||
#ifdef __SPU__
|
||||
#include <spu_printf.h>
|
||||
#define printf spu_printf
|
||||
#endif //__SPU__
|
||||
#endif
|
||||
|
||||
//must be above the machine epsilon
|
||||
#define REL_ERROR2 float(1.0e-6)
|
||||
|
||||
//temp globals, to improve GJK/EPA/penetration calculations
|
||||
int gSpuNumDeepPenetrationChecks = 0;
|
||||
int gSpuNumGjkChecks = 0;
|
||||
|
||||
|
||||
|
||||
SpuGjkPairDetector::SpuGjkPairDetector(void* objectA,void* objectB,int shapeTypeA, int shapeTypeB, float marginA,float marginB,SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver)
|
||||
:m_cachedSeparatingAxis(float(0.),float(0.),float(1.)),
|
||||
m_penetrationDepthSolver(penetrationDepthSolver),
|
||||
m_simplexSolver(simplexSolver),
|
||||
m_minkowskiA(objectA),
|
||||
m_minkowskiB(objectB),
|
||||
m_shapeTypeA(shapeTypeA),
|
||||
m_shapeTypeB(shapeTypeB),
|
||||
m_marginA(marginA),
|
||||
m_marginB(marginB),
|
||||
m_ignoreMargin(false),
|
||||
m_lastUsedMethod(-1),
|
||||
m_catchDegeneracies(1)
|
||||
{
|
||||
}
|
||||
|
||||
void SpuGjkPairDetector::getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output)
|
||||
{
|
||||
float distance=float(0.);
|
||||
Vectormath::Aos::Vector3 normalInB(float(0.),float(0.),float(0.));
|
||||
Vectormath::Aos::Point3 pointOnA,pointOnB;
|
||||
Vectormath::Aos::Transform3 localTransA = input.m_transformA;
|
||||
Vectormath::Aos::Transform3 localTransB = input.m_transformB;
|
||||
|
||||
// World space coordinate
|
||||
Vectormath::Aos::Vector3 localOriginA = localTransA.getTranslation();
|
||||
Vectormath::Aos::Vector3 localOriginB = localTransB.getTranslation();
|
||||
|
||||
// Average instance position.
|
||||
Vectormath::Aos::Vector3 positionOffset = (localOriginA + localOriginB) * float(0.5);
|
||||
|
||||
// Adjust the instance positions so that they're equidistant from the origin.
|
||||
localTransA.setTranslation(localOriginA - positionOffset);
|
||||
localTransB.setTranslation(localOriginB - positionOffset);
|
||||
|
||||
float marginA = m_marginA;
|
||||
float marginB = m_marginB;
|
||||
|
||||
gSpuNumGjkChecks++;
|
||||
|
||||
//for CCD we don't use margins
|
||||
if (m_ignoreMargin)
|
||||
{
|
||||
marginA = float(0.);
|
||||
marginB = float(0.);
|
||||
}
|
||||
|
||||
m_curIter = 0;
|
||||
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
|
||||
m_cachedSeparatingAxis = Vectormath::Aos::Vector3(0.f,1.f,0.f);
|
||||
|
||||
bool isValid = false;
|
||||
bool checkSimplex = false;
|
||||
bool checkPenetration = true;
|
||||
m_degenerateSimplex = 0;
|
||||
|
||||
m_lastUsedMethod = -1;
|
||||
|
||||
{
|
||||
float squaredDistance = 1e30f;
|
||||
// There's no reason to have this delta declared out here, it's set in the loop before it's used and it's not used outside of the loop.
|
||||
float delta = float(0.);
|
||||
|
||||
float margin = marginA + marginB;
|
||||
|
||||
m_simplexSolver->reset();
|
||||
|
||||
while (true)
|
||||
{
|
||||
// Get the separating axes into each bound's local space.
|
||||
Vectormath::Aos::Vector3 seperatingAxisInA = orthoInverse(input.m_transformA) * (-m_cachedSeparatingAxis);
|
||||
Vectormath::Aos::Vector3 seperatingAxisInB = orthoInverse(input.m_transformB) * m_cachedSeparatingAxis;
|
||||
|
||||
int shapeTypeA = m_shapeTypeA;
|
||||
int shapeTypeB = m_shapeTypeB;
|
||||
// int featureIndexA = 0, featureIndexB = 0; // Feature index basically means vertex index.
|
||||
Vectormath::Aos::Point3 pInA = localGetSupportingVertexWithoutMargin(shapeTypeA, m_minkowskiA, seperatingAxisInA);//, &featureIndexA);
|
||||
Vectormath::Aos::Point3 qInB = localGetSupportingVertexWithoutMargin(shapeTypeB, m_minkowskiB, seperatingAxisInB);//, &featureIndexB);
|
||||
|
||||
// These are in a 'translated' world space where the origin of that world space corresponds to 'positionOffset' in the 'real' world space.
|
||||
Vectormath::Aos::Point3 pWorld = localTransA * pInA;
|
||||
Vectormath::Aos::Point3 qWorld = localTransB * qInB;
|
||||
|
||||
//spu_printf("support point A: %f %f %f\n", pWorld.getX(), pWorld.getY(), pWorld.getZ());
|
||||
//spu_printf("support point B: %f %f %f\n", qWorld.getX(), qWorld.getY(), qWorld.getZ());
|
||||
|
||||
// delta is sort of the distance between the current 'closest points' ...
|
||||
// This seems kind of weird to me since m_cachedSeparatingAxis isn't a unit vector, so I'm not really sure what this signifies.
|
||||
Vectormath::Aos::Vector3 w = pWorld - qWorld;
|
||||
delta = dot(m_cachedSeparatingAxis, w);
|
||||
|
||||
// potential exit, they don't overlap
|
||||
if ((delta > float(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
|
||||
{
|
||||
checkPenetration = false;
|
||||
break;
|
||||
}
|
||||
|
||||
//exit 0: the new point is already in the simplex, or we didn't come any closer
|
||||
if (m_simplexSolver->inSimplex(w))
|
||||
{
|
||||
m_degenerateSimplex = 1;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
// are we getting any closer ?
|
||||
float f0 = squaredDistance - delta;
|
||||
float f1 = squaredDistance * REL_ERROR2;
|
||||
|
||||
// Are we close enough
|
||||
if (f0 <= f1)
|
||||
{
|
||||
if (f0 <= float(0.))
|
||||
{
|
||||
m_degenerateSimplex = 2;
|
||||
}
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
//add current vertex to simplex
|
||||
m_simplexSolver->addVertex(w, pWorld, qWorld);//, featureIndexA, featureIndexB);
|
||||
|
||||
//calculate the closest point to the origin (update vector v)
|
||||
if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
|
||||
{
|
||||
m_degenerateSimplex = 3;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
float previousSquaredDistance = squaredDistance;
|
||||
squaredDistance = lengthSqr(m_cachedSeparatingAxis);
|
||||
|
||||
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
|
||||
//are we getting any closer ?
|
||||
if (previousSquaredDistance - squaredDistance <= FLT_EPSILON * previousSquaredDistance)
|
||||
{
|
||||
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
|
||||
if (m_curIter++ > gGjkMaxIter)
|
||||
{
|
||||
#if defined(DEBUG) || defined (_DEBUG)
|
||||
|
||||
printf("SpuGjkPairDetector maxIter exceeded:%i\n",m_curIter);
|
||||
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
|
||||
m_cachedSeparatingAxis.getX(),
|
||||
m_cachedSeparatingAxis.getY(),
|
||||
m_cachedSeparatingAxis.getZ(),
|
||||
squaredDistance,
|
||||
shapeTypeA,
|
||||
shapeTypeB);
|
||||
|
||||
#endif
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool check = (!m_simplexSolver->fullSimplex());
|
||||
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > FLT_EPSILON * m_simplexSolver->maxVertex());
|
||||
|
||||
if (!check)
|
||||
{
|
||||
//do we need this backup_closest here ?
|
||||
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (checkSimplex)
|
||||
{
|
||||
m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
normalInB = pointOnA-pointOnB;
|
||||
float lenSqr = lengthSqr(m_cachedSeparatingAxis);
|
||||
//valid normal
|
||||
if (lenSqr < 0.0001)
|
||||
{
|
||||
m_degenerateSimplex = 5;
|
||||
}
|
||||
if (lenSqr > FLT_EPSILON*FLT_EPSILON)
|
||||
{
|
||||
float rlen = float(1.) / sqrtf(lenSqr );
|
||||
normalInB *= rlen; //normalize
|
||||
float s = sqrtf(squaredDistance);
|
||||
|
||||
btAssert(s > float(0.0));
|
||||
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
|
||||
pointOnB += m_cachedSeparatingAxis * (marginB / s);
|
||||
distance = ((float(1.)/rlen) - margin);
|
||||
isValid = true;
|
||||
|
||||
m_lastUsedMethod = 1;
|
||||
} else
|
||||
{
|
||||
m_lastUsedMethod = 2;
|
||||
}
|
||||
}
|
||||
|
||||
bool catchDegeneratePenetrationCase =
|
||||
(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
|
||||
|
||||
//if (checkPenetration && !isValid)
|
||||
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
|
||||
{
|
||||
//penetration case
|
||||
|
||||
//if there is no way to handle penetrations, bail out
|
||||
if (m_penetrationDepthSolver)
|
||||
{
|
||||
// Penetration depth case.
|
||||
Vectormath::Aos::Point3 tmpPointOnA,tmpPointOnB;
|
||||
|
||||
// spu_printf("SPU: deep penetration check\n");
|
||||
gSpuNumDeepPenetrationChecks++;
|
||||
|
||||
bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
|
||||
*m_simplexSolver,
|
||||
m_minkowskiA,m_minkowskiB,
|
||||
m_shapeTypeA, m_shapeTypeB,
|
||||
marginA, marginB,
|
||||
localTransA,localTransB,
|
||||
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
|
||||
0,input.m_stackAlloc
|
||||
);
|
||||
|
||||
|
||||
if (isValid2)
|
||||
{
|
||||
Vectormath::Aos::Vector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
|
||||
float lenSqr = lengthSqr(tmpNormalInB);
|
||||
if (lenSqr > (FLT_EPSILON*FLT_EPSILON))
|
||||
{
|
||||
tmpNormalInB /= sqrtf(lenSqr);
|
||||
float distance2 = -dist(tmpPointOnA,tmpPointOnB);
|
||||
//only replace valid penetrations when the result is deeper (check)
|
||||
if (!isValid || (distance2 < distance))
|
||||
{
|
||||
distance = distance2;
|
||||
pointOnA = tmpPointOnA;
|
||||
pointOnB = tmpPointOnB;
|
||||
normalInB = tmpNormalInB;
|
||||
isValid = true;
|
||||
m_lastUsedMethod = 3;
|
||||
} else
|
||||
{
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
//isValid = false;
|
||||
m_lastUsedMethod = 4;
|
||||
}
|
||||
} else
|
||||
{
|
||||
m_lastUsedMethod = 5;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (isValid)
|
||||
{
|
||||
#ifdef __SPU__
|
||||
//spu_printf("distance\n");
|
||||
#endif //__CELLOS_LV2__
|
||||
|
||||
|
||||
Vectormath::Aos::Point3 tmpPtOnB=pointOnB+positionOffset;
|
||||
Vectormath::Aos::Point3 vmPtOnB(tmpPtOnB.getX(),tmpPtOnB.getY(),tmpPtOnB.getZ());
|
||||
Vectormath::Aos::Vector3 vmNormalOnB(normalInB.getX(),normalInB.getY(),normalInB.getZ());
|
||||
|
||||
output.addContactPoint(
|
||||
vmNormalOnB,
|
||||
vmPtOnB,
|
||||
distance
|
||||
);
|
||||
|
||||
|
||||
//printf("gjk add:%f",distance);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1,649 +0,0 @@
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
Elsevier CDROM license agreements grants nonexclusive license to use the software
|
||||
for any purpose, commercial or non-commercial as long as the following credit is included
|
||||
identifying the original source of the software:
|
||||
|
||||
Parts of the source are "from the book Real-Time Collision Detection by
|
||||
Christer Ericson, published by Morgan Kaufmann Publishers,
|
||||
(c) 2005 Elsevier Inc."
|
||||
|
||||
*/
|
||||
|
||||
|
||||
// Needed to be able to DMA.
|
||||
#ifdef WIN32
|
||||
#include "SpuFakeDma.h"
|
||||
#else
|
||||
#include "SPU_Common/SpuDefines.h"
|
||||
#include <cell/spurs/common.h>
|
||||
#include <cell/dma.h>
|
||||
#endif //WIN32
|
||||
|
||||
|
||||
|
||||
#include "SpuVoronoiSimplexSolver.h"
|
||||
#include "LinearMath/btScalar.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
#define VERTA 0
|
||||
#define VERTB 1
|
||||
#define VERTC 2
|
||||
#define VERTD 3
|
||||
|
||||
#define CATCH_DEGENERATE_TETRAHEDRON 1
|
||||
void SpuVoronoiSimplexSolver::removeVertex(int index)
|
||||
{
|
||||
assert(m_numVertices>0);
|
||||
m_numVertices--;
|
||||
m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
|
||||
m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
|
||||
m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
|
||||
// m_VertexIndexA[index] = m_VertexIndexA[m_numVertices];
|
||||
// m_VertexIndexB[index] = m_VertexIndexB[m_numVertices];
|
||||
}
|
||||
|
||||
void SpuVoronoiSimplexSolver::reduceVertices (const SpuUsageBitfield& usedVerts)
|
||||
{
|
||||
if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
|
||||
removeVertex(3);
|
||||
|
||||
if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
|
||||
removeVertex(2);
|
||||
|
||||
if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
|
||||
removeVertex(1);
|
||||
|
||||
if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
|
||||
removeVertex(0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//clear the simplex, remove all the vertices
|
||||
void SpuVoronoiSimplexSolver::reset()
|
||||
{
|
||||
m_cachedValidClosest = false;
|
||||
m_numVertices = 0;
|
||||
m_needsUpdate = true;
|
||||
m_lastW = Vectormath::Aos::Vector3(float(1e30),float(1e30),float(1e30));
|
||||
m_cachedBC.reset();
|
||||
}
|
||||
|
||||
|
||||
|
||||
//add a vertex
|
||||
void SpuVoronoiSimplexSolver::addVertex(const Vectormath::Aos::Vector3& w, const Vectormath::Aos::Point3& p, const Vectormath::Aos::Point3& q)//, int vertexIndexA, int vertexIndexB)
|
||||
{
|
||||
m_lastW = w;
|
||||
m_needsUpdate = true;
|
||||
|
||||
m_simplexVectorW[m_numVertices] = w;
|
||||
m_simplexPointsP[m_numVertices] = Vectormath::Aos::Vector3(p);
|
||||
m_simplexPointsQ[m_numVertices] = Vectormath::Aos::Vector3(q);
|
||||
|
||||
//m_VertexIndexA[m_numVertices] = vertexIndexA;
|
||||
//m_VertexIndexB[m_numVertices] = vertexIndexB;
|
||||
|
||||
m_numVertices++;
|
||||
}
|
||||
|
||||
bool SpuVoronoiSimplexSolver::updateClosestVectorAndPoints()
|
||||
{
|
||||
|
||||
if (m_needsUpdate)
|
||||
{
|
||||
m_cachedBC.reset();
|
||||
|
||||
m_needsUpdate = false;
|
||||
|
||||
switch (numVertices())
|
||||
{
|
||||
case 0:
|
||||
m_cachedValidClosest = false;
|
||||
break;
|
||||
case 1:
|
||||
{
|
||||
m_cachedP1 = m_simplexPointsP[0];
|
||||
m_cachedP2 = m_simplexPointsQ[0];
|
||||
m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
|
||||
m_cachedBC.reset();
|
||||
m_cachedBC.setBarycentricCoordinates(float(1.),float(0.),float(0.),float(0.));
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
break;
|
||||
};
|
||||
case 2:
|
||||
{
|
||||
//closest point origin from line segment
|
||||
const Vectormath::Aos::Vector3& from = m_simplexVectorW[0];
|
||||
const Vectormath::Aos::Vector3& to = m_simplexVectorW[1];
|
||||
Vectormath::Aos::Vector3 nearest;
|
||||
|
||||
Vectormath::Aos::Vector3 p (float(0.),float(0.),float(0.));
|
||||
Vectormath::Aos::Vector3 diff = p - from;
|
||||
Vectormath::Aos::Vector3 v = to - from;
|
||||
float t = dot(v, diff);
|
||||
|
||||
if (t > 0) {
|
||||
float dotVV = dot(v, v);
|
||||
if (t < dotVV) {
|
||||
t /= dotVV;
|
||||
diff -= t*v;
|
||||
m_cachedBC.m_usedVertices.usedVertexA = true;
|
||||
m_cachedBC.m_usedVertices.usedVertexB = true;
|
||||
} else {
|
||||
t = 1;
|
||||
diff -= v;
|
||||
//reduce to 1 point
|
||||
m_cachedBC.m_usedVertices.usedVertexB = true;
|
||||
}
|
||||
} else
|
||||
{
|
||||
t = 0;
|
||||
//reduce to 1 point
|
||||
m_cachedBC.m_usedVertices.usedVertexA = true;
|
||||
}
|
||||
m_cachedBC.setBarycentricCoordinates(1-t,t);
|
||||
nearest = from + t*v;
|
||||
|
||||
m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
|
||||
m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
|
||||
m_cachedV = m_cachedP1 - m_cachedP2;
|
||||
|
||||
reduceVertices(m_cachedBC.m_usedVertices);
|
||||
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
break;
|
||||
}
|
||||
case 3:
|
||||
{
|
||||
//closest point origin from triangle
|
||||
Vectormath::Aos::Vector3 p (float(0.),float(0.),float(0.));
|
||||
|
||||
const Vectormath::Aos::Vector3& a = m_simplexVectorW[0];
|
||||
const Vectormath::Aos::Vector3& b = m_simplexVectorW[1];
|
||||
const Vectormath::Aos::Vector3& c = m_simplexVectorW[2];
|
||||
|
||||
closestPtPointTriangle(p,a,b,c,m_cachedBC);
|
||||
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];
|
||||
|
||||
m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];
|
||||
|
||||
m_cachedV = m_cachedP1-m_cachedP2;
|
||||
|
||||
reduceVertices (m_cachedBC.m_usedVertices);
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
{
|
||||
|
||||
|
||||
Vectormath::Aos::Vector3 p (float(0.),float(0.),float(0.));
|
||||
|
||||
const Vectormath::Aos::Vector3& a = m_simplexVectorW[0];
|
||||
const Vectormath::Aos::Vector3& b = m_simplexVectorW[1];
|
||||
const Vectormath::Aos::Vector3& c = m_simplexVectorW[2];
|
||||
const Vectormath::Aos::Vector3& d = m_simplexVectorW[3];
|
||||
|
||||
bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
|
||||
|
||||
if (hasSeperation)
|
||||
{
|
||||
|
||||
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
|
||||
m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
|
||||
|
||||
m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
|
||||
m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
|
||||
|
||||
m_cachedV = m_cachedP1-m_cachedP2;
|
||||
reduceVertices (m_cachedBC.m_usedVertices);
|
||||
} else
|
||||
{
|
||||
// printf("sub distance got penetration\n");
|
||||
|
||||
if (m_cachedBC.m_degenerate)
|
||||
{
|
||||
m_cachedValidClosest = false;
|
||||
} else
|
||||
{
|
||||
m_cachedValidClosest = true;
|
||||
//degenerate case == false, penetration = true + zero
|
||||
m_cachedV = Vectormath::Aos::Vector3(float(0.),float(0.),float(0.));
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
|
||||
//closest point origin from tetrahedron
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
m_cachedValidClosest = false;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
return m_cachedValidClosest;
|
||||
|
||||
}
|
||||
|
||||
//return/calculate the closest vertex
|
||||
bool SpuVoronoiSimplexSolver::closest(Vectormath::Aos::Vector3& v)
|
||||
{
|
||||
bool succes = updateClosestVectorAndPoints();
|
||||
v = m_cachedV;
|
||||
return succes;
|
||||
}
|
||||
|
||||
|
||||
|
||||
float SpuVoronoiSimplexSolver::maxVertex()
|
||||
{
|
||||
int i, numverts = numVertices();
|
||||
float maxV = float(0.);
|
||||
for (i=0;i<numverts;i++)
|
||||
{
|
||||
float curLen2 = lengthSqr(m_simplexVectorW[i]);
|
||||
if (maxV < curLen2)
|
||||
maxV = curLen2;
|
||||
}
|
||||
return maxV;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//return the current simplex
|
||||
int SpuVoronoiSimplexSolver::getSimplex(Vectormath::Aos::Vector3 *pBuf, Vectormath::Aos::Vector3 *qBuf, Vectormath::Aos::Vector3 *yBuf) const
|
||||
{
|
||||
int i;
|
||||
for (i=0;i<numVertices();i++)
|
||||
{
|
||||
yBuf[i] = m_simplexVectorW[i];
|
||||
pBuf[i] = m_simplexPointsP[i];
|
||||
qBuf[i] = m_simplexPointsQ[i];
|
||||
}
|
||||
return numVertices();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
bool SpuVoronoiSimplexSolver::inSimplex(const Vectormath::Aos::Vector3& w)
|
||||
{
|
||||
bool found = false;
|
||||
int i, numverts = numVertices();
|
||||
//float maxV = float(0.);
|
||||
|
||||
//w is in the current (reduced) simplex
|
||||
for (i=0;i<numverts;i++)
|
||||
{
|
||||
// TODO: find a better way to determine equality
|
||||
if (m_simplexVectorW[i].getX() == w.getX() &&
|
||||
m_simplexVectorW[i].getY() == w.getY() &&
|
||||
m_simplexVectorW[i].getZ() == w.getZ())
|
||||
found = true;
|
||||
}
|
||||
|
||||
//check in case lastW is already removed
|
||||
// TODO: find a better way to determine equality
|
||||
if (w.getX() == m_lastW.getX() &&
|
||||
w.getY() == m_lastW.getY() &&
|
||||
w.getZ() == m_lastW.getZ())
|
||||
return true;
|
||||
|
||||
return found;
|
||||
}
|
||||
|
||||
void SpuVoronoiSimplexSolver::backup_closest(Vectormath::Aos::Vector3& v)
|
||||
{
|
||||
v = m_cachedV;
|
||||
}
|
||||
|
||||
|
||||
bool SpuVoronoiSimplexSolver::emptySimplex() const
|
||||
{
|
||||
return (numVertices() == 0);
|
||||
|
||||
}
|
||||
|
||||
void SpuVoronoiSimplexSolver::compute_points(Vectormath::Aos::Point3& p1, Vectormath::Aos::Point3& p2)
|
||||
{
|
||||
updateClosestVectorAndPoints();
|
||||
p1 = Vectormath::Aos::Point3(m_cachedP1);
|
||||
p2 = Vectormath::Aos::Point3(m_cachedP2);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
bool SpuVoronoiSimplexSolver::closestPtPointTriangle(const Vectormath::Aos::Vector3& p, const Vectormath::Aos::Vector3& a, const Vectormath::Aos::Vector3& b, const Vectormath::Aos::Vector3& c,SpuSubSimplexClosestResult& result)
|
||||
{
|
||||
result.m_usedVertices.reset();
|
||||
|
||||
// Check if P in vertex region outside A
|
||||
Vectormath::Aos::Vector3 ab = b - a;
|
||||
Vectormath::Aos::Vector3 ac = c - a;
|
||||
Vectormath::Aos::Vector3 ap = p - a;
|
||||
float d1 = dot(ab,ap);
|
||||
float d2 = dot(ac,ap);
|
||||
if (d1 <= float(0.0) && d2 <= float(0.0))
|
||||
{
|
||||
result.m_closestPointOnSimplex = Vectormath::Aos::Point3(a);
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.setBarycentricCoordinates(1,0,0);
|
||||
return true;// a; // barycentric coordinates (1,0,0)
|
||||
}
|
||||
|
||||
// Check if P in vertex region outside B
|
||||
Vectormath::Aos::Vector3 bp = p - b;
|
||||
float d3 = dot(ab,bp);
|
||||
float d4 = dot(ac,bp);
|
||||
if (d3 >= float(0.0) && d4 <= d3)
|
||||
{
|
||||
result.m_closestPointOnSimplex = Vectormath::Aos::Point3(b);
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.setBarycentricCoordinates(0,1,0);
|
||||
|
||||
return true; // b; // barycentric coordinates (0,1,0)
|
||||
}
|
||||
// Check if P in edge region of AB, if so return projection of P onto AB
|
||||
float vc = d1*d4 - d3*d2;
|
||||
if (vc <= float(0.0) && d1 >= float(0.0) && d3 <= float(0.0)) {
|
||||
float v = d1 / (d1 - d3);
|
||||
result.m_closestPointOnSimplex = Vectormath::Aos::Point3(a + v * ab);
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.setBarycentricCoordinates(1-v,v,0);
|
||||
return true;
|
||||
//return a + v * ab; // barycentric coordinates (1-v,v,0)
|
||||
}
|
||||
|
||||
// Check if P in vertex region outside C
|
||||
Vectormath::Aos::Vector3 cp = p - c;
|
||||
float d5 = dot(ab,cp);
|
||||
float d6 = dot(ac,cp);
|
||||
if (d6 >= float(0.0) && d5 <= d6)
|
||||
{
|
||||
result.m_closestPointOnSimplex = Vectormath::Aos::Point3(c);
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(0,0,1);
|
||||
return true;//c; // barycentric coordinates (0,0,1)
|
||||
}
|
||||
|
||||
// Check if P in edge region of AC, if so return projection of P onto AC
|
||||
float vb = d5*d2 - d1*d6;
|
||||
if (vb <= float(0.0) && d2 >= float(0.0) && d6 <= float(0.0)) {
|
||||
float w = d2 / (d2 - d6);
|
||||
result.m_closestPointOnSimplex = Vectormath::Aos::Point3(a + w * ac);
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(1-w,0,w);
|
||||
return true;
|
||||
//return a + w * ac; // barycentric coordinates (1-w,0,w)
|
||||
}
|
||||
|
||||
// Check if P in edge region of BC, if so return projection of P onto BC
|
||||
float va = d3*d6 - d5*d4;
|
||||
if (va <= float(0.0) && (d4 - d3) >= float(0.0) && (d5 - d6) >= float(0.0)) {
|
||||
float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
|
||||
|
||||
result.m_closestPointOnSimplex = Vectormath::Aos::Point3(b + w * (c - b));
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(0,1-w,w);
|
||||
return true;
|
||||
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
|
||||
}
|
||||
|
||||
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
|
||||
float denom = float(1.0) / (va + vb + vc);
|
||||
float v = vb * denom;
|
||||
float w = vc * denom;
|
||||
|
||||
result.m_closestPointOnSimplex = Vectormath::Aos::Point3(a + ab * v + ac * w);
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(1-v-w,v,w);
|
||||
|
||||
return true;
|
||||
// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = float(1.0) - v - w
|
||||
}
|
||||
|
||||
|
||||
// This is specifically just removing duplicate indices.
|
||||
int SpuVoronoiSimplexSolver::RemoveDegenerateIndices (const int* inArray, int numIndices, int* outArray) const
|
||||
{
|
||||
int outIndex = 0;
|
||||
for (int firstIndex=0; firstIndex<numIndices; firstIndex++)
|
||||
{
|
||||
bool duplicate = false;
|
||||
for (int secondIndex=0; secondIndex<firstIndex; secondIndex++)
|
||||
{
|
||||
if (inArray[secondIndex]==inArray[firstIndex])
|
||||
{
|
||||
duplicate = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!duplicate)
|
||||
{
|
||||
outArray[outIndex++] = inArray[firstIndex];
|
||||
}
|
||||
}
|
||||
|
||||
return outIndex;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/// Test if point p and d lie on opposite sides of plane through abc
|
||||
int SpuVoronoiSimplexSolver::pointOutsideOfPlane(const Vectormath::Aos::Vector3& p, const Vectormath::Aos::Vector3& a, const Vectormath::Aos::Vector3& b, const Vectormath::Aos::Vector3& c, const Vectormath::Aos::Vector3& d)
|
||||
{
|
||||
Vectormath::Aos::Vector3 normal = cross(b-a,c-a);
|
||||
|
||||
float signp = dot(p - a, normal); // [AP AB AC]
|
||||
float signd = dot(d - a, normal); // [AD AB AC]
|
||||
|
||||
#ifdef CATCH_DEGENERATE_TETRAHEDRON
|
||||
if (signd * signd < (float(1e-4) * float(1e-4)))
|
||||
{
|
||||
// printf("affine dependent/degenerate\n");//
|
||||
return -1;
|
||||
}
|
||||
#endif
|
||||
// Points on opposite sides if expression signs are opposite
|
||||
return signp * signd < float(0.);
|
||||
}
|
||||
|
||||
|
||||
bool SpuVoronoiSimplexSolver::closestPtPointTetrahedron(const Vectormath::Aos::Vector3& p, const Vectormath::Aos::Vector3& a, const Vectormath::Aos::Vector3& b, const Vectormath::Aos::Vector3& c, const Vectormath::Aos::Vector3& d, SpuSubSimplexClosestResult& finalResult)
|
||||
{
|
||||
SpuSubSimplexClosestResult tempResult;
|
||||
|
||||
// Start out assuming point inside all halfspaces, so closest to itself
|
||||
finalResult.m_closestPointOnSimplex = Vectormath::Aos::Point3(p);
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexA = true;
|
||||
finalResult.m_usedVertices.usedVertexB = true;
|
||||
finalResult.m_usedVertices.usedVertexC = true;
|
||||
finalResult.m_usedVertices.usedVertexD = true;
|
||||
|
||||
// Check only the tetrahedron faces that are closest to p. We do that by checking each face (which itself is a triangle) to see if the excluded
|
||||
// vertex (the one vertex that isn't part of that face) is on the other side of that face from the point p.
|
||||
int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
|
||||
int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
|
||||
int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
|
||||
int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
|
||||
|
||||
if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
|
||||
{
|
||||
finalResult.m_degenerate = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
float bestSqDist = 1e30f;//FLT_MAX;
|
||||
// If point outside face abc then compute closest point on abc
|
||||
if (pointOutsideABC)
|
||||
{
|
||||
closestPtPointTriangle(p, a, b, c,tempResult);
|
||||
Vectormath::Aos::Vector3 q = Vectormath::Aos::Vector3(tempResult.m_closestPointOnSimplex);
|
||||
|
||||
float sqDist = dot(q - p, q - p);
|
||||
// Update best closest point if (squared) distance is less than current best
|
||||
//if (sqDist < bestSqDist)
|
||||
btAssert(sqDist < bestSqDist); // This has to be true; we haven't actually tested any other combinations.
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = Vectormath::Aos::Point3(q);
|
||||
//convert result bitmask!
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
|
||||
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
|
||||
finalResult.setBarycentricCoordinates(
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTB],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
0
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Repeat test for face acd
|
||||
if (pointOutsideACD)
|
||||
{
|
||||
closestPtPointTriangle(p, a, c, d,tempResult);
|
||||
Vectormath::Aos::Vector3 q = Vectormath::Aos::Vector3(tempResult.m_closestPointOnSimplex);
|
||||
//convert result bitmask!
|
||||
|
||||
float sqDist = dot(q - p, q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = Vectormath::Aos::Point3(q);
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
|
||||
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
|
||||
finalResult.setBarycentricCoordinates(
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTB],
|
||||
tempResult.m_barycentricCoords[VERTC]
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
// Repeat test for face adb
|
||||
|
||||
|
||||
if (pointOutsideADB)
|
||||
{
|
||||
closestPtPointTriangle(p, a, d, b,tempResult);
|
||||
Vectormath::Aos::Vector3 q = Vectormath::Aos::Vector3(tempResult.m_closestPointOnSimplex);
|
||||
//convert result bitmask!
|
||||
|
||||
float sqDist = dot(q - p, q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = Vectormath::Aos::Point3(q);
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
|
||||
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
|
||||
finalResult.setBarycentricCoordinates(
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTB]
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
// Repeat test for face bdc
|
||||
|
||||
|
||||
if (pointOutsideBDC)
|
||||
{
|
||||
closestPtPointTriangle(p, b, d, c,tempResult);
|
||||
Vectormath::Aos::Vector3 q = Vectormath::Aos::Vector3(tempResult.m_closestPointOnSimplex);
|
||||
//convert result bitmask!
|
||||
float sqDist = dot(q - p, q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = Vectormath::Aos::Point3(q);
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
|
||||
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
|
||||
|
||||
finalResult.setBarycentricCoordinates(
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
tempResult.m_barycentricCoords[VERTB]
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//help! we ended up full !
|
||||
|
||||
if (finalResult.m_usedVertices.usedVertexA &&
|
||||
finalResult.m_usedVertices.usedVertexB &&
|
||||
finalResult.m_usedVertices.usedVertexC &&
|
||||
finalResult.m_usedVertices.usedVertexD)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user