From 96134ee8964efc91be68bcb32f87f4869b1578b4 Mon Sep 17 00:00:00 2001
From: Tobias Leemann <tobias.leemann@fau.de>
Date: Fri, 17 Nov 2017 19:46:58 +0100
Subject: [PATCH] Removed implementation from header
---
src/pe/collision/EPA.h | 929 +-----------------------------------
src/pe/collision/GJK.h | 1028 +---------------------------------------
2 files changed, 25 insertions(+), 1932 deletions(-)
diff --git a/src/pe/collision/EPA.h b/src/pe/collision/EPA.h
index c73183941..da611ecae 100644
--- a/src/pe/collision/EPA.h
+++ b/src/pe/collision/EPA.h
@@ -73,18 +73,17 @@ public:
//**Query functions*****************************************************************************
/*!\name Query functions */
//@{
- inline bool doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ bool doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
Vec3& contactPoint, real_t& penetrationDepth);
- inline bool doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ bool doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
Vec3& contactPoint, real_t& penetrationDepth, real_t eps_rel);
-
- inline bool doEPAmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ bool doEPAmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
Vec3& contactPoint, real_t& penetrationDepth, real_t margin);
- inline bool doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ bool doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
Vec3& contactPoint, real_t& penetrationDepth, real_t margin, real_t eps_rel );
//@}
@@ -122,14 +121,14 @@ private:
const Vec3& D );
inline bool pointInTetrahedron ( const Vec3& A, const Vec3& B, const Vec3& C,
const Vec3& D, const Vec3& point );
- inline bool searchTetrahedron (GeomPrimitive &geom1, GeomPrimitive &geom2, std::vector<Vec3>& epaVolume,
+ bool searchTetrahedron (GeomPrimitive &geom1, GeomPrimitive &geom2, std::vector<Vec3>& epaVolume,
std::vector<Vec3>& supportA, std::vector<Vec3>& supportB, EPA_EntryBuffer& entryBuffer, real_t margin );
- inline void createInitialTetrahedron ( size_t top, size_t frontLeft, size_t frontRight,
+ void createInitialTetrahedron ( size_t top, size_t frontLeft, size_t frontRight,
size_t back, std::vector<Vec3>& epaVolume,
EPA_EntryBuffer& entryBuffer );
- inline void createInitialSimplex ( size_t numPoints, GeomPrimitive &geom1, GeomPrimitive &geom2,
+ void createInitialSimplex ( size_t numPoints, GeomPrimitive &geom1, GeomPrimitive &geom2,
std::vector<Vec3>& supportA,
std::vector<Vec3>& supportB,
std::vector<Vec3>& epaVolume,
@@ -157,7 +156,7 @@ private:
//=================================================================================================
//*************************************************************************************************
-/*!\brief TODO
+/*!\brief Class storing Information about an Edge of the EPA-Polytope
*/
class EPA::EPA_Edge {
public:
@@ -199,7 +198,7 @@ private:
//=================================================================================================
//*************************************************************************************************
-/*!\brief TODO
+/*!\brief Class storing Information about a triangular facette (Triangle) of the EPA-Polytope
*
* see Collision detction in interactiv 3D environments; Gino van den bergen page 155
*/
@@ -232,12 +231,12 @@ public:
inline void silhouette( const Vec3& w, EPA_EdgeBuffer& edgeBuffer );
//@}
//**********************************************************************************************
- inline bool containsPoint(const std::vector<Vec3> &epaVolume);
+
private:
//**Utility functions***************************************************************************
/*!\name Utility functions */
//@{
- inline void silhouette( size_t index, const Vec3& w, EPA_EdgeBuffer& edgeBuffer );
+ void silhouette( size_t index, const Vec3& w, EPA_EdgeBuffer& edgeBuffer );
//@}
//**********************************************************************************************
@@ -260,8 +259,6 @@ private:
//*************************************************************************************************
-
-
//=================================================================================================
//
// EPA::EPA_TRIANGLECOMP CLASS DEFINITION
@@ -284,8 +281,6 @@ public:
//*************************************************************************************************
-
-
//=================================================================================================
//
// EPA_EDGE CONSTRUCTOR
@@ -313,8 +308,7 @@ inline EPA::EPA_Edge::EPA_Edge( EPA_Triangle* triangle, size_t index )
//=================================================================================================
//*************************************************************************************************
-/*! \brief
- * Return the triangle this edge belongs to.
+/*! \brief Return the triangle this edge belongs to.
*/
inline EPA::EPA_Triangle* EPA::EPA_Edge::getTriangle() const
{
@@ -324,8 +318,7 @@ inline EPA::EPA_Triangle* EPA::EPA_Edge::getTriangle() const
//*************************************************************************************************
-/*! \brief
- * Get the Index of this edge in its triangle.
+/*! \brief Get the Index of this edge in its triangle.
*/
inline size_t EPA::EPA_Edge::getIndex() const
{
@@ -358,68 +351,6 @@ inline size_t EPA::EPA_Edge::getEnd() const
-//=================================================================================================
-//
-// EPA::EPA_TRIANGLE CONSTRUCTOR
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*! \brief Construct a new EPA_Triangle.
- * \param a First point index
- * \param b Second point index
- * \param c Third point index
- * \param points Vector with all points
- */
-inline EPA::EPA_Triangle::EPA_Triangle( size_t a, size_t b, size_t c,
- const std::vector<Vec3>& points )
-{
- WALBERLA_ASSERT_UNEQUAL( a, b, "EPA_Triangle impoosible indices"); //TODO kein assert im constructor
- WALBERLA_ASSERT_UNEQUAL( b, c, "EPA_Triangle impoosible indices"); //TODO kein assert im constructor
- WALBERLA_ASSERT_UNEQUAL( c, a, "EPA_Triangle impoosible indices"); //TODO kein assert im constructor
-
- const Vec3& A = points[a];
- const Vec3& B = points[b];
- const Vec3& C = points[c];
-
- indices_[0] = a;
- indices_[1] = b;
- indices_[2] = c;
-
- //calculate the closest point to the origin
- //Real-Time Collsion Buch Seite 137
- Vec3 ab = B-A;
- Vec3 ac = C-A;
- //Vec3 bc = C-B;
-
- normal_ = ab % ac;
- Vec3 nT = normal_;
-
- //
- real_t vc = nT * (A % B);
- real_t va = nT * (B % C);
- real_t vb = nT * (C % A);
- real_t denom = real_t(1.0) / (va + vb + vc);
-
- bar_[0] = va * denom;
- bar_[1] = vb * denom;
- bar_[2] = real_t(1.0) - bar_[0] - bar_[1];
-
- closest_ = bar_[0] * A + bar_[1] * B + bar_[2] * C;
-
- //sqrDist=key is square distance of v to origin
- sqrDist_ = closest_.sqrLength();
-
- //adjoined triangles not set yet
- adjTriangle_[0] = adjTriangle_[1] = adjTriangle_[2] = NULL;
- adjEdges_[0] = adjEdges_[1] = adjEdges_[2] = 4;
-
- obsolete_ = false;
-}
-
-
-
-
//=================================================================================================
//
// EPA::EPA_TRIANGLE GET FUNCTIONS
@@ -502,82 +433,6 @@ inline bool EPA::EPA_Triangle::isClosestInternal() const
//*************************************************************************************************
-//=================================================================================================
-//
-// EPA::EPA_TRIANGLE UTILITY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*! \brief Sets the link of this triangles edge0 neighbor to tria and vice versa.
- */
-inline bool EPA::EPA_Triangle::link( size_t edge0, EPA_Triangle* tria, size_t edge1 )
-{
- WALBERLA_ASSERT_LESS(edge0, 3, "link: invalid edge index");
- WALBERLA_ASSERT_LESS(edge1, 3, "link: invalid edge index");
-
- adjTriangle_[edge0] = tria;
- adjEdges_[edge0] = edge1;
- tria->adjTriangle_[edge1] = this;
- tria->adjEdges_[edge1] = edge0;
- /*
- size_t v1 = indices_[edge0] ;
- size_t v2 = tria->indices_[(edge1+1)%3];
- size_t v3 = indices_[(edge0+1)%3];
- size_t v4 = tria->indices_[edge1];
-
- if( !(v1==v2 && v3==v4)) {
- std::cout << "kaputt" << std::endl;
- }
- */
-
- bool b = indices_[edge0] == tria->indices_[(edge1+1)%3] &&
- indices_[(edge0+1)%3] == tria->indices_[edge1];
- return b;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Fills edgeBuffer with the CCW contour of triangles not seen from point w which is in normal direction of the triangle.
- */
-inline void EPA::EPA_Triangle::silhouette( const Vec3& w, EPA_EdgeBuffer& edgeBuffer )
-{
- //std::cerr << "Starting Silhoutette search on Triangle {" << indices_[0] << "," << indices_[1] << "," << indices_[2] << "}" << std::endl;
- edgeBuffer.clear();
- obsolete_ = true;
-
- adjTriangle_[0]->silhouette(adjEdges_[0], w, edgeBuffer);
- adjTriangle_[1]->silhouette(adjEdges_[1], w, edgeBuffer);
- adjTriangle_[2]->silhouette(adjEdges_[2], w, edgeBuffer);
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Recursive silhuette finding method.
- */
-inline void EPA::EPA_Triangle::silhouette( size_t index, const Vec3& w,
- EPA_EdgeBuffer& edgeBuffer )
-{
- if (!obsolete_) {
- real_t test = (closest_ * w);
- if (test < sqrDist_) {
- edgeBuffer.push_back(EPA_Edge(this, index));
- }
- else {
- obsolete_ = true; // Facet is visible
- size_t next = (index+1) % 3;
- adjTriangle_[next]->silhouette(adjEdges_[next], w, edgeBuffer);
- next = (next+1) % 3;
- adjTriangle_[next]->silhouette(adjEdges_[next], w, edgeBuffer);
- }
- }
-}
-//*************************************************************************************************
-
-
-
//=================================================================================================
//
@@ -586,7 +441,7 @@ inline void EPA::EPA_Triangle::silhouette( size_t index, const Vec3& w,
//=================================================================================================
//*************************************************************************************************
-/*!\brief TODO
+/*!\brief Compare two triangles by their distance.
*/
inline bool EPA::EPA_TriangleComp::operator()( const EPA_Triangle *tria1,
const EPA_Triangle *tria2 )
@@ -596,341 +451,6 @@ inline bool EPA::EPA_TriangleComp::operator()( const EPA_Triangle *tria1,
//*************************************************************************************************
-/*inline void printHeap(std::vector<EPA::EPA_Triangle*> &entryHeap){
- std::cerr << "Heap size: " << entryHeap.size();
- for(int i = 0; i< entryHeap.size(); i++){
- std::cerr << "[" << i << "]: " << entryHeap[i]->getSqrDist() <<std::endl;
- }
-}*/
-
-inline bool EPA::EPA_Triangle::containsPoint(const std::vector<Vec3> &epaVolume){
- Vec3 check_point(real_t(0.0265539),real_t(0.458987),real_t(0.888046));
- for(int i = 0; i < 3; i++){
- Vec3 A = epaVolume[indices_[i]];
- Vec3 B = epaVolume[indices_[(i+1)%3]];
- if(((A % B) * check_point )< real_t(0.0)){
- return false;
- }
- }
- return true;
-}
-
-template< class T > struct EpsilonRelEPA;
-template<> struct EpsilonRelEPA< float > { static const float value; };
-template<> struct EpsilonRelEPA< double > { static const double value; };
-template<> struct EpsilonRelEPA< long double > { static const long double value; };
-
-const float EpsilonRelEPA< float >::value = static_cast< float >(1e-4);
-const double EpsilonRelEPA< double >::value = static_cast< double >(1e-6);
-const long double EpsilonRelEPA< long double >::value = static_cast< long double >(1e-6);
-
-//=================================================================================================
-//
-// EPA QUERY FUNCTIONS
-//
-//=================================================================================================
-
-
-
-//*************************************************************************************************
-/*! \brief Does an EPA computation with contactthreshold added. Use Default relative Error.
- */
-inline bool EPA::doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& retNormal,
- Vec3& contactPoint, real_t& penetrationDepth){
-
- //Default relative epsilon
- return doEPA(geom1, geom2, gjk, retNormal, contactPoint, penetrationDepth, contactThreshold, EpsilonRelEPA<real_t>::value);
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Does an EPA computation with contactThreshold added. Relative Error can be specified.
- */
-inline bool EPA::doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& retNormal,
- Vec3& contactPoint, real_t& penetrationDepth, real_t eps_rel){
-
- return doEPA(geom1, geom2, gjk, retNormal, contactPoint, penetrationDepth, contactThreshold, eps_rel);
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Does an EPA computation with margin added. Use Default relative Error.
- */
-inline bool EPA::doEPAmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& retNormal,
- Vec3& contactPoint, real_t& penetrationDepth, real_t margin){
- //Default relative epsilon
- return doEPA(geom1, geom2, gjk, retNormal, contactPoint, penetrationDepth, margin, EpsilonRelEPA<real_t>::value);
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Does an epa computation with contact margin added and specified realtive error.
- */
-inline bool EPA::doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& retNormal,
- Vec3& contactPoint, real_t& penetrationDepth, real_t margin, real_t eps_rel )
-{
- //have in mind that we use a support mapping which blows up the objects a wee bit so
- //zero penetraion aka toching contact means that the original bodies have a distance of 2*margin between them
-
- //Set references to the results of GJK
- size_t numPoints( static_cast<size_t>( gjk.getSimplexSize() ) );
- std::vector<Vec3> epaVolume( gjk.getSimplex() );
- std::vector<Vec3> supportA ( gjk.getSupportA() );
- std::vector<Vec3> supportB ( gjk.getSupportB() );
-
- Vec3 support;
-
- epaVolume.reserve( maxSupportPoints_ );
- supportA.reserve ( maxSupportPoints_ );
- supportB.reserve ( maxSupportPoints_ );
-
- EPA_EntryBuffer entryBuffer;
- entryBuffer.reserve(maxTriangles_);
-
- EPA_EntryHeap entryHeap;
- entryHeap.reserve(maxTriangles_);
-
- EPA_EdgeBuffer edgeBuffer;
- edgeBuffer.reserve(20);
-
- real_t lowerBoundSqr = math::Limits<real_t>::inf();
- real_t upperBoundSqr = math::Limits<real_t>::inf();
-
- //create an Initial simplex
- if(numPoints == 1) {
- //If the GJK-Simplex contains only one point, it must be the origin and it must be on the boundary of the CSO.
- //This means the enlarged bodies are in touching contact and the original bodies do not intersect.
- return false;
- }
- else {
- createInitialSimplex(numPoints, geom1, geom2, supportA, supportB, epaVolume, entryBuffer, margin);
- }
-
- for(EPA_EntryBuffer::iterator it=entryBuffer.begin(); it != entryBuffer.end(); ++it) {
- if(it->isClosestInternal()) {
- entryHeap.push_back(&(*it));
- }
- }
-
- if(entryHeap.size() == 0) {
- //unrecoverable error.
- return false;
- }
-
- std::make_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
- EPA_Triangle* current = NULL;
-
- //EPA Main-Loop
- do {
- std::pop_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
- current = entryHeap.back();
- entryHeap.pop_back();
- if(!current->isObsolete()) {
- WALBERLA_ASSERT_GREATER(current->getSqrDist(), real_t(0.0), "EPA_Trianalge distance is negative.");
- lowerBoundSqr = current->getSqrDist();
-
- if(epaVolume.size() == maxSupportPoints_) {
- WALBERLA_ASSERT(false, "Support point limit reached.");
- break;
- }
-
- // Compute new support direction
- // if origin is contained in plane, use out-facing normal.
- Vec3 normal;
- if(current->getSqrDist() < real_comparison::Epsilon<real_t>::value*real_comparison::Epsilon<real_t>::value){
- normal = current->getNormal().getNormalized();
- }else{
- normal = current->getClosest().getNormalized();
- }
- //std::cerr << "Current Closest: " << current->getClosest();
- //std::cerr << "New support direction: " << normal << std::endl;
-
- pushSupportMargin(geom1, geom2, normal, margin, epaVolume, supportA, supportB);
- support = epaVolume.back();
-
- numPoints++;
-
- real_t farDist = support * normal; //not yet squared
-
- WALBERLA_ASSERT_GREATER(farDist, real_t(0.0), "EPA support mapping gave invalid point in expansion direction");
- //std::cerr << "New upper bound: " << farDist*farDist << std::endl;
- upperBoundSqr = std::min(upperBoundSqr, farDist*farDist);
-
- //Try to approximate the new surface with a sphere
- Vec3 ctr;
- real_t radius2 = calculateCircle(support, epaVolume[(*current)[0]],
- epaVolume[(*current)[1]], epaVolume[(*current)[2]], ctr);
- if(radius2 > real_t(0.0)){ //if a Sphere exists
- //std::cerr << "Circle created with center at " << ctr << ". r2=" << radius2 << std::endl;
- real_t center_len = ctr.length();
- real_t circle_dist = (std::sqrt(radius2) - center_len); //Distance from center to the spheres surface
- //Check if the circle matches the bounds given by EPA and limit max error to ca. 5%
- if(circle_dist*circle_dist <= upperBoundSqr && circle_dist*circle_dist >= lowerBoundSqr &&
- (circle_dist*circle_dist)/lowerBoundSqr < real_t(1.10) && !floatIsEqual(center_len, real_t(0.0))) {
-
- ctr = -1*ctr.getNormalized();
- //std::cerr << "New support direction: " << ctr << std::endl;
- pushSupportMargin(geom1, geom2, ctr, margin, epaVolume, supportA, supportB);
- support = epaVolume.back();
- // Check if support is in expected direction
-
- if(floatIsEqual((support % ctr).sqrLength()/support.sqrLength(), real_t(0.0))){ //Accept sphere
-
- contactPoint = real_t(0.5) * (supportA.back() + supportB.back());
- penetrationDepth = -support.length()+ real_t(2.0) * margin;
- retNormal = -ctr;
- //std::cerr << "Found penetration depth " << penetrationDepth << " with CurvedEPA!" << std::endl;
- if(penetrationDepth < contactThreshold){
- return true;
- }else{
- return false;
- }
- } else { //Reject sphere
- removeSupportMargin(epaVolume, supportA, supportB);
- support = epaVolume.back();
- }
- }
- }
-
- //terminating criteria's
- //- we found that the two bounds are close enough
- //- the added support point was already in the epaVolume
- if(upperBoundSqr <= (real_t(1.0)+eps_rel)*(real_t(1.0)+eps_rel)*lowerBoundSqr
- || support == epaVolume[(*current)[0]]
- || support == epaVolume[(*current)[1]]
- || support == epaVolume[(*current)[2]])
- {
- //std::cerr << "Tolerance reached." << std::endl;
- break;
- }
-
- // Compute the silhouette cast by the new vertex
- // Note that the new vertex is on the positive side
- // of the current triangle, so the current triangle
- // will not be in the convex hull. Start local search
- // from this facet.
-
- current->silhouette(support, edgeBuffer);
- if(edgeBuffer.size() < 3 ) {
- return false;
- }
-
- if(entryBuffer.size() == maxSupportPoints_) {
- //"out of memory" so stop here
- //std::cerr << "Memory Limit reached." << std::endl;
- break;
- }
-
- EPA_EdgeBuffer::const_iterator it = edgeBuffer.begin();
- entryBuffer.push_back(EPA_Triangle(it->getEnd(), it->getStart(), epaVolume.size()-1, epaVolume));
-
- EPA_Triangle* firstTriangle = &(entryBuffer.back());
- //if it is expanding candidate add to heap
- //std::cerr << "Considering Triangle (" << firstTriangle->getSqrDist() << ") {" << (*firstTriangle)[0] << "," << (*firstTriangle)[1] << ","<< (*firstTriangle)[2] << "} ("<< epaVolume[(*firstTriangle)[0]] * firstTriangle->getNormal()<< ")" << std::endl;
- if(epaVolume[(*firstTriangle)[0]] * firstTriangle->getNormal() < real_t(0.0)){
- //the whole triangle is on the wrong side of the origin.
- //This is a numerical error and will produce wrong results, if the search is continued. Stop here.
- break;
- }
- if(firstTriangle->isClosestInternal()
- && firstTriangle->getSqrDist() > lowerBoundSqr
- && firstTriangle->getSqrDist() < upperBoundSqr)
- {
- entryHeap.push_back(firstTriangle);
- std::push_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
- }
-
- firstTriangle->link(0, it->getTriangle(), it->getIndex());
-
- EPA_Triangle* lastTriangle = firstTriangle;
-
- ++it;
- for(; it != edgeBuffer.end(); ++it){
- if(entryBuffer.size() == maxSupportPoints_) {
- //"out of memory" so stop here
- break;
- }
-
- entryBuffer.push_back(EPA_Triangle(it->getEnd(), it->getStart(), epaVolume.size()-1, epaVolume));
- EPA_Triangle* newTriangle = &(entryBuffer.back());
-
- //std::cerr << "Considering Triangle (" << newTriangle->getSqrDist() << ") {" << (*newTriangle)[0] << "," << (*newTriangle)[1] << ","<< (*newTriangle)[2] << "} ("<< epaVolume[(*newTriangle)[0]] * newTriangle->getNormal() << ")" << std::endl;
-
- if(epaVolume[(*newTriangle)[0]] * newTriangle->getNormal() < real_t(0.0)){
- //the whole triangle is on the wrong side of the origin.
- //This is an error.
- break;
- }
- //if it is expanding candidate add to heap
- if(newTriangle->isClosestInternal()
- && newTriangle->getSqrDist() > lowerBoundSqr
- && newTriangle->getSqrDist() < upperBoundSqr)
- {
- entryHeap.push_back(newTriangle);
- std::push_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
- }
-
- if(!newTriangle->link(0, it->getTriangle(), it->getIndex())) {
- break;
- }
-
- if(!newTriangle->link(2, lastTriangle, 1)) {
- break;
- }
-
- lastTriangle = newTriangle;
- }
-
- if(it != edgeBuffer.end()) {
- //For some reason the silhouette couldn't be processed completely
- //so we stop here and take the last result
- break;
- }
-
- firstTriangle->link(2, lastTriangle, 1);
- }
- } while (entryHeap.size() > 0 && entryHeap[0]->getSqrDist() <= upperBoundSqr);
-
- //Normal must be inverted
- retNormal = -current->getClosest().getNormalized();
-
- //Calculate Witness points
- const Vec3 wittnessA = current->getClosestPoint(supportA);
- const Vec3 wittnessB = current->getClosestPoint(supportB);
- contactPoint = real_t(0.5) * (wittnessA + wittnessB);
-
- //Penetration Depth
- penetrationDepth = -(current->getClosest().length() - real_t(2.0) * margin);
-
- /*std::cerr << "normal=" << retNormal <<std::endl;
- std::cerr << "close =" << current->getClosest() << std::endl;
- std::cerr << "diff =" << wittnesA - wittnesB <<std::endl;
- std::cerr << "wittnesA =" << wittnesA <<std::endl;
- std::cerr << "wittnesB =" << wittnesB <<std::endl;
- std::cerr << "contactPoint=" << contactPoint << std::endl;
- std::cerr << "penDepth=" << penetrationDepth <<std::endl;
- std::cerr << "lowerBound=" << sqrt(lowerBoundSqr) <<std::endl;
- std::cerr << "curreBound=" << current->getClosest().length() << std::endl;
- std::cerr << "upperBound=" << sqrt(upperBoundSqr) <<std::endl;
- std::cerr << "Heap Size=" << entryHeap.size() << std::endl;
- std::cerr << "entryHeap[0]->getSqrDist()=" << entryHeap[0]->getSqrDist() << std::endl;*/
- //std::cout << "EPA penetration depth: " << penetrationDepth << std::endl;
-
- if(penetrationDepth < contactThreshold) {
- return true;
- }
-
- //no intersection found!
- return false;
-}
-//*************************************************************************************************
-
-
-
-
//=================================================================================================
//
// EPA UTILITY FUNCTIONS
@@ -1001,427 +521,6 @@ inline void EPA::removeSupportMargin(std::vector<Vec3>& epaVolume, std::vector<V
}
//*************************************************************************************************
-//*************************************************************************************************
-/*! \brief TODO
- *
- * see Book "collision detection in interactive 3D environments" page161
- * ATTENTION seems to have no consistent behavior on the surface and vertices
- */
-inline bool EPA::originInTetrahedron( const Vec3& p0, const Vec3& p1, const Vec3& p2,
- const Vec3& p3 )
-{
- Vec3 normal0T = (p1 -p0) % (p2-p0);
- if( (normal0T*p0 > real_t(0.0)) == (normal0T*p3 > real_t(0.0)) ) {
- return false;
- }
- Vec3 normal1T = (p2 -p1) % (p3-p1);
- if( (normal1T*p1 > real_t(0.0)) == (normal1T*p0 > real_t(0.0)) ) {
- return false;
- }
- Vec3 normal2T = (p3 -p2) % (p0-p2);
- if( (normal2T*p2 > real_t(0.0)) == (normal2T*p1 > real_t(0.0)) ) {
- return false;
- }
- Vec3 normal3T = (p0 -p3) % (p1-p3);
- if( (normal3T*p3 > real_t(0.0)) == (normal3T*p2 > real_t(0.0)) ) {
- return false;
- }
-
- return true;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Retrurns true, if the origin lies in the tetrahedron ABCD.
- */
-inline bool EPA::originInTetrahedronVolumeMethod( const Vec3& A, const Vec3& B, const Vec3& C,
- const Vec3& D )
-{
- Vec3 aoT = A;
- if((aoT * (B % C)) <= real_t(0.0)) {
- //if volume of ABC and Origin <0.0 than the origin is on the wrong side of ABC
- //http://mathworld.wolfram.com/Tetrahedron.html volume formula
- return false;
- }
- if((aoT * (C % D)) <= real_t(0.0)) {
- return false;
- }
- if((aoT * (D % B)) <= real_t(0.0)) {
- return false;
- }
- if((B * (D % C)) <= real_t(0.0)) {
- return false;
- }
- return true;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Retrurns true, if a point lies in the tetrahedron ABCD.
- * \param point The point to be checked for containment.
- */
-inline bool EPA::pointInTetrahedron( const Vec3& A, const Vec3& B, const Vec3& C, const Vec3& D,
- const Vec3& point )
-{
- return originInTetrahedronVolumeMethod( A-point, B-point, C-point, D-point );
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief TODO
- * top, frontLeft ... are indices
- */
-inline void EPA::createInitialTetrahedron( size_t top, size_t frontLeft, size_t frontRight,
- size_t back, std::vector<Vec3>& epaVolume,
- EPA_EntryBuffer& entryBuffer )
-{
- //insert triangle 1
- entryBuffer.push_back(EPA_Triangle(top, frontLeft, frontRight, epaVolume)); //[0] vorne
- //insert triangle 2
- entryBuffer.push_back(EPA_Triangle(top, frontRight, back, epaVolume)); //[1] rechts hinten
- //insert triangle 3
- entryBuffer.push_back(EPA_Triangle(top, back, frontLeft, epaVolume)); //[2] links hinten
- //insert triangle 4
- entryBuffer.push_back(EPA_Triangle(back, frontRight, frontLeft, epaVolume)); //[3] unten
-
- //make links between the triangles
- entryBuffer[0].link(0, &(entryBuffer[2]), 2); //Kante vorne links
- entryBuffer[0].link(2, &(entryBuffer[1]), 0); //Kante vorne rechts
- entryBuffer[0].link(1, &(entryBuffer[3]), 1); //kante vorne unten
-
- entryBuffer[1].link(2, &(entryBuffer[2]), 0); //Kante hinten
- entryBuffer[1].link(1, &(entryBuffer[3]), 0); //kante rechts unten
-
- entryBuffer[2].link(1, &(entryBuffer[3]), 2); //kante links unten
-
-}
-//*************************************************************************************************
-
-/*! \brief Search a tetrahedron that contains the origin.
- * Start with four arbitrary support points in epaVolume that form a
- * tetrahedron. (This needn't contain the origin.)
- * This algorithm will search and return an altered tetrahedron
- * containing the origin. Do only use this function if the object/body
- * certainly contains the origin!
- * \return True, if a tetrahedron was found. False if search has been aborted.
- */
-inline bool EPA::searchTetrahedron(GeomPrimitive &geom1, GeomPrimitive &geom2, std::vector<Vec3>& epaVolume,
- std::vector<Vec3>& supportA, std::vector<Vec3>& supportB, EPA_EntryBuffer& entryBuffer, real_t margin )
-{
- //Store the point no longer needed (0 if all points are needed, and origin is contained.)
- int loopCount = 0;
- int pointIndexToRemove = -1;
- Vec3 newSearchDirection;
- do{
- loopCount++;
- pointIndexToRemove = -1;
- //Check if opposite tetrahedron point and orign are on the same side
- //of the face. (for all faces)
- Vec3 normal0T = (epaVolume[1] -epaVolume[0]) % (epaVolume[2]-epaVolume[0]);
- real_t dot_val = normal0T*epaVolume[0];
- if( (normal0T*epaVolume[3] < dot_val) == (dot_val < real_t(0.0)) ) {
- pointIndexToRemove = 3;
- newSearchDirection = (normal0T*epaVolume[3] < dot_val) ? normal0T : -normal0T;
- }
-
- Vec3 normal1T = (epaVolume[2] -epaVolume[1]) % (epaVolume[3]-epaVolume[1]);
- dot_val = normal1T*epaVolume[1];
- if( (normal1T*epaVolume[0] < dot_val) == (dot_val < real_t(0.0)) ) {
- pointIndexToRemove = 0;
- newSearchDirection = (normal1T*epaVolume[0] < dot_val) ? normal1T : -normal1T;
- }
-
- Vec3 normal2T = (epaVolume[3] -epaVolume[2]) % (epaVolume[0]-epaVolume[2]);
- dot_val = normal2T*epaVolume[2];
- if( (normal2T*epaVolume[1] < dot_val) == (dot_val < real_t(0.0)) ) {
- pointIndexToRemove = 1;
- newSearchDirection = (normal2T*epaVolume[1] < dot_val) ? normal2T : -normal2T;
- }
-
- Vec3 normal3T = (epaVolume[0] -epaVolume[3]) % (epaVolume[1]-epaVolume[3]);
- dot_val = normal3T*epaVolume[3];
- if( (normal3T*epaVolume[2] < dot_val) == (dot_val < real_t(0.0)) ) {
- pointIndexToRemove = 2;
- newSearchDirection = (normal3T*epaVolume[2] < dot_val) ? normal3T : -normal3T;
- }
- //Origin not contained in tetrahedron.
- if(pointIndexToRemove != -1){
- if(loopCount > 50){
- return false;
- }
- //Get new support point and replace old.
- /*std::cerr << "Search Direction is: "<< newSearchDirection << std::endl;
- std::cerr << "Projection of unnecc. point " << pointIndexToRemove << ": " << epaVolume[pointIndexToRemove] * newSearchDirection << std::endl;
- std::cerr << "Projection of other points: " << epaVolume[(pointIndexToRemove+1)%4] * newSearchDirection << std::endl;*/
- newSearchDirection = newSearchDirection.getNormalized();
- /*supportA[pointIndexToRemove] = geom1.supportContactThreshold(newSearchDirection);
- supportB[pointIndexToRemove] = geom2.supportContactThreshold(-newSearchDirection);
- epaVolume[pointIndexToRemove] = supportA[pointIndexToRemove] - supportB[pointIndexToRemove];*/
- replaceSupportMargin(geom1, geom2, newSearchDirection, margin, epaVolume, supportA, supportB, (size_t)pointIndexToRemove);
- //std::cerr << "Projection of new support point " << epaVolume[pointIndexToRemove] << ": " << epaVolume[pointIndexToRemove] * newSearchDirection << std::endl;
-
- }
- }
- while(pointIndexToRemove != 0);
- //std::cerr << "Found Tet after " << loopCount << " searches." << std::endl;
-
- //Build final tetrahedron
- Vec3 check_normal = (epaVolume[1] -epaVolume[0]) % (epaVolume[2]-epaVolume[0]);
- if(check_normal*epaVolume[3] > check_normal*epaVolume[0]){
- //p3 is behind.
- createInitialTetrahedron(1, 0, 2, 3, epaVolume, entryBuffer);
- }else{
- //p3 is in front
- createInitialTetrahedron(1, 3, 2, 0, epaVolume, entryBuffer);
- }
- return true;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Create a starting tetrahedron for EPA, from the GJK Simplex.
- */
-inline void EPA::createInitialSimplex( size_t numPoints, GeomPrimitive &geom1, GeomPrimitive &geom2,
- std::vector<Vec3>& supportA, std::vector<Vec3>& supportB,
- std::vector<Vec3>& epaVolume, EPA_EntryBuffer& entryBuffer, real_t margin )
-{
- switch(numPoints) {
- case 2:
- {
- //simplex is a line segement
- //add 3 points around the this segment
- //the COS is konvex so the resulting hexaheadron should be konvex too
-
- Vec3 d = epaVolume[1] - epaVolume[0];
- //find coordinate axis e_i which is furthest from paralell to d
- //and therefore d has the smallest abs(d[i])
- real_t abs0 = std::abs(d[0]);
- real_t abs1 = std::abs(d[1]);
- real_t abs2 = std::abs(d[2]);
-
- Vec3 axis;
- if( abs0 < abs1 && abs0 < abs2) {
- axis = Vec3(real_t(1.0), real_t(0.0), real_t(0.0));
- }
- else if( abs1 < abs0 && abs1 < abs2) {
- axis = Vec3(real_t(0.0), real_t(1.0), real_t(0.0));
- }
- else {
- axis = Vec3(real_t(0.0), real_t(0.0), real_t(1.0));
- }
-
- Vec3 direction1 = (d % axis).getNormalized();
- Quat q(d, (real_t(2.0)/real_t(3.0)) * real_t(walberla::math::M_PI));
- Mat3 rot = q.toRotationMatrix();
- Vec3 direction2 = (rot*direction1).getNormalized();
- Vec3 direction3 = (rot*direction2).getNormalized();
-
- //add point in positive normal direction1
- /*supportA.push_back(geom1.supportContactThreshold(direction1));
- supportB.push_back(geom2.supportContactThreshold(-direction1));
- Vec3 support1 = supportA.back() - supportB.back();
- epaVolume.push_back(support1);*/ //epaVolume[2]
-
- pushSupportMargin(geom1, geom2, direction1, margin, epaVolume, supportA, supportB);
- //Vec3 support1 = epaVolume.back();
-
- //std::cerr << "S1: " << support1 << std::endl;
- //add point in negative normal direction2
- /*supportA.push_back(geom1.supportContactThreshold(direction2));
- supportB.push_back(geom2.supportContactThreshold(-direction2));
- Vec3 support2 = supportA.back() - supportB.back();
- epaVolume.push_back(support2); *///epaVolume[3]
- pushSupportMargin(geom1, geom2, direction2, margin, epaVolume, supportA, supportB);
- //Vec3 support2 = epaVolume.back();
- //std::cerr << "S2: " << support2 << std::endl;
-
- /* //add point in negative normal direction3
- supportA.push_back(geom1.supportContactThreshold(direction3));
- supportB.push_back(geom2.supportContactThreshold(-direction3));
- Vec3 support3 = supportA.back() - supportB.back();
- epaVolume.push_back(support3); //epaVolume[4]*/
- pushSupportMargin(geom1, geom2, direction3, margin, epaVolume, supportA, supportB);
- /*std::cerr << "S3: " << support3 << std::endl;
- // TODO check what happens if two of the new support points are identical*/
-
- //Build the hexahedron as it is convex
- //epaVolume[1] = up
- //epaVolume[0] = down
- //epaVolume[2] = ccw1
- //epaVolume[3] = ccw2
- //epaVolume[4] = ccw3
-
-
-
- //check for containment inside
- if(originInTetrahedron(epaVolume[0], epaVolume[2], epaVolume[3], epaVolume[4]) || originInTetrahedron(epaVolume[1], epaVolume[2], epaVolume[3], epaVolume[4]) ){
- //insert triangle 1
- entryBuffer.push_back(EPA_Triangle(1, 2, 3, epaVolume)); //[0] up->ccw1->ccw2
- //insert triangle 2
- entryBuffer.push_back(EPA_Triangle(1, 3, 4, epaVolume)); //[1] up->ccw2->ccw3
- //insert triangle 3
- entryBuffer.push_back(EPA_Triangle(1, 4, 2, epaVolume)); //[2] up->ccw3->ccw1
-
- //link these 3 triangles
- entryBuffer[0].link(2, &(entryBuffer[1]), 0); //edge up->ccw1
- entryBuffer[1].link(2, &(entryBuffer[2]), 0); //edge up->ccw2
- entryBuffer[2].link(2, &(entryBuffer[0]), 0); //edge up->ccw3
-
-
- //insert triangle 4
- entryBuffer.push_back(EPA_Triangle(0, 2, 4, epaVolume)); //[3] down->ccw1->ccw3
- //insert triangle 5
- entryBuffer.push_back(EPA_Triangle(0, 4, 3, epaVolume)); //[4] down->ccw3->ccw2
- //insert triangle 6
- entryBuffer.push_back(EPA_Triangle(0, 3, 2, epaVolume)); //[5] down->ccw2->ccw1
-
- //link these 3 triangles
- entryBuffer[3].link(2, &(entryBuffer[4]), 0); //edge down->ccw3
- entryBuffer[4].link(2, &(entryBuffer[5]), 0); //edge down->ccw1
- entryBuffer[5].link(2, &(entryBuffer[3]), 0); //edge down->ccw1
-
- //link the two pyramids
- entryBuffer[0].link(1, &(entryBuffer[5]), 1); //edge ccw1->ccw2
- entryBuffer[1].link(1, &(entryBuffer[4]), 1); //edge ccw2->ccw3
- entryBuffer[2].link(1, &(entryBuffer[3]), 1); //edge ccw3->ccw1
- }else{
- //Apply iterative search
- removeSupportMargin(epaVolume, supportA, supportB); //remove 5th point.
- //Search starts from the remaining 4 points
- searchTetrahedron(geom1, geom2, epaVolume, supportA, supportB, entryBuffer, margin);
- }
-
- break;
- }
- case 3:
- {
- //simplex is a triangle, add tow points in positive and negative normal direction
-
- const Vec3& A = epaVolume[2]; //The Point last added to the simplex
- const Vec3& B = epaVolume[1]; //One Point that was already in the simplex
- const Vec3& C = epaVolume[0]; //One Point that was already in the simplex
- //ABC is a conterclockwise triangle
-
- const Vec3 AB = B-A; //The vector A->B
- const Vec3 AC = C-A; //The vector A->C
- const Vec3 ABC = (AB%AC).getNormalized(); //The the normal pointing towards the viewer if he sees a CCW triangle ABC
-
- //add point in positive normal direction
- pushSupportMargin(geom1, geom2, ABC, margin, epaVolume, supportA, supportB);
- //Vec3 support1 = epaVolume.back();
- /*supportA.push_back(geom1.supportContactThreshold(ABC));
- supportB.push_back(geom2.supportContactThreshold(-ABC));
- Vec3 support1 = supportA.back() - supportB.back();
- epaVolume.push_back(support1); //epaVolume[3]*/
-
- //add point in negative normal direction
- /*supportA.push_back(geom1.supportContactThreshold(-ABC));
- supportB.push_back(geom2.supportContactThreshold(ABC));
- Vec3 support2 = supportA.back() - supportB.back();
- epaVolume.push_back(support2); //epaVolume[4]*/
- pushSupportMargin(geom1, geom2, -ABC, margin, epaVolume, supportA, supportB);
- //Vec3 support2 = epaVolume.back();
-
- //check if the hexahedron is convex aka check if a partial tetrahedron contains the last point
- if(pointInTetrahedron(epaVolume[3], epaVolume[4], epaVolume[0], epaVolume[2], epaVolume[1])) {
- //epaVolumne[1] is whithin the tetraheadron 3-4-0-2 so this is the epaVolume to take
- createInitialTetrahedron(3,4,0,2, epaVolume, entryBuffer);
- }
- else if(pointInTetrahedron(epaVolume[3], epaVolume[4], epaVolume[1], epaVolume[0], epaVolume[2])) {
- createInitialTetrahedron(3,4,1,0, epaVolume, entryBuffer);
- }
- else if(pointInTetrahedron(epaVolume[3], epaVolume[4], epaVolume[2], epaVolume[1], epaVolume[0])) {
- createInitialTetrahedron(3,4,2,1, epaVolume, entryBuffer);
- }
- else {
- //Build the hexahedron as it is convex
- //insert triangle 1
- entryBuffer.push_back(EPA_Triangle(3, 2, 1, epaVolume)); //[0] support1->A->B
- //insert triangle 2
- entryBuffer.push_back(EPA_Triangle(3, 1, 0, epaVolume)); //[1] support1->B->C
- //insert triangle 3
- entryBuffer.push_back(EPA_Triangle(3, 0, 2, epaVolume)); //[2] support1->C->A
-
- //link these 3 triangles
- entryBuffer[0].link(2, &(entryBuffer[1]), 0); //edge support1->A
- entryBuffer[1].link(2, &(entryBuffer[2]), 0); //edge support1->B
- entryBuffer[2].link(2, &(entryBuffer[0]), 0); //edge support1->C
-
-
- //insert triangle 4
- entryBuffer.push_back(EPA_Triangle(4, 2, 0, epaVolume)); //[3] support2->A->C
- //insert triangle 5
- entryBuffer.push_back(EPA_Triangle(4, 0, 1, epaVolume)); //[4] support2->C->B
- //insert triangle 6
- entryBuffer.push_back(EPA_Triangle(4, 1, 2, epaVolume)); //[5] support2->B->A
-
- //link these 3 triangles
- entryBuffer[3].link(2, &(entryBuffer[4]), 0); //edge support2->C
- entryBuffer[4].link(2, &(entryBuffer[5]), 0); //edge support2->B
- entryBuffer[5].link(2, &(entryBuffer[3]), 0); //edge support2->A
-
- //link the two pyramids
- entryBuffer[0].link(1, &(entryBuffer[5]), 1); //edge A->B
- entryBuffer[1].link(1, &(entryBuffer[4]), 1); //edge B->C
- entryBuffer[2].link(1, &(entryBuffer[3]), 1); //edge C->A
-
- }
-
- break;
- }
- case 4:
- {
- createInitialTetrahedron(3,2,1,0, epaVolume, entryBuffer);
- break;
- }
- default:
- {
- WALBERLA_ASSERT( false, "invalid number of simplex points in EPA" );
- break;
- }
- }
-}
-
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Calculate a Circle through the for Points A, B, C, D.
- * \param center Contains the center point of the circle after the call
- * \return The squared radius of the circle or a negative value if no such circle exists.
- */
-inline real_t EPA::calculateCircle(const Vec3& A, const Vec3& B, const Vec3& C,
- const Vec3& D, Vec3& center ){
- real_t l1, l2, l3, d1, d2, d3;
- l1 = (A-B).length(); /* These three sqrt evaluations are necessary */
- l2 = (A-C).length();
- l3 = (A-D).length();
-
- Vec3 n1, n2, n3;
- n1 = (real_t(1.0)/l1)*(A-B);
- n2 = (real_t(1.0)/l2)*(A-C);
- n3 = (real_t(1.0)/l3)*(A-D);
-
- // Here we already see if such circle exists.
- real_t det = n1 * (n2 % n3);
- if(std::fabs(det) < math::Limits<real_t>::fpuAccuracy()){
- //no circle exists. Leave center untouched, and return -1.0
- return real_t(-1.0);
- }
- real_t Alen = A.sqrLength();
- d1 = (Alen - B.sqrLength())/(real_t(2.0)*l1);
- d2 = (Alen - C.sqrLength())/(real_t(2.0)*l2);
- d3 = (Alen - D.sqrLength())/(real_t(2.0)*l3);
-
- //Apply solution formula
- center = (real_t(1.0)/det)*(d1 * (n2 % n3) + d2 * (n3 % n1) + d3 * (n1 % n2));
-
- return (A - center).sqrLength();
-}
//@}
diff --git a/src/pe/collision/GJK.h b/src/pe/collision/GJK.h
index 49bde5a5f..a562ccc0d 100644
--- a/src/pe/collision/GJK.h
+++ b/src/pe/collision/GJK.h
@@ -27,7 +27,7 @@
#include <vector>
-#include <pe/Types.h>
+#include <pe/rigidbody/GeomPrimitive.h>
#include <pe/Thresholds.h>
#include <core/Abort.h>
@@ -61,9 +61,9 @@ public:
//**Query functions*****************************************************************************
/*! \name Query functions */
//@{
- inline real_t doGJK( GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& normal, Vec3& contactPoint );
+ real_t doGJK( GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& normal, Vec3& contactPoint );
- inline bool doGJKcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const real_t margin = contactThreshold);
+ bool doGJKmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const real_t margin = contactThreshold);
//@}
//**********************************************************************************************
@@ -76,18 +76,18 @@ public:
inline const std::vector<Vec3>& getSupportB() const;
//@}
//**********************************************************************************************
- std::vector<Vec3> simplex_; //<! Container to hold the simplex. (Public for debug reasons)
+
private:
//**Utility functions***************************************************************************
/*! \name Utility functions */
//@{
- inline bool simplex2(Vec3& d); //TODO rename 0-Simplex is a line not 1-Simplex
- inline bool simplex3(Vec3& d);
- inline bool simplex4(Vec3& d);
+ bool simplex2(Vec3& d);
+ bool simplex3(Vec3& d);
+ bool simplex4(Vec3& d);
inline bool sameDirection ( const Vec3& vec1, const Vec3& vec2 ) const;
inline bool zeroLengthVector( const Vec3& vec ) const;
- inline real_t calcDistance ( Vec3& normal, Vec3& contactPoint );
+ real_t calcDistance ( Vec3& normal, Vec3& contactPoint );
inline const Vec3 putSupport(const GeomPrimitive &geom1, const GeomPrimitive &geom2, const Vec3& dir, const real_t margin,
std::vector<Vec3> &simplex, std::vector<Vec3> &supportA, std::vector<Vec3> &supportB, size_t index);
//@}
@@ -96,7 +96,7 @@ private:
//**Member variables****************************************************************************
/*! \name Member variables */
//@{
-
+ std::vector<Vec3> simplex_; //<! Container to hold the simplex.
std::vector<Vec3> supportA_; //<! Container to hold the support points generated in triangle mesh mA
std::vector<Vec3> supportB_; //<! Container to hold the support points generated in triangle mesh mB
unsigned char numPoints_; //<! Current number of points in the simplex.
@@ -106,9 +106,6 @@ private:
};
//*************************************************************************************************
-
-
-
//=================================================================================================
//
// CONSTRUCTOR
@@ -123,247 +120,6 @@ inline GJK::GJK() : simplex_(4), supportA_(4), supportB_(4), numPoints_(0)
//*************************************************************************************************
-
-
-//=================================================================================================
-//
-// QUERY FUNCTIONS
-//
-//=================================================================================================
-
-//*************************************************************************************************
-/*! \brief Calculate an upper bound for the distance of two Geometries.
- * \return Distance between geom1 and geom2 or 0.0 if they are intersecting.
- */
-inline real_t GJK::doGJK(GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& normal, Vec3& contactPoint)
-{
-
-
- //Variables
- Vec3 support; //the current support point
- real_t ret; //return value aka distance between geom1 and geom2
-
-
- ////////////////////////////////////////////////////////////////////////
- //Initial initialisation step
- ret = 0.0;
- supportA_.resize(4);
- supportB_.resize(4);
- simplex_.resize(4);
-
- //get any first support point
-
- supportA_[0] = geom1.support(d_);
- supportB_[0] = geom2.support(-d_);
- support = supportA_[0] - supportB_[0];
-
- //add this point to the simplex_
- simplex_[0] = support;
- numPoints_ = 1;
-
- if(support * d_ < real_t(0.0)){
- //we went as far as we could in direction 'd' but not passed the origin
- //this means the bodies don't overlap
- ret = calcDistance(normal, contactPoint);
- return ret;
- }
-
- //first real search direction is in the opposite direction of the first support po
- d_ = -support;
-
- ////////////////////////////////////////////////////////////////////////
- //GJK main loop
- while (true) {
- //get the support point in the current search direction
- normalize(d_);
- supportA_[numPoints_] = geom1.support(d_);
- supportB_[numPoints_] = geom2.support(-d_);
- support = supportA_[numPoints_] - supportB_[numPoints_];
- //std::cerr << "[GJK] Support Direction: " << d_ << std::endl;
- //std::cerr << "[GJK] Got Support: " << support << std::endl;
-
- //check if "support" is passed the origin in search direction
- if(support * d_ < real_t(0.0)){
- //we went as far as we could in direction 'd' but not passed the origin
- //this means the bodies don't overlap
- //calc distance simplex to Origin
- ret = calcDistance(normal, contactPoint);
-
- return ret;
- }
-
- //add the new support point into the simplex
- simplex_[numPoints_] = support;
- numPoints_++;
-
- ////////////////////////////////////////////////////////////////
- //check if the origin is in the simplex
- //if it is the triangle mashes are overlapping
- switch(numPoints_)
- {
- case 2:
- {
- if(simplex2(d_)) {
- simplex_.pop_back();
- simplex_.pop_back();
- supportA_.pop_back();
- supportA_.pop_back();
- supportB_.pop_back();
- supportB_.pop_back();
- return ret;
- }
- }
- break;
-
- case 3:
- {
- if(simplex3(d_)) {
- simplex_.pop_back();
- supportA_.pop_back();
- supportB_.pop_back();
- return ret;
- }
- }
- break;
-
- case 4:
- {
- if(simplex4(d_)) {
-
- return ret;
- }
- }
- break;
- default:
- {
- WALBERLA_ABORT( "Number of points in the simplex is not 1<=n<=4" );
- }
- break;
- }
- }
-
- return ret; //never reach this point
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Compute if two geometries intersect. Both can be enlarged by a specified margin.
- * \param margin The margin by which the objects will be enlarged.
- */
-inline bool GJK::doGJKcontactThreshold(GeomPrimitive &geom1, GeomPrimitive &geom2, real_t margin)
-{
- //Variables
- Vec3 support; //the current support point
-
- ////////////////////////////////////////////////////////////////////////
- //Initial initialisation step
- supportA_.resize(4);
- supportB_.resize(4);
- simplex_.resize(4);
-
- //get any first support point
- if(numPoints_ != 0) {
- normalize(d_);
- }
- support = putSupport(geom1, geom2, d_, margin, simplex_, supportA_, supportB_, 0);
-
- //std::cerr << "Support 1: " << support << std::endl;
- //add this point to the simplex_
- numPoints_ = 1;
-
- //first real_t search direction is in the opposite direction of the first support point
- d_ = -support;
-
- /*
- if(support * d_ < 0.0){
- //we went as far as we could in direction 'd' but not passed the origin
- //this means the triangle mashes don't overlap
- //and as the support()-function extends the support point by contactThreshold
- //the mashes are not even close enough to be considered in contact.
- return false;
- }
- */
- ////////////////////////////////////////////////////////////////////////
- //GJK main loop
- while (true) {
- //get the support point in the current search direction
- normalize(d_);
- support = putSupport(geom1, geom2, d_, margin, simplex_, supportA_, supportB_, numPoints_);
-
- //std::cerr << "GJK: Got support storing at " << (int)numPoints_ << ": "<< support << std::endl;
- //check if "support" is passed the origin in search direction
- if(support * d_ < 0.0){
- // std::cerr << support * d_ << ": Returning false." << std::endl;
- //we went as far as we could in direction 'd' but not passed the origin
- //this means the triangle meshes don't overlap
- //and as the support()-function extends the support point by contactThreshold
- //the meshes are not even close enough to be considered in contact.
- return false;
- }
-
- //add the new support point into the simplex
- numPoints_++;
-
- //std::cerr << "Num points " << (int)numPoints_ << std::endl;
- ////////////////////////////////////////////////////////////////
- //check if the origin is in the simplex
- //if it is the triangle mashes are overlapping
- switch(numPoints_)
- {
- case 2:
- {
- if(simplex2(d_)) {
-
- //std::cerr << "Simplex2 success." << std::endl;
- while(simplex_.size() > numPoints_){
- simplex_.pop_back();
- supportA_.pop_back();
- supportB_.pop_back();
- }
- return true;
- }
- }
- break;
-
- case 3:
- {
- if(simplex3(d_)) {
- //std::cerr << "Simplex3 success." << std::endl;
- while(simplex_.size() > numPoints_){
- simplex_.pop_back();
- supportA_.pop_back();
- supportB_.pop_back();
- }
- return true;
- }
- }
- break;
-
- case 4:
- {
- if(simplex4(d_)) {
- //std::cerr << "Simplex4 success." << std::endl;
- return true;
- }
- }
- break;
-
- default:
- {
- //std::cerr << "numPoints_="<< numPoints_ <<std::endl;
- WALBERLA_ABORT( "Number of points in the simplex is not 1<=n<=4" );
- }
- break;
- }
- }
-
- return false; //never reach this point
-}
-//*************************************************************************************************
-
-
-
//=================================================================================================
//
// GET FUNCTIONS
@@ -402,7 +158,6 @@ inline const std::vector<Vec3>& GJK::getSupportB() const
//*************************************************************************************************
-
//=================================================================================================
//
// UTILITY FUNCTIONS
@@ -410,664 +165,7 @@ inline const std::vector<Vec3>& GJK::getSupportB() const
//=================================================================================================
//*************************************************************************************************
-/*! \brief Process a simplex with two nodes.
- */
-inline bool GJK::simplex2(Vec3& d)
-{
- //the simplex is a line
- const Vec3& A = simplex_[1]; //The Point last added to the simplex
- const Vec3& B = simplex_[0]; //The Point that was already in the simplex
- const Vec3 AO = -A; //The vector A->O with 0 the origin
- const Vec3 AOt = AO; //The transposed vector A->O with O the origin
- const Vec3 AB = B-A; //The vector A->B
-
- if( sameDirection(AOt, AB) ) {
- //The origin O is in the same direction as B is so the line AB is closest to the origin
- //=> keep A and B in the simplex
- d = AB % AO % AB;
- }
- else {
- //The origin is not in the direction of B seen from A.
- //So O lies in the voronoi region of A
- //=> simplex is just A
- simplex_[0] = A; //aka simplex_[1]
- supportA_[0] = supportA_[1];
- supportB_[0] = supportB_[1];
- numPoints_ = 1;
- d = AO;
- }
-
- //if the new search direction has zero length
- //than the origin is on the simplex
- if(zeroLengthVector(d)) {
- d_ = Vec3(real_t(0.0),real_t(0.6),real_t(0.8)); // give the GJK a chance to rerun
- return true;
- }
- return false;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Process a simplex with three nodes.
- */
-inline bool GJK::simplex3(Vec3& d)
-{
- //the simplex is a triangle
- const Vec3& A = simplex_[2]; //The Point last added to the simplex
- const Vec3& B = simplex_[1]; //One Point that was already in the simplex
- const Vec3& C = simplex_[0]; //One Point that was already in the simplex
- //ABC is a conterclockwise triangle
-
- const Vec3 AO = -A; //The vector A->O with 0 the origin
- const Vec3 AOt = AO; //The transposed vector A->O with O the origin
- const Vec3 AB = B-A; //The vector A->B
- const Vec3 AC = C-A; //The vector A->C
- const Vec3 ABC = AB%AC; //The the normal pointing towards the viewer if he sees a CCW triangle ABC
-
- if( sameDirection(AOt, (AB % ABC)) ) {
- //Origin is on the outside of the triangle of the line AB
- if( AOt * AB > 0.0) {
- //Origin in the voronoi region of AB outside the triangle
- //=> AB is the new simplex
- simplex_[0] = B; //aka simplex_[1]
- simplex_[1] = A; //aka simplex_[2]
- supportA_[0] = supportA_[1];
- supportA_[1] = supportA_[2];
- supportB_[0] = supportB_[1];
- supportB_[1] = supportB_[2];
- numPoints_ = 2;
- d = AB % AO % AB;
-
-
- }
- else {
- //STAR
- if( sameDirection(AOt,AC) ) {
- //Origin is on a subspace of the voronio region of AC
- //=> AC is the new simplex
- //simplex_[0] = C; //aka simplex_[0] already there
- simplex_[1] = A; //aka simplex_[2]
- supportA_[1] = supportA_[2];
- supportB_[1] = supportB_[2];
- numPoints_ = 2;
- d = AC % AO % AC;
- }
- else {
- //Origin is in the voronio region of A
- //=> A is the new simplex
- simplex_[0] = A; //aka simplex_[2]
- supportA_[0] = supportA_[2];
- supportB_[0] = supportB_[2];
- numPoints_ = 1;
- d = AO;
- }
- }
- }
- else {
- if( sameDirection(AOt, (ABC % AC)) ) {
- //Origin is on the outside of the triangle of the line AC
- //STAR
- if( AOt * AC > 0.0) {
- //Origin is on a subspace of the voronio region of AC
- //=> AC is the new simplex
- //simplex_[0] = C; //aka simplex_[0] already there
- simplex_[1] = A; //aka simplex_[2]
- supportA_[1] = supportA_[2];
- supportB_[1] = supportB_[2];
- numPoints_ = 2;
- d = AC % AO % AC;
- }
- else {
- //Origin is in the voronio region of A
- //=> A is the new simplex
- simplex_[0] = A; //aka simplex_[2]
- supportA_[0] = supportA_[2];
- supportB_[0] = supportB_[2];
- numPoints_ = 1;
- d = AO;
- }
- }
- else {
- //origin is above or below the triangle ABC but its mapping on the plane ABC lies within ABC
- if( sameDirection(AOt, ABC) ) {
- //Origin is above the triangle
- //=>Keep triangle as simplex seen from the origin it is already CCW
- d = ABC;
- }
- else {
- if( sameDirection(AOt, -ABC) ) {
- //Origin is below the triangle
- //=>Keep triangle as simplex.
- //seen from the origin ABC is CW so change the winding
- Vec3 temp = B; //aka simplex_[1]
- simplex_[1] = C; //aka simplex_[0]
- simplex_[0] = temp;
- //simplex_[2] = A; //aka simplex_[2] already there
- //old simplex 2:A 1:B 0:C
- //simplex now contains 2:A 1:C 0:B
-
- temp = supportA_[1];
- supportA_[1] = supportA_[0];
- supportA_[0] = temp;
- temp = supportB_[1];
- supportB_[1] = supportB_[0];
- supportB_[0] = temp;
-
- d = -ABC;
- }
- else{
- //Origin lies in the triangle
- return true;
- }
- }
- }
- }
-
- //if the new search direction has zero length
- //than the origin is on the boundary of the simplex
- if(zeroLengthVector(d)) {
- d_ = Vec3(real_t(0.0),real_t(0.6),real_t(0.8)); // give the GJK a chance to rerun
- return true;
- }
- return false;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*! \brief Process a simplex with four nodes.
- */
-inline bool GJK::simplex4(Vec3& d)
-{
- //the simplex is a tetrahedron
- const Vec3& A = simplex_[3]; //The Point last added to the tetrahedron
- //t in front mens just a temp varialble
- const Vec3& B = simplex_[2]; //One Point that was already in the simplex
- const Vec3& C = simplex_[1]; //One Point that was already in the simplex
- const Vec3& D = simplex_[0];
- //BCD is a clockwise triangle wenn seen from A
-
- const Vec3 AO = -A; //The vector A->O with 0 the origin
- const Vec3 AOt = AO; //The transposed vector A->O with O the origin
- const Vec3 AB = B-A; //The vector A->B
- const Vec3 AC = C-A; //The vector A->C
- const Vec3 AD = D-A; //The vector A-D
-
- //https://mollyrocket.com/forums/viewtopic.php?p=1829#1829
- unsigned char testWhere = 0;
-
- const Vec3 ABC = AB % AC; //The the normal pointing out of the tetrahedron towards the viewer if he sees a CCW triangle ABC
- const Vec3 ACD = AC % AD; //The the normal pointing out of the tetrahedron towards the viewer if he sees a CCW triangle ACD
- const Vec3 ADB = AD % AB; //The the normal pointing out of the tetrahedron towards the viewer if he sees a CCW triangle ADB
-
- if(sameDirection(AOt, ABC)) {
- testWhere |= 0x1;
- }
-
- if(sameDirection(AOt, ACD)) {
- testWhere |= 0x2;
- }
-
- if(sameDirection(AOt, ADB)) {
- testWhere |= 0x4;
- }
-
- switch(testWhere)
- {
- case 0:
- {
- //origin is in the tetrahedro
- //=> the two triangle mashes overlap
- //std::cout << "Origin is within the tetrahedron\nA=" << A << " B=" << B << " C="<< C << " D="<< D << std::endl;
- return true;
- } break;
-
- case 1:
- {
- // In front of ABC only
- //Origin is outside the tetrahedron above ABC
- //=> rearrange simplex to use the triangle case
- simplex_[0] = C; //aka simplex_[1] 0:C
- simplex_[1] = B; //aka simplex_[2] 1:B
- simplex_[2] = A; //aka simplex_[3] 2:A
-
- supportA_[0] = supportA_[1];
- supportA_[1] = supportA_[2];
- supportA_[2] = supportA_[3];
- supportB_[0] = supportB_[1];
- supportB_[1] = supportB_[2];
- supportB_[2] = supportB_[3];
-
- numPoints_ = 3;
-
- return simplex3(d);
- } break;
-
- case 2:
- {
- // In front of ACD only
- //Origin is outside the tetrahedron above ACD
- //=> rearrange simplex to use the triangle case
- //simplex_[0] = D; //aka simplex_[0] 0:D already the case
- //simplex_[1] = C; //aka simplex_[1] 1:C already the case
- simplex_[2] = A; //aka simplex_[3] 2:A
-
- supportA_[2] = supportA_[3];
- supportB_[2] = supportB_[3];
-
- numPoints_ = 3;
-
- return simplex3(d);
- } break;
-
- case 4:
- {
- // In front of ADB only
- //Origin is outside the tetrahedron above ADB
- //=> rearrange simplex to use the triangle case
- simplex_[1] = D; //aka simplex_[0] 1:D
- simplex_[0] = B; //aka simplex_[2] 0:B already there
- simplex_[2] = A; //aka simplex_[3] 2:A
-
- supportA_[1] = supportA_[0];
- supportA_[0] = supportA_[2];
- supportA_[2] = supportA_[3];
- supportB_[1] = supportB_[0];
- supportB_[0] = supportB_[2];
- supportB_[2] = supportB_[3];
-
- numPoints_ = 3;
-
- return simplex3(d);
- } break;
-
- case 3:
- {
- // In front of ABC and ACD
- if(sameDirection(AOt, ABC%AC)) {
- if(sameDirection(AOt, AC%ACD)) {
- if(sameDirection(AOt, AC)) {
- //AddEdgeSimplex(A, C);
- simplex_[0] = C; //aka simplex_[1] 0:C
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[0] = supportA_[1];
- supportA_[1] = supportA_[3];
- supportB_[0] = supportB_[1];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AC % AO % AC;
- }
- else {
- //AddPointSimplex;
- simplex_[0] = A; //aka simplex_[3] 0:A
-
- supportA_[0] = supportA_[3];
- supportB_[0] = supportB_[3];
-
- numPoints_ = 1;
- d = AO;
- }
- }
- else
- {
- if(sameDirection(AOt, ACD%AD)) {
- //AddEdgeSimplex(A, D);
- //simplex_[0] = D; //aka simplex_[0] 0:D already there
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[1] = supportA_[3];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AD % AO % AD;
- }
- else {
- //AddTriangleSimplex(A, C, D);
- //simplex_[0] = D; //aka simplex_[0] 0:D already there
- //simplex_[1] = C; //aka simplex_[1] 1:C already there
- simplex_[2] = A; //aka simplex_[3] 2:A
-
- supportA_[2] = supportA_[3];
- supportB_[2] = supportB_[3];
-
- numPoints_ = 3;
- d = ACD;
- }
- }
- }
- else
- {
- if(sameDirection(AOt, AB%ABC)) {
- if(sameDirection(AOt, AB)) {
- //AddEdgeSimplex(A, B);
- simplex_[0] = B; //aka simplex_[2] 0:B
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[0] = supportA_[2];
- supportA_[1] = supportA_[3];
- supportB_[0] = supportB_[2];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AB % AO % AB;
- }
- else {
- //AddPointSimplex;
- simplex_[0] = A; //aka simplex_[3] 0:A
-
- supportA_[0] = supportA_[3];
- supportB_[0] = supportB_[3];
-
- numPoints_ = 1;
- d = AO;
- }
- }
- else {
- //AddTriangleSimplex(A, B, C);
- simplex_[0] = C; //aka simplex_[1] 0:C
- simplex_[1] = B; //aka simplex_[2] 1:B
- simplex_[2] = A; //aka simplex_[3] 2:A
-
- supportA_[0] = supportA_[1];
- supportA_[1] = supportA_[2];
- supportA_[2] = supportA_[3];
- supportB_[0] = supportB_[1];
- supportB_[1] = supportB_[2];
- supportB_[2] = supportB_[3];
-
- numPoints_ = 3;
- d = ABC;
- }
- }
- } break;
-
-
- case 5:
- {
- // In front of ADB and ABC
- if(sameDirection(AOt, ADB%AB)) {
- if(sameDirection(AOt, AB%ABC)) {
- if(sameDirection(AOt, AB)) {
- //AddEdgeSimplex(A, B);
- simplex_[0] = B; //aka simplex_[2] 0:B
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[0] = supportA_[2];
- supportA_[1] = supportA_[3];
- supportB_[0] = supportB_[2];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AB % AO % AB;
- }
- else {
- //AddPointSimplex;
- simplex_[0] = A; //aka simplex_[3] 0:A
-
- supportA_[0] = supportA_[3];
- supportB_[0] = supportB_[3];
-
- numPoints_ = 1;
- d = AO;
- }
- }
- else
- {
- if(sameDirection(AOt, ABC%AC)) {
- //AddEdgeSimplex(A, C);
- simplex_[0] = C; //aka simplex_[1] 0:C
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[0] = supportA_[1];
- supportA_[1] = supportA_[3];
- supportB_[0] = supportB_[1];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AC % AO % AC;
- }
- else {
- //AddTriangleSimplex(A, B, C);
- simplex_[0] = C; //aka simplex_[1] 0:C
- simplex_[1] = B; //aka simplex_[2] 1:B
- simplex_[2] = A; //aka simplex_[3] 2:A
-
- supportA_[0] = supportA_[1];
- supportA_[1] = supportA_[2];
- supportA_[2] = supportA_[3];
- supportB_[0] = supportB_[1];
- supportB_[1] = supportB_[2];
- supportB_[2] = supportB_[3];
-
- numPoints_ = 3;
- d = ABC;
- }
- }
- }
- else
- {
- if(sameDirection(AOt, AD%ADB)) {
- if(sameDirection(AOt, AD)) {
- //AddEdgeSimplex(A, D);
- //simplex_[0] = D; //aka simplex_[0] 0:D already there
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[1] = supportA_[3];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AD % AO % AD;
- }
- else {
- //AddPointSimplex;
- simplex_[0] = A; //aka simplex_[3] 0:A
-
- supportA_[0] = supportA_[3];
- supportB_[0] = supportB_[3];
-
- numPoints_ = 1;
- d = AO;
- }
- }
- else {
- //AddTriangleSimplex(A, D, B);
- simplex_[1] = D; //aka simplex[0] 1:D
- simplex_[0] = B; //aka simplex[2] 0:B
- simplex_[2] = A; //aka simplex[3] 2:A
-
- supportA_[1] = supportA_[0];
- supportA_[0] = supportA_[2];
- supportA_[2] = supportA_[3];
- supportB_[1] = supportB_[0];
- supportB_[0] = supportB_[2];
- supportB_[2] = supportB_[3];
-
- numPoints_ = 3;
-
- numPoints_ = 3;
- d = ADB;
- }
- }
- } break;
-
- case 6:
- {
- // In front of ACD and ADB
- if(sameDirection(AOt, ACD%AD)) {
- if(sameDirection(AOt, AD%ADB)) {
- if(sameDirection(AOt, AD)) {
- //AddEdgeSimplex(A, D);
- //simplex_[0] = D; //aka simplex_[0] 0:D already there
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[1] = supportA_[3];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AD % AO % AD;
- }
- else {
- //AddPointSimplex;
- simplex_[0] = A; //aka simplex_[3] 0:A
-
- supportA_[0] = supportA_[3];
- supportB_[0] = supportB_[3];
-
- numPoints_ = 1;
- d = AO;
- }
- }
- else
- {
- if(sameDirection(AOt, ADB%AB)) {
- //AddEdgeSimplex(A, B);
- simplex_[0] = B; //aka simplex_[2] 0:B
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[0] = supportA_[2];
- supportA_[1] = supportA_[3];
- supportB_[0] = supportB_[2];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AB % AO % AB;
- }
- else {
- //AddTriangleSimplex(A, D, B);
- simplex_[1] = D; //aka simplex[0] 1:D
- simplex_[0] = B; //aka simplex[2] 0:B
- simplex_[2] = A; //aka simplex[3] 2:A
-
- supportA_[1] = supportA_[0];
- supportA_[0] = supportA_[2];
- supportA_[2] = supportA_[3];
- supportB_[1] = supportB_[0];
- supportB_[0] = supportB_[2];
- supportB_[2] = supportB_[3];
-
- numPoints_ = 3;
-
- numPoints_ = 3;
- d = ADB;
- }
- }
- }
- else
- {
- if(sameDirection(AOt, AC%ACD)) {
- if(sameDirection(AOt, AC)) {
- //AddEdgeSimplex(A, C);
- simplex_[0] = C; //aka simplex_[1] 0:C
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[0] = supportA_[1];
- supportA_[1] = supportA_[3];
- supportB_[0] = supportB_[1];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AC % AO % AC;
- }
- else
- {
- //AddPointSimplex;
- simplex_[0] = A; //aka simplex_[3] 0:A
-
- supportA_[0] = supportA_[3];
- supportB_[0] = supportB_[3];
-
- numPoints_ = 1;
- d = AO;
- }
- }
- else
- {
- //AddTriangleSimplex(A, C, D);
- //simplex_[0] = D; //aka simplex_[0] 0:D already there
- //simplex_[1] = C; //aka simplex_[1] 1:C already there
- simplex_[2] = A; //aka simplex_[3] 2:A
-
- supportA_[2] = supportA_[3];
- supportB_[2] = supportB_[3];
-
- numPoints_ = 3;
- d = ACD;
- }
- }
- } break;
-
- case 7:
- {
- // In front of ABC, ACD and ADB
- if(sameDirection(AOt, AB)) {
- simplex_[0] = B; //aka simplex_[2] 0:B
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[0] = supportA_[2];
- supportA_[1] = supportA_[3];
- supportB_[0] = supportB_[2];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AB % AO % AB;
- }
- else
- {
- if(sameDirection(AOt, AC)) {
- simplex_[0] = C; //aka simplex_[1] 0:C
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[0] = supportA_[1];
- supportA_[1] = supportA_[3];
- supportB_[0] = supportB_[1];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AC % AO % AC;
-
- }
- else
- {
- if(sameDirection(AOt, AD)) {
- //simplex_[0] = D; //aka simplex_[1] 0:D already there
- simplex_[1] = A; //aka simplex_[3] 1:A
-
- supportA_[1] = supportA_[3];
- supportB_[1] = supportB_[3];
-
- numPoints_ = 2;
- d = AD % AO % AD;
- }
- else {
- simplex_[0] = A; //aka simplex_[3] 0:A
-
- supportA_[0] = supportA_[3];
- supportB_[0] = supportB_[3];
-
- numPoints_ = 1;
- d = AO;
- }
- }
- }
- } break;
- default:
- {
- //all 8 cases 0-7 are covered
- } break;
- }
-
- return false;
-}
-//*************************************************************************************************
-
-
-//*************************************************************************************************
-/*!\brief TODO
- *
- * Checks if tow vectors roughly point in the same direction
+/*!\brief Checks if two vectors roughly point in the same directionTODO
*/
inline bool GJK::sameDirection(const Vec3& vec1, const Vec3& vec2) const
{
@@ -1077,9 +175,7 @@ inline bool GJK::sameDirection(const Vec3& vec1, const Vec3& vec2) const
//*************************************************************************************************
-/*!\brief TODO
- *
- * Checks if the length of a vector is zero or as close to zero that it can not be distinguished form zero
+/* Checks if the length of a vector is zero or as close to zero that it can not be distinguished form zero
*/
inline bool GJK::zeroLengthVector(const Vec3& vec) const
{
@@ -1105,108 +201,6 @@ inline const Vec3 GJK::putSupport(const GeomPrimitive &geom1, const GeomPrimitiv
//*************************************************************************************************
-//*************************************************************************************************
-/*!\brief TODO
- */
-inline real_t GJK::calcDistance( Vec3& normal, Vec3& contactPoint )
-{
- //find the point in the simplex closest to the origin#
- //its distance to the origin is the distance of the two objects
- real_t dist= 0.0;
-
- real_t barCoords[3] = { 0.0, 0.0, 0.0};
- real_t& u = barCoords[0];
- real_t& v = barCoords[1];
- real_t& w = barCoords[2];
-
- Vec3& A = simplex_[0];
- Vec3& B = simplex_[1];
- Vec3& C = simplex_[2];
- //std::cerr << (int) numPoints_ << " " << A << B << C << std::endl;
- switch(numPoints_){
- case 1:
- {
- //the only point in simplex is closest to Origin
- dist = std::sqrt(A.sqrLength());
- u = real_t(1.0);
- break;
- }
- case 2:
- {
- //calc distance Origin to line segment
- //it is definitively closest do the segment not to one of the end points
- //as the voronoi regions of the points also consist of the border borderline between
- //point region and segment region
- // compare "Real-Time Collision Detection" by Christer Ericson page 129ff
- Vec3 ab = B - A;
- //Vec3 ac = -A;
- //Vec3 bc = -simplex[1];
-
- //calc baryzenctric coordinats
- // compare "Real-Time Collision Detection" by Christer Ericson page 129
- //double t = ac*ab;
- real_t t = real_t(-1.0) * (A * ab);
- real_t denom = std::sqrt(ab.sqrLength());
- u = t / denom;
- v = real_t(1.0) - u;
- Vec3 closestPoint = u*A + v*B;
- dist = std::sqrt(closestPoint.sqrLength());
- // compare "Real-Time Collision Detection" by Christer Ericson page 130
- //double& e = t;
- //double& f = denom;
- //dist = ac.sqrLength() - e*e/f;
- break;
- }
- case 3:
- {
- //origin is surly in the voronoi region of the face itself
- //not the bordering lines or one of the 3 points
- //to be more precise it is also in normal direction.
- // compare "Real-Time Collision Detection" by Christer Ericson page 139
- //TODO: evlt kann man das berechnen ohne den projektionspunkt zu bestimmen
-
- //Vec3 ab= B - A;
- //Vec3 ac= C - A;
- //Vec3 bc= C - B;
- Vec3& n = d_; //we already know the normal
- Vec3 nT = n;
-
- real_t vc = nT * (A % B);
- real_t va = nT * (B % C);
- real_t vb = nT * (C % A);
- real_t denom = real_t(1.0) / (va + vb + vc);
- u = va * denom;
- v = vb * denom;
- w = real_t(1.0) - u - v;
- //std::cerr << u << " " << v << " " << w << std::endl;
- Vec3 closestPoint = u*A + v*B + w*C;
- dist = std::sqrt(closestPoint.sqrLength());
-
- break;
- }
- default:
- {
- std::cout << "falsche anzahl an Punkten im simplex" <<std::endl;
- break;
- }
- }
-
- Vec3 pointOnA = u * supportA_[0];
- Vec3 pointOnB = u * supportB_[0];
- for( size_t i = 1; i < numPoints_; ++i) {
- pointOnA += barCoords[i] * supportA_[i];
- pointOnB += barCoords[i] * supportB_[i];
- }
-
- normal = (pointOnA - pointOnB).getNormalized();
- contactPoint = (pointOnA + pointOnB) * real_t(0.5);
-
-
- return dist;
-}
-//*************************************************************************************************
-
-
} // namespace fcd
} // namespace pe
--
GitLab