[Freesolid-cvs] freesolid/libsolid C-api.cpp,1.10,1.11 Convex.cpp,1.6,1.7 Convex.h,1.3,1.4 Object.cpp,1.8,1.9

[Rodrigo H. <kw...@us...>]

Update of /cvsroot/freesolid/freesolid/libsolid
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv27929/libsolid

Modified Files:
	C-api.cpp Convex.cpp Convex.h Object.cpp 
Log Message:
Started implementing EPA

Index: Object.cpp
===================================================================
RCS file: /cvsroot/freesolid/freesolid/libsolid/Object.cpp,v
retrieving revision 1.8
retrieving revision 1.9
diff -C2 -d -r1.8 -r1.9
*** Object.cpp	4 Jul 2005 23:35:05 -0000	1.8
--- Object.cpp	6 Jul 2005 23:40:57 -0000	1.9
***************
*** 105,109 ****
  {
    static IntersectTable p;
!   p.addEntry(CONVEX, CONVEX, intersectConvexConvex);
    p.addEntry(COMPLEX, CONVEX, intersectComplexConvex);
    p.addEntry(COMPLEX, COMPLEX, intersectComplexComplex);
--- 105,109 ----
  {
    static IntersectTable p;
!   p.addEntry(CONVEX,  CONVEX, intersectConvexConvex);
    p.addEntry(COMPLEX, CONVEX, intersectComplexConvex);
    p.addEntry(COMPLEX, COMPLEX, intersectComplexComplex);
***************
*** 136,140 ****
  }
  
! Common_pointTable *common_pointInitialize() {
    static Common_pointTable p;
    p.addEntry(CONVEX, CONVEX, common_pointConvexConvex);
--- 136,141 ----
  }
  
! Common_pointTable *common_pointInitialize() 
! {
    static Common_pointTable p;
    p.addEntry(CONVEX, CONVEX, common_pointConvexConvex);
***************
*** 151,156 ****
  
  /*! \brief I THINK this returns closest points from last test
!   \param a Object A to test against B ?
!   \param b Object B to test against A ?
    \param v No idea.
    \param pa Point in A?
--- 152,157 ----
  
  /*! \brief I THINK this returns closest points from last test
!   \param a Object A to test against B
!   \param b Object B to test against A
    \param v No idea.
    \param pa Point in A?
***************
*** 208,213 ****
    else 
      {
!       // if both are simplex
!       if (!intersect(a, b, v)) return false;
        closest_points((const Convex&)*a.shapePtr, 
  		     (const Convex&)*b.shapePtr, 
--- 209,215 ----
    else 
      {
!       // if both are simple convex objects
!       if (!intersect(a, b, v)) // Apply GJK
! 	return false;          // If there is no intersection, return false
        closest_points((const Convex&)*a.shapePtr, 
  		     (const Convex&)*b.shapePtr, 

Index: C-api.cpp
===================================================================
RCS file: /cvsroot/freesolid/freesolid/libsolid/C-api.cpp,v
retrieving revision 1.10
retrieving revision 1.11
diff -C2 -d -r1.10 -r1.11
*** C-api.cpp	4 Jul 2005 23:35:05 -0000	1.10
--- C-api.cpp	6 Jul 2005 23:40:57 -0000	1.11
***************
*** 125,129 ****
  }
  
! void dtBegin(DtPolyType type) {
    currentType = type;
  }
--- 125,130 ----
  }
  
! void dtBegin(DtPolyType type) 
! {
    currentType = type;
  }
***************
*** 135,139 ****
  }
  
! void dtVertex(DT_Scalar x,DT_Scalar y,DT_Scalar z) {
    Point p(x,y,z);
    int i = int(pointBuf.size())-20;
--- 136,141 ----
  }
  
! void dtVertex(DT_Scalar x,DT_Scalar y,DT_Scalar z) 
! {
    Point p(x,y,z);
    int i = int(pointBuf.size())-20;
***************
*** 144,179 ****
  }
  
! void dtVertexBase(const void *base) {
    currentComplex->setBase(base); 
  }
  
! void dtVertexIndex(DT_Index idx) {
    indexBuf.push_back(idx);
  }
  
! void dtVertexIndices(DtPolyType type,DT_Count cnt,const DT_Index *indices) {
!   if (currentComplex) {
!     const Polytope *poly;
!     switch (type) {
!     case DT_SIMPLEX:
!       poly = new Simplex(currentComplex->getBase(), cnt, indices);
!       break;
!     case DT_POLYGON:
!       poly = new Polygon(currentComplex->getBase(), cnt, indices);
!       break;
!     case DT_POLYHEDRON:
!       if (currentComplex->getBase().getPointer()==0) {
! 	currentComplex->setBase(&pointBuf[0]);
! 	poly = new Polyhedron(currentComplex->getBase(),cnt,indices);
! 	currentComplex->setBase(0);
        }
!       else poly = new Polyhedron(currentComplex->getBase(),cnt,indices);
!       break;
      }
-     polyList.push_back(poly);
-   }
  }
  
! void dtVertexRange(DtPolyType type, DT_Index first, DT_Count cnt) {
    DT_Index *indices = new DT_Index[cnt];
    for (unsigned int i = 0; i < cnt;++i) indices[i] = first + i;
--- 146,186 ----
  }
  
! void dtVertexBase(const void *base) 
! {
    currentComplex->setBase(base); 
  }
  
! void dtVertexIndex(DT_Index idx) 
! {
    indexBuf.push_back(idx);
  }
  
! void dtVertexIndices(DtPolyType type,DT_Count cnt,const DT_Index *indices) 
! {
!   if (currentComplex) 
!     {
!       const Polytope *poly;
!       switch (type) {
!       case DT_SIMPLEX:
! 	poly = new Simplex(currentComplex->getBase(), cnt, indices);
! 	break;
!       case DT_POLYGON:
! 	poly = new Polygon(currentComplex->getBase(), cnt, indices);
! 	break;
!       case DT_POLYHEDRON:
! 	if (currentComplex->getBase().getPointer()==0) {
! 	  currentComplex->setBase(&pointBuf[0]);
! 	  poly = new Polyhedron(currentComplex->getBase(),cnt,indices);
! 	  currentComplex->setBase(0);
! 	}
! 	else poly = new Polyhedron(currentComplex->getBase(),cnt,indices);
! 	break;
        }
!       polyList.push_back(poly);
      }
  }
  
! void dtVertexRange(DtPolyType type, DT_Index first, DT_Count cnt) 
! {
    DT_Index *indices = new DT_Index[cnt];
    for (unsigned int i = 0; i < cnt;++i) indices[i] = first + i;
***************
*** 182,203 ****
  }
  
! void dtDeleteShape(DtShapeRef shape) { 
!   if (((Shape *)shape)->getType() == COMPLEX) {
!     ComplexList::iterator i = 
!       find(complexList.begin(), complexList.end(), (Complex *)shape);
!     if (i != complexList.end()) complexList.erase(i);
!   }
    delete (Shape *)shape; 
  }
  
! void dtChangeVertexBase(DtShapeRef shape, const void *base) {
    if (((Shape *)shape)->getType() == COMPLEX)
      ((Complex *)shape)->changeBase(base);
    for (ObjectList::const_iterator i = objectList.begin(); 
!        i != objectList.end(); ++i) {
!     if ((*i).second->shapePtr == (Shape *)shape) {
!       (*i).second->do_broadphase();
      }
-   }   
  }
  
--- 189,215 ----
  }
  
! void dtDeleteShape(DtShapeRef shape) 
! {
!   if (((Shape *)shape)->getType() == COMPLEX) 
!     {
!       ComplexList::iterator i = 
! 	find(complexList.begin(), complexList.end(), (Complex *)shape);
!       if (i != complexList.end()) complexList.erase(i);
!     }
    delete (Shape *)shape; 
  }
  
! void dtChangeVertexBase(DtShapeRef shape, const void *base) 
! {
    if (((Shape *)shape)->getType() == COMPLEX)
      ((Complex *)shape)->changeBase(base);
    for (ObjectList::const_iterator i = objectList.begin(); 
!        i != objectList.end(); ++i) 
!     {
!       if ((*i).second->shapePtr == (Shape *)shape) 
! 	{
! 	  (*i).second->do_broadphase();
! 	}
      }
  }
  
***************
*** 205,210 ****
  // Object instantiation
  
! void dtCreateObject(DtObjectRef object, DtShapeRef shape) {
!  currentObject = objectList[object] = new Object(object,(Shape *)shape, broadphase);
  }
  
--- 217,223 ----
  // Object instantiation
  
! void dtCreateObject(DtObjectRef object, DtShapeRef shape) 
! {
!   currentObject = objectList[object] = new Object(object,(Shape *)shape, broadphase);
  }
  
***************
*** 303,307 ****
  // Runtime
  
! void dtProceed() {
    for (ComplexList::iterator i = complexList.begin(); 
         i != complexList.end(); ++i) 
--- 316,321 ----
  // Runtime
  
! void dtProceed() 
! {
    for (ComplexList::iterator i = complexList.begin(); 
         i != complexList.end(); ++i) 
***************
*** 312,316 ****
  }
  
! void dtEnableCaching() {
    for (ObjectList::const_iterator i=objectList.begin();i!=objectList.end();++i)
      (*i).second->do_broadphase();
--- 326,331 ----
  }
  
! void dtEnableCaching() 
! {
    for (ObjectList::const_iterator i=objectList.begin();i!=objectList.end();++i)
      (*i).second->do_broadphase();
***************
*** 318,324 ****
  }
  
! void dtDisableCaching() { caching = false; }
  
! void dtSetTolerance(DT_Scalar tol) { rel_error = tol; }
  
  /*! 
--- 333,345 ----
  }
  
! void dtDisableCaching() 
! { 
!   caching = false; 
! }
  
! void dtSetTolerance(DT_Scalar tol) 
! { 
!   rel_error = tol; 
! }
  
  /*! 
***************
*** 351,356 ****
  	  // Added these to see if there is any difference, 
  	  // and there is, but not quite what was expected, or is it?.
! 	  ((Object *)e.obj1)->proceed();
! 	  ((Object *)e.obj2)->proceed();
  	  return true; 
  	}
--- 372,377 ----
  	  // Added these to see if there is any difference, 
  	  // and there is, but not quite what was expected, or is it?.
! 	  //((Object *)e.obj1)->proceed();
! 	  //((Object *)e.obj2)->proceed();
  	  return true; 
  	}

Index: Convex.cpp
===================================================================
RCS file: /cvsroot/freesolid/freesolid/libsolid/Convex.cpp,v
retrieving revision 1.6
retrieving revision 1.7
diff -C2 -d -r1.6 -r1.7
*** Convex.cpp	4 Jul 2005 23:35:05 -0000	1.6
--- Convex.cpp	6 Jul 2005 23:40:57 -0000	1.7
***************
*** 24,28 ****
  		  P.O. Box 513, 5600 MB Eindhoven, The Netherlands
  */
! 
  #include "Convex.h"
  #include "BBox.h"
--- 24,28 ----
  		  P.O. Box 513, 5600 MB Eindhoven, The Netherlands
  */
! #include <vector>
  #include "Convex.h"
  #include "BBox.h"
***************
*** 57,60 ****
--- 57,62 ----
  static Vector y[4];   // support points of A - B in world coordinates
  
+ static std::vector<Vector> inflated_polyhedron; // Inflated Polyhedron for depth penetration
+ 
  static int bits;      // identifies current simplex
  static int last;      // identifies last found support point
***************
*** 174,215 ****
  #endif
  
! inline bool closest(Vector& v) {
    compute_det();
!   for (int s = bits; s; --s) {
!     if ((s & bits) == s) {
!       if (valid(s|last_bit)) {
! 	bits = s|last_bit;
!  	compute_vector(bits, v);
! 	return true;
!       }
      }
-   }
-   if (valid(last_bit)) {
-     bits = last_bit;
-     v = y[last];
-     return true;
-   }
    // Original GJK calls the backup procedure at this point.
! 
  #ifdef USE_BACKUP_PROCEDURE
! 
    Scalar min_dist2 = SOLID_INFINITY;
!   for (int s = all_bits; s; --s) {
!     if ((s & all_bits) == s) {
!       if (proper(s)) {
! 	Vector u;
!  	compute_vector(s, u);
! 	Scalar dist2 = u.length2();
! 	if (dist2 < min_dist2) {
! 	  min_dist2 = dist2;
! 	  bits = s;
! 	  v = u;
  	}
-       }
      }
!   }
! 
! #endif 
! 
    return false;
  }
--- 176,224 ----
  #endif
  
! inline bool closest(Vector& v) 
! {
    compute_det();
!   for (int s = bits; s; --s) 
!     {
!       if ((s & bits) == s) 
! 	{
! 	  if (valid(s|last_bit)) 
! 	    {
! 	      bits = s|last_bit;
! 	      compute_vector(bits, v);
! 	      return true;
! 	    }
! 	}
!     }
!   if (valid(last_bit)) 
!     {
!       bits = last_bit;
!       v = y[last];
!       return true;
      }
    // Original GJK calls the backup procedure at this point.
!   
  #ifdef USE_BACKUP_PROCEDURE
!   
    Scalar min_dist2 = SOLID_INFINITY;
!   for (int s = all_bits; s; --s) 
!     {
!       if ((s & all_bits) == s) 
! 	{
! 	  if (proper(s)) 
! 	    {
! 	      Vector u;
! 	      compute_vector(s, u);
! 	      Scalar dist2 = u.length2();
! 	      if (dist2 < min_dist2) {
! 		min_dist2 = dist2;
! 		bits = s;
! 		v = u;
! 	      }
! 	    }
  	}
      }
!   
! #endif
    return false;
  }
***************
*** 237,241 ****
    \param b2w [IN]  Transformation Matrix for B
    \param v   [OUT] Separating axis
!   \note This is core of the GJK Algorithm
  */
  bool intersect(const Convex& a, 
--- 246,250 ----
    \param b2w [IN]  Transformation Matrix for B
    \param v   [OUT] Separating axis
!   \note This is the core of the GJK Algorithm
  */
  bool intersect(const Convex& a, 
***************
*** 245,248 ****
--- 254,258 ----
  	       Vector& v) 
  {
+   inflated_polyhedron.clear();
    Vector w;
  
***************
*** 253,287 ****
    num_iterations = 0;
  #endif
! 
!   do {
!     last = 0;
!     last_bit = 1;
!     while (bits & last_bit) 
!       { ++last; last_bit <<= 1; }
! 
!     w = a2w(a.support((-v) * a2w.getBasis())) - 
!       b2w(b.support(v * b2w.getBasis())); 
!     if (dot(v, w) > 0) return false;
!     if (degenerate(w)) 
!       {
  #ifdef STATISTICS
! 	++num_irregularities;
  #endif
! 	return false;
!       }
!     y[last] = w;
!     all_bits = bits|last_bit;
  #ifdef STATISTICS
!     ++num_iterations;
  #endif
!     if (!closest(v)) 
!       {
  #ifdef STATISTICS
! 	++num_irregularities;
  #endif
! 	return false;
!       }
!   }
!   while (bits < 15 && !approxZero(v)); 
    return true;
  }
--- 263,341 ----
    num_iterations = 0;
  #endif
!   do 
!     {
!       last = 0;
!       last_bit = 1;
!       while (bits & last_bit) 
! 	{ ++last; last_bit <<= 1; }
!       
!       w = a2w(a.support((-v) * a2w.getBasis())) - 
! 	b2w(b.support(v * b2w.getBasis())); 
!       if (dot(v, w) > 0) return false;
!       if (degenerate(w)) 
! 	{
  #ifdef STATISTICS
! 	  ++num_irregularities;
  #endif
! 	  return false;
! 	}
!       y[last] = w;
!       all_bits = bits|last_bit;
  #ifdef STATISTICS
!       ++num_iterations;
  #endif
!       if (!closest(v)) 
! 	{
  #ifdef STATISTICS
! 	  ++num_irregularities;
  #endif
! 	  return false;
! 	}
!     }
!   while (bits < 15 && !approxZero(v));
!   switch(last)
!     {
!     case 0:
!       /* 
! 	 Simplex is a point this should not happen,
! 	 maybe if the 2 objects are duplicates and
! 	 concentric, for now, just report.
!       */
!       fprintf(stdout,"Panic: Simplex is a Point!\n");
!       break;
!     case 1:
!       // The Simplex is line, return the point closer to the origin
!       if(y[0].length2()<y[1].length2())
! 	{
! 	  v=y[0];
! 	}
!       else
! 	{
! 	  v=y[1];	  
! 	}
!       break;
!     case 2:
!       // The simplex is a triangle, add support points in the normal
!       // of the triangle direction and the inverse of the normal
!       v.normal(y[0],y[1],y[2]);
!       w = a2w(a.support((-v) * a2w.getBasis())) - 
! 	b2w(b.support(v * b2w.getBasis())); 
!       inflated_polyhedron.push_back(w);
!       v*=-1;
!       w = a2w(a.support((-v) * a2w.getBasis())) - 
! 	b2w(b.support(v * b2w.getBasis())); 
!       inflated_polyhedron.push_back(w);
!       // no break here since we go thru the same process as if this was
!       // a tetrahedron.
!     case 3:
!       // The simplex is a tetrahedron, just what we need!
!       for(int i=0;i<=last;++i)
! 	{
! 	  inflated_polyhedron.push_back(y[i]);
! 	}
!       // ???????
!       // Profit! :)
!       break;
!     }
    return true;
  }
***************
*** 470,473 ****
--- 524,531 ----
  }
  
+ /*! \brief returns the shortest penetration depth for the last test */
+ void shortest_penetration_depth(Vector& pd)
+ {
+ }
  
  

Index: Convex.h
===================================================================
RCS file: /cvsroot/freesolid/freesolid/libsolid/Convex.h,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** Convex.h	11 May 2005 23:31:04 -0000	1.3
--- Convex.h	6 Jul 2005 23:40:57 -0000	1.4
***************
*** 71,73 ****
--- 71,75 ----
  		    Point&, Point&);
  
+ void shortest_penetration_depth(Vector& pd);
+ 
  #endif