vclip.h
Go to the documentation of this file.
00001 
00002 //
00003 //  Copyright 1997 Mitsubishi Electric Information Technology Center
00004 //  America (MEITCA).  All Rights Reserved.
00005 //
00006 //  Permission to use, copy, modify and distribute this software and
00007 //  its documentation for educational, research and non-profit
00008 //  purposes, without fee, and without a written agreement is hereby
00009 //  granted, provided that the above copyright notice and the
00010 //  following three paragraphs appear in all copies.
00011 //
00012 //  Permission to incorporate this software into commercial products
00013 //  may be obtained from MERL - A Mitsubishi Electric Research Lab, 201
00014 //  Broadway, Cambridge, MA 02139.
00015 //
00016 //  IN NO EVENT SHALL MEITCA BE LIABLE TO ANY PARTY FOR DIRECT,
00017 //  INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING
00018 //  LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
00019 //  DOCUMENTATION, EVEN IF MEITCA HAS BEEN ADVISED OF THE POSSIBILITY
00020 //  OF SUCH DAMAGES.
00021 //
00022 //  MEITCA SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT
00023 //  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 //  FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS ON
00025 //  AN "AS IS" BASIS, AND MEITCA HAS NO OBLIGATIONS TO PROVIDE
00026 //  MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
00027 //
00028 //  Author:
00029 //    Brian Mirtich
00030 //    mirtich@merl.com
00031 //    617.621.7573
00032 //    www.merl.com/people/mirtich
00033 //
00035 
00036 
00037 
00038 #ifndef VCLIP_H
00039 #define VCLIP_H
00040 
00041 #include <list>
00042 #include <map>
00043 #include <string.h>
00044 
00045 #include "mv.h"
00046 
00047 #if INVENTOR
00048 #include <Inventor/nodekits/SoShapeKit.h>
00049 #endif
00050 
00051 using namespace std;
00052 namespace Vclip {
00054 // constants
00056 
00057 
00058 #define VF_NAME_SZ  15   // max # of chars in a Face or Vertex name (incl. \0)
00059 #define PTREE_NAME_SZ 80 // max # of chars in a PolyTree name (including \0)
00060 
00061 typedef char VertFaceName[VF_NAME_SZ];
00062 
00063 // Used as the initial size for certain resizable vectors.  Generally,
00064 // the number defined below is plenty big, however exceeding this
00065 // limit at runtime won't break anything.
00066 #define MAX_VERTS_PER_FACE 100
00067 
00068 
00070 // data structures
00072 
00073 template<class T>
00074 class Handle {
00075   T *ptr;
00076 public:
00077   void set(T *p) {ptr = p;}
00078   Handle() {ptr = NULL;}
00079   Handle(T *p) {set(p);}
00080   ~Handle() {delete ptr;}
00081   T* operator->() {return ptr;}
00082   T& operator*() {return *ptr;}
00083   const T* operator->() const {return ptr;}
00084   const T& operator*() const {return *ptr;}
00085 };
00086 
00087 
00088 template<class T>
00089 class ShareHandle {
00090   T *ptr;
00091 public:
00092   void set(T *p) {ptr = p; p->handleCount++;}
00093   ShareHandle() {ptr = NULL;}
00094   ShareHandle(T *p) {set(p);}
00095   ShareHandle(const ShareHandle &orig) {set(orig.ptr);}
00096   ~ShareHandle() {if (ptr && --ptr->handleCount == 0) delete ptr;}
00097   ShareHandle &operator=(const ShareHandle &orig) {
00098     if (ptr && --ptr->handleCount == 0) delete ptr;
00099     set(orig.ptr);
00100     return *this;
00101   };
00102   T *operator->() {return ptr;}
00103   T &operator*() {return *ptr;}
00104   const T* operator->() const {return ptr;}
00105   const T& operator*() const {return *ptr;}
00106 };
00107 
00108 
00109 
00111 // Extensions to SGI's standard library
00113 
00114 // If using this macro, make sure the list variable is not changed
00115 // inside the loop, e.g.:
00116 
00117 // FOR_EACH(num.primeFactors, itor) {
00118 //   ...
00119 //   num = otherNum;  // error!
00120 //   ...
00121 // } 
00122 
00123 #ifndef FOR_EACH 
00124 #define FOR_EACH(list, iterator) \
00125   for(iterator = (list).begin(); iterator != (list).end(); ++iterator)
00126 #endif
00127 
00128 
00130 // typedef VclipPose
00132 
00133 // V-Clip can use either class MatX or class Se3 as the basis for pose
00134 // specification (see mv.h).  Class MatX requires fewer floating point
00135 // operations to transform points and vectors.  However the Se3
00136 // representation may be numerically robust (further study is needed).
00137 // During testing, cases were found where V-Clip returned an incorrect
00138 // disjoint result for penetrating Polyhedra when using the MatX
00139 // representation; this behavior went away when the Se3 representation
00140 // was used.  These cases were extremely contrived and unlikely to
00141 // occur in normal use.  The MatX representation is initially
00142 // recommended for all applications.  If problems occur, consider
00143 // switching to the Se3 representation.  The interface is the same for
00144 // either option, except for the type of the relative pose parameter
00145 // Xr1r2 passed to PolyTree::vclip().
00146 
00147 // define as 1 for MatX pose representation and 0 for Se3 pose
00148 // representation.  
00149 #define VCLIP_MATRIX_POSE 0
00150 
00151 #if VCLIP_MATRIX_POSE
00152 typedef MatX VclipPose;
00153 #else
00154 typedef Se3 VclipPose;
00155 #endif
00156 
00157 
00158 // used in V-Clip to cache transforme geometry
00159 struct XformedGeom {
00160   const class Feature *feat;
00161   Vect3 coords;  
00162   Vect3 tail;
00163   Vect3 head;
00164   Vect3 seg;
00165 };
00166 
00167 
00169 // class Plane
00171 
00172 
00173 class Plane {
00174 
00175   Vect3 normal_;   // plane = { p | < p , normal_ > + offset_ = 0 }
00176   Real offset_;
00177 
00178 public:
00179 
00180   const Vect3 &normal() const {return normal_;}
00181   const Real  &offset() const {return offset_;}
00182 
00183   void set(const Vect3 &normal, const Vect3 &thruPoint)
00184     {normal_ = normal; offset_ = - Vect3::dot(thruPoint, normal);}
00185 
00186   // Compute signed distance from point to plane; assumes unit length normal_
00187   Real dist(const Vect3 &point) const 
00188     {return Vect3::dot(normal_, point) + offset_;}
00189 
00190   ostream& print(ostream &os) const;
00191 };
00192 
00193 
00194 
00196 // struct VertConeNode & FaceConeNode
00198 
00199 
00200 struct VertConeNode
00201 {
00202   const Plane *plane;
00203   class Edge *nbr;     // neighboring edge when plane violated
00204 
00205   ostream& print(ostream &os) const;
00206 
00207 };
00208 
00209 
00210 struct FaceConeNode
00211 {
00212   const Plane *plane;
00213   const class Edge *nbr;     // neighboring edge when plane violated
00214 
00215   const FaceConeNode *ccw, *cw;
00216   int idx;  // ranges from 0 to n-1, where n = number of edges on face
00217 
00218   ostream& print(ostream &os) const;
00219 
00220 };
00221 
00222 
00223 
00225 // class Feature
00227 
00228 class Feature {
00229 
00230 public:
00231   enum Type {VERTEX, EDGE, FACE};
00232 
00233 protected:
00234   Type type_;
00235 
00236 public:
00237   Type type() const {return type_;}
00238   virtual const char *name() const = 0;
00239 };
00240 
00241 
00242 
00244 // closest feature pairs and hash table
00246 
00247 struct PolyTreePair
00248 {
00249   const class PolyTree *first, *second;
00250 };
00251 
00252 inline int operator==(const PolyTreePair &ptree1, const PolyTreePair &ptree2)
00253 {return ptree1.first == ptree2.first && ptree1.second == ptree2.second;}
00254 
00255 inline int operator<(const PolyTreePair &ptree1, const PolyTreePair &ptree2)
00256 {return ptree1 < ptree2;}
00257 
00258 
00259 struct FeaturePair
00260 {
00261   const Feature  *first, *second;
00262   FeaturePair() {first = second = NULL;}
00263 };
00264 
00265 #if 0
00266 struct PolyTreePairHasher : unary_function<PolyTreePair, size_t> {
00267   size_t operator() (const PolyTreePair &ptrees) const
00268     {return ((size_t) ptrees.first) 
00269        ^ (((unsigned int) ptrees.second) << 16) 
00270        ^ (((unsigned int) ptrees.second) >> 16);}
00271 };
00272 #endif
00273 
00274 typedef
00275 std::map<PolyTreePair, FeaturePair>
00276 ClosestFeaturesHT;
00277 
00279 // class Vertex
00281 
00282 
00283 class Vertex : private Feature
00284 {
00285   friend class Polyhedron;
00286   friend class PolyTree;
00287 
00288   Vect3 coords_;
00289   list<VertConeNode> cone;
00290   VertFaceName name_;
00291 
00292 public:
00293   Vertex() {type_ = VERTEX;}
00294   const char *name() const;
00295   const Vect3 &coords() const {return coords_;}
00296 
00297 };
00298 
00299 
00300 
00302 // class Edge
00304 
00305 
00306 class Edge : private Feature
00307 {
00308   friend class Polyhedron;
00309   friend class PolyTree;
00310 
00311   const Vertex *tail, *head;
00312   const class Face *left, *right;
00313   Real len;
00314   Vect3 dir;
00315   Plane tplane, hplane, lplane, rplane;
00316 
00317   Edge() {type_ = EDGE;}
00318 
00319 public:
00320   const char *name() const;
00321 
00322 };
00323 
00324 
00325 
00327 // class Face
00329 
00330 
00331 class Face : private Feature
00332 {
00333   friend class Polyhedron;
00334   friend class PolyTree;
00335 
00336   int sides;     // number of edges around boundary
00337   Plane plane;
00338   list<FaceConeNode> cone;
00339   VertFaceName name_;
00340 
00341   Face() {type_ = FACE;}
00342 
00343 public:
00344   const char *name() const;
00345 
00346 };
00347 
00348 
00349 
00351 // class Polyhedron
00353 
00354 
00355 class Polyhedron
00356 {
00357   friend class PolyTree;
00358   friend class ShareHandle<Polyhedron>;
00359 
00360   int handleCount;  // number of PolyTrees pointing to thie Polyhedron
00361 
00362   list<Vertex> verts_;
00363   list<Edge  > edges_;
00364   list<Face  > faces_;
00365 
00366   void processEdge(Face *f, Vertex *tail, Vertex *head);
00367 
00368   static int vertVertTest(const Feature *&v1, const Feature *&v2, 
00369                           XformedGeom &xv1, XformedGeom &xv2,
00370                           const VclipPose &X12, const VclipPose &X21,
00371                           Vect3 &cp1, Vect3 &cp2, Real &dist);
00372 
00373   static int vertFaceTest(const Feature *&v, const Feature *&f, XformedGeom &xv,
00374                           const VclipPose &Xvf, const list<Face> &allFaces,
00375                           Vect3 &cpv, Vect3 &cpf, Real &dist);
00376 
00377   static int vertEdgeTest(const Feature *&v, const Feature *&e, 
00378                           XformedGeom &xv, XformedGeom &xe,
00379                           const VclipPose &Xve, const VclipPose &Xev,
00380                           Vect3 &cpv, Vect3 &cpe, Real &dist);
00381 
00382   static int edgeEdgeSubtest(const Feature *&e, XformedGeom &xe, Vect3 &cp);
00383 
00384   static int edgeEdgeTest(const Feature *&e1, const Feature *&e2, 
00385                           XformedGeom &xe1, XformedGeom &xe2,
00386                           const VclipPose &X12, const VclipPose &X21,
00387                           Vect3 &cp1, Vect3 &cp2, Real &dist);
00388 
00389   static int edgeFaceTest(const Feature *&e, const Feature *&f, 
00390                           XformedGeom &xe, const VclipPose &Xef, 
00391                           Vect3 &cpe, Vect3 &cpf, Real &dist);
00392 
00393   public: static Real vclip(const Polyhedron *const poly1,
00394                     const Polyhedron *const poly2, 
00395                     const VclipPose &X12, const VclipPose &X21, 
00396                     const Feature *&feat1, const Feature *&feat2,
00397                     Vect3 &cp1, Vect3 &cp2, int oneStep = 0);
00398 
00399 #if INVENTOR
00400   SoShapeKit *buildInvModel() const;
00401 #endif
00402 
00403 public:
00404 
00405   Polyhedron() {handleCount = 0;}
00406 
00407   // construction
00408   inline Vertex *addVertex(const char *name, const Vect3 &coords);
00409   void addFace(const char *name, 
00410                            vector<Vertex *> &verts, int clockwise = 0);
00411   int buildHull();
00412   int check() const;
00413 
00414   // examination
00415   ostream& print(ostream &os) const;
00416   const list<Vertex> &verts() const {return verts_;}
00417   const list<Edge  > &edges() const {return edges_;}
00418   const list<Face  > &faces() const {return faces_;}
00419 };
00420 
00421 
00422 
00424 //  class PolyTree
00426 
00427 // Since the same Polytree's structure may be used multiple times,
00428 // possibly even within the same rigid object, we need a way to refer
00429 // to a specific instance of a given PolyTree.  The Inventor solution
00430 // is to use paths to refer to specific instances of nodes in a scene
00431 // graph, so the node doesn't need to be stored twice.  Our solution
00432 // is simpler: we replicate instances of the PolyTree structures, that
00433 // is, every occurrence of the same PolyTree is uniquely stored.
00434 // Thus, addresses of PolyTree nodes can unambiguously specify
00435 // instances.  The Polyhedron structures remain shared: different
00436 // PolyTree nodes can point to the same Polyhedron.  Since most of the
00437 // storage is in the Polyhedron structures, not the PolyTree nodes,
00438 // this is not too wasteful.  It is also much easier to point to
00439 // sepcific instances of PolyTrees than with the path method.
00440 
00441 // Replication of PolyTree nodes allows us to have different names for
00442 // the different instances of the same PolyTree.  It also allows the
00443 // Tpm field (see below).  If PolyTrees were not replicated, there
00444 // could be no such field since the same PolyTree node could be shared
00445 // by different geometry hierarchies.  In this situation, the only
00446 // alternative is to store a transformation to the parent's frame, and
00447 // accumulate these transformations along a specific path.  The
00448 // replication approach saves time because the Tpm's can be
00449 // precomputed.
00450 
00451 class PolyTree 
00452 {
00453 
00454   friend class PolyTreeLibrary;
00455 
00456   // Pointer to a Polyhedron.  For an atomic PolyTree, this is the
00457   // geometry of the PolyTree itself; for a compound PolyTree, this is
00458   // the geometry of the convex hull.
00459   ShareHandle<Polyhedron> poly_;   
00460 
00461   // Volume integrals, relative to this PolyTree's reference frame
00462   Real vol_;   // volume:                                vol    = int(dV)
00463   Vect3 mov1_; // 1st moment of volume:                  mov1.x = int(x dV)
00464   Vect3 mov2_; // undiagonalized 2nd moment of volume:   mov2.x = int(x^2 dV)
00465   Vect3 pov_;  // product of volume:                     pov.x  = int(yz dV)
00466 
00467   Real rad_;   // "radius" of PolyTree, relative to center of volume
00468 
00469 
00470   // An entire PolyTree shares a common reference frame (r).  Tpr_ and
00471   // Trp_ are the transformations between each PolyTree's local frame
00472   // (p) and the PolyTree reference frame.  These fields are
00473   // recomputed each time the PolyTree is included (replicated) into
00474   // another hierarchy.  By default, the reference frame is simply the
00475   // local frame of the root PolyTree, so for the root node Tpr_ and
00476   // Trp_ are identity transformations.  In some cases, however, it is
00477   // advantageous to store transformations with respect to a different
00478   // frame.  For example, in rigid body simulation, an object's body
00479   // frame is the most useful frame, so all of the PolyTree nodes in
00480   // it's geometry use that as the reference frame.  The reference
00481   // frame of a hierarchy is updated using the xform() method.  Xpr_
00482   // and Xrp_ are the MatX_ equivalents of Tpr_ and Trp_.
00483   Se3 Tpr_, Trp_;
00484   MatX Xpr_, Xrp_;
00485 
00486   list< Handle<PolyTree> > components;  // children in convex decomp'n, if any
00487 
00488 public:
00489   char name[PTREE_NAME_SZ];
00490   
00491 private:
00492 
00493   void printRecur(ostream &os, int level) const;
00494 
00495   // copy constructor (perform a deep copy of this)
00496   PolyTree(const PolyTree &orig);
00497 
00498 public:
00499 
00500   PolyTree();
00501 
00502   // construction
00503   void setPoly(Polyhedron *p) {poly_ = ShareHandle<Polyhedron>(p);}
00504   void addComponent(PolyTree *comp)
00505     {Handle<PolyTree> h(comp); components.push_back(h); h.set(NULL);}
00506   int buildHull();
00507   void xform(const Se3 &T);
00508 
00509   // examination
00510   const Polyhedron  *poly() const {return &*poly_;}
00511   int numNodes() const;
00512   int numLeaves() const;
00513   ostream& print(ostream &os) const;
00514   const Se3   &Tpr()  const {return Tpr_;}
00515 #if INVENTOR
00516   SoNode *buildInvModel() const;
00517 #endif
00518 
00519   // volume integrals
00520   void compVolInts();
00521   const Real  &vol()  const {return vol_;}
00522   const Vect3 &mov1() const {return mov1_;}
00523   const Vect3 &mov2() const {return mov2_;}
00524   const Vect3 &pov()  const {return pov_;}
00525   const Real  &rad()  const {return rad_;}
00526 
00527   // V-Clip
00528 private:
00529   static Real vclip_(
00530                      const PolyTree *const ptree1, const PolyTree *const ptree2,
00531                      const VclipPose &Xr1r2, const VclipPose &Xr2r1,
00532                      ClosestFeaturesHT &ht,
00533                      Vect3 &cp1, Vect3 &cp2);
00534 public:
00535 
00536   // Application callpoint for V-Clip.  Xr1r2 is the transformation
00537   // from the reference frame of ptree1 to the reference frame of
00538   // ptree2.  Cp1 and cp2 are also returned in these frames.
00539   inline static Real vclip(const PolyTree *const ptree1, 
00540                            const PolyTree *const ptree2,
00541                            const VclipPose &Xr1r2, 
00542                            ClosestFeaturesHT &ht,
00543                            Vect3 &cp1, Vect3 &cp2)
00544     {
00545       VclipPose Xr2r1;
00546       Xr2r1.invert(Xr1r2);
00547       return vclip_(ptree1, ptree2, Xr1r2, Xr2r1, ht, cp1, cp2);
00548     }
00549 
00550   // V-Clip does not return closest features to the caller.  If they're
00551   // needed, this function returns the most recent feature pair for the
00552   // given PolyTrees, or NULL is no hash table entry can be found.
00553   static void vclipFeatures(
00554                             const PolyTree *const ptree1, 
00555                             const PolyTree *const ptree2,
00556                             ClosestFeaturesHT &ht,
00557                             const Feature *&feat1, const Feature *&feat2);
00558 };
00559 
00560 
00561 
00563 //  class PolyTreeLibrary
00565 
00566 
00567 class PolyTreeLibrary
00568 {
00569   list< Handle<PolyTree> > lib;
00570 
00571 public:
00572 
00573   void clear() {lib.clear();}                      // clear out the library
00574   int size() {return lib.size();}                  // return # of entries in lib
00575 
00576   void add(PolyTree *pt)                           // add PolyTree to library
00577     {Handle<PolyTree> h(pt); lib.push_front(h); h.set(NULL);}
00578 
00579   const PolyTree *lookup(const char *name) const;  // lookup by name
00580   const PolyTree *lookup(int i) const;             // lookup by index
00581   
00582   PolyTree *create(const char *name) const         // instantiate by name
00583     {const PolyTree *pt; return (pt = lookup(name)) ? new PolyTree(*pt) : NULL;}
00584 
00585   PolyTree *create(int i) const                    // instantiate by index
00586     {const PolyTree *pt; return (pt = lookup(i)) ? new PolyTree(*pt) : NULL;}
00587 };
00588   
00589 
00591 //  stream operators
00593 
00594 
00595 inline ostream &operator<<(ostream &os, const Plane &p) {return p.print(os);}
00596 inline ostream &operator<<(ostream &os, const VertConeNode &vcn) 
00597   {return vcn.print(os);}
00598 inline ostream &operator<<(ostream &os, const FaceConeNode &fcn) 
00599   {return fcn.print(os);}
00600 inline ostream &operator<<(ostream &os, const Polyhedron *poly)
00601   {return poly->print(os);}
00602 inline ostream &operator<<(ostream &os, const PolyTree *pt) 
00603   {return pt->print(os);}
00604 
00605 
00606 
00608 //  inline methods
00610 
00611 
00612 Vertex *Polyhedron::addVertex(const char *name, const Vect3 &coords)
00613 {
00614   Vertex v;
00615   v.coords_ = coords;
00616   strcpy(v.name_, name);
00617   verts_.push_back(v);
00618   return &verts_.back();
00619 }
00620 
00621 
00622 
00624 //  default goemetry readers
00626 
00627 // Provided for convenience; not part of API.
00628 
00629 int loadPolyTreeFile(const char *fname, PolyTreeLibrary &library);
00630 };
00631 #endif  // #ifndef VCLIP_H


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:19