PQP.h
Go to the documentation of this file.
00001 /*************************************************************************\
00002 
00003   Copyright 1999 The University of North Carolina at Chapel Hill.
00004   All Rights Reserved.
00005 
00006   Permission to use, copy, modify and distribute this software and its
00007   documentation for educational, research and non-profit purposes, without
00008   fee, and without a written agreement is hereby granted, provided that the
00009   above copyright notice and the following three paragraphs appear in all
00010   copies.
00011 
00012   IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
00013   LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
00014   CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
00015   USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
00016   OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
00017   DAMAGES.
00018 
00019   THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
00020   WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
00021   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
00022   PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
00023   NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
00024   UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
00025 
00026   The authors may be contacted via:
00027 
00028   US Mail:             S. Gottschalk, E. Larsen
00029                        Department of Computer Science
00030                        Sitterson Hall, CB #3175
00031                        University of N. Carolina
00032                        Chapel Hill, NC 27599-3175
00033 
00034   Phone:               (919)962-1749
00035 
00036   EMail:               geom@cs.unc.edu
00037 
00038 
00039 \**************************************************************************/
00040 
00041 #ifndef PQP_H
00042 #define PQP_H
00043 
00044 #include "PQP_Compile.h"   
00045 #include "PQP_Internal.h"                             
00046                         
00047 //----------------------------------------------------------------------------
00048 //
00049 //  PQP API Return Values
00050 //
00051 //----------------------------------------------------------------------------
00052 
00053 const int PQP_OK = 0; 
00054   // Used by all API routines upon successful completion except
00055   // constructors and destructors
00056 
00057 const int PQP_ERR_MODEL_OUT_OF_MEMORY = -1; 
00058   // Returned when an API function cannot obtain enough memory to
00059   // store or process a PQP_Model object.
00060 
00061 const int PQP_ERR_OUT_OF_MEMORY = -2;
00062   // Returned when a PQP query cannot allocate enough storage to
00063   // compute or hold query information.  In this case, the returned
00064   // data should not be trusted.
00065 
00066 const int PQP_ERR_UNPROCESSED_MODEL = -3;
00067   // Returned when an unprocessed model is passed to a function which
00068   // expects only processed models, such as PQP_Collide() or
00069   // PQP_Distance().
00070 
00071 const int PQP_ERR_BUILD_OUT_OF_SEQUENCE = -4;
00072   // Returned when: 
00073   //       1. AddTri() is called before BeginModel().  
00074   //       2. BeginModel() is called immediately after AddTri().  
00075   // This error code is something like a warning: the invoked
00076   // operation takes place anyway, and PQP does what makes "most
00077   // sense", but the returned error code may tip off the client that
00078   // something out of the ordinary is happenning.
00079 
00080 const int PQP_ERR_BUILD_EMPTY_MODEL = -5; 
00081   // Returned when EndModel() is called on a model to which no
00082   // triangles have been added.  This is similar in spirit to the
00083   // OUT_OF_SEQUENCE return code, except that the requested operation
00084   // has FAILED -- the model remains "unprocessed", and the client may
00085   // NOT use it in queries.
00086 
00087 //----------------------------------------------------------------------------
00088 //
00089 //  PQP_REAL 
00090 //
00091 //  The floating point type used throughout the package. The type is defined 
00092 //  in PQP_Compile.h, and by default is "double"
00093 //
00094 //----------------------------------------------------------------------------
00095 
00096 //----------------------------------------------------------------------------
00097 //
00098 //  PQP_Model
00099 //
00100 //  A PQP_Model stores geometry to be used in a proximity query.
00101 //  The geometry is loaded with a call to BeginModel(), at least one call to 
00102 //  AddTri(), and then a call to EndModel().
00103 //
00104 //  // create a two triangle model, m
00105 //
00106 //  PQP_Model m;
00107 //
00108 //  PQP_REAL p1[3],p2[3],p3[3];  // 3 points will make triangle p
00109 //  PQP_REAL q1[3],q2[3],q3[3];  // another 3 points for triangle q
00110 //
00111 //  // some initialization of these vertices not shown
00112 //
00113 //  m.BeginModel();              // begin the model
00114 //  m.AddTri(p1,p2,p3,0);        // add triangle p
00115 //  m.AddTri(q1,q2,q3,1);        // add triangle q
00116 //  m.EndModel();                // end (build) the model
00117 //
00118 //  The last parameter of AddTri() is the number to be associated with the 
00119 //  triangle. These numbers are used to identify the triangles that overlap.
00120 // 
00121 //  AddTri() copies into the PQP_Model the data pointed to by the three vertex 
00122 //  pointers, so that it is safe to delete vertex data after you have 
00123 //  passed it to AddTri().
00124 //
00125 //----------------------------------------------------------------------------
00126 //
00127 //  class PQP_Model  - declaration contained in PQP_Internal.h
00128 //  {
00129 //
00130 //  public:
00131 //    PQP_Model();
00132 //    ~PQP_Model();
00133 //
00134 //    int BeginModel(int num_tris = 8); // preallocate for num_tris triangles;
00135 //                                      // the parameter is optional, since
00136 //                                      // arrays are reallocated as needed
00137 //
00138 //    int AddTri(const PQP_REAL *p1, const PQP_REAL *p2, const PQP_REAL *p3, 
00139 //               int id);
00140 //
00141 //    int EndModel();
00142 //    int MemUsage(int msg);  // returns model mem usage in bytes
00143 //                            // prints message to stderr if msg == TRUE
00144 //  };
00145 
00146 //----------------------------------------------------------------------------
00147 //
00148 //  PQP_CollideResult 
00149 //
00150 //  This saves and reports results from a collision query.  
00151 //
00152 //----------------------------------------------------------------------------
00153 //
00154 //  struct PQP_CollideResult - declaration contained in PQP_Internal.h
00155 //  {
00156 //    // statistics
00157 //
00158 //    int NumBVTests();
00159 //    int NumTriTests();
00160 //    PQP_REAL QueryTimeSecs();
00161 //
00162 //    // free the list of contact pairs; ordinarily this list is reused
00163 //    // for each query, and only deleted in the destructor.
00164 //
00165 //    void FreePairsList(); 
00166 //
00167 //    // query results
00168 //
00169 //    int Colliding();
00170 //    int NumPairs();
00171 //    int Id1(int k);
00172 //    int Id2(int k);
00173 //  };
00174 
00175 //----------------------------------------------------------------------------
00176 //
00177 //  PQP_Collide() - detects collision between two PQP_Models
00178 //
00179 //
00180 //  Declare a PQP_CollideResult struct and pass its pointer to collect 
00181 //  collision data.
00182 //
00183 //  [R1, T1] is the placement of model 1 in the world &
00184 //  [R2, T2] is the placement of model 2 in the world.
00185 //  The columns of each 3x3 matrix are the basis vectors for the model
00186 //  in world coordinates, and the matrices are in row-major order:
00187 //  R(row r, col c) = R[r][c].
00188 //
00189 //  If PQP_ALL_CONTACTS is the flag value, after calling PQP_Collide(),
00190 //  the PQP_CollideResult object will contain an array with all
00191 //  colliding triangle pairs. Suppose CR is a pointer to the
00192 //  PQP_CollideResult object.  The number of pairs is gotten from
00193 //  CR->NumPairs(), and the ids of the 15'th pair of colliding
00194 //  triangles is gotten from CR->Id1(14) and CR->Id2(14).
00195 //
00196 //  If PQP_FIRST_CONTACT is the flag value, the PQP_CollideResult array
00197 //  will only get the first colliding triangle pair found.  Thus
00198 //  CR->NumPairs() will be at most 1, and if 1, CR->Id1(0) and
00199 //  CR->Id2(0) give the ids of the colliding triangle pair.
00200 //
00201 //----------------------------------------------------------------------------
00202 
00203 const int PQP_ALL_CONTACTS = 1;  // find all pairwise intersecting triangles
00204 const int PQP_FIRST_CONTACT = 2; // report first intersecting tri pair found
00205 
00206 int 
00207 PQP_Collide(PQP_CollideResult *result,
00208             PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1,
00209             PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2,
00210             int flag = PQP_ALL_CONTACTS);
00211 
00212 int PQP_TriLineIntersect(PQP_CollideResult *result,
00213                                                  PQP_REAL RR[3][3], PQP_REAL T[3], PQP_Model* o1,
00214                                                  PQP_REAL P1[3], PQP_REAL P2[3],
00215                                                  int flag = PQP_ALL_CONTACTS);
00216 
00217 
00218 #if PQP_BV_TYPE & RSS_TYPE  // this is true by default,
00219                             // and explained in PQP_Compile.h
00220 
00221 //----------------------------------------------------------------------------
00222 //
00223 //  PQP_DistanceResult
00224 //
00225 //  This saves and reports results from a distance query.  
00226 //
00227 //----------------------------------------------------------------------------
00228 //
00229 //  struct PQP_DistanceResult - declaration contained in PQP_Internal.h
00230 //  {
00231 //    // statistics
00232 //  
00233 //    int NumBVTests();
00234 //    int NumTriTests();
00235 //    PQP_REAL QueryTimeSecs();
00236 //  
00237 //    // The following distance and points established the minimum distance
00238 //    // for the models, within the relative and absolute error bounds 
00239 //    // specified.
00240 //
00241 //    PQP_REAL Distance();
00242 //    const PQP_REAL *P1();  // pointers to three PQP_REALs
00243 //    const PQP_REAL *P2();  
00244 //  };
00245 
00246 //----------------------------------------------------------------------------
00247 //
00248 //  PQP_Distance() - computes the distance between two PQP_Models
00249 //
00250 //
00251 //  Declare a PQP_DistanceResult struct and pass its pointer to collect
00252 //  distance information.
00253 //
00254 //  "rel_err" is the relative error margin from actual distance.
00255 //  "abs_err" is the absolute error margin from actual distance.  The
00256 //  smaller of the two will be satisfied, so set one large to nullify
00257 //  its effect.
00258 //
00259 //  "qsize" is an optional parameter controlling the size of a priority
00260 //  queue used to direct the search for closest points.  A larger queue
00261 //  can help the algorithm discover the minimum with fewer steps, but
00262 //  will increase the cost of each step. It is not beneficial to increase
00263 //  qsize if the application has frame-to-frame coherence, i.e., the
00264 //  pair of models take small steps between each call, since another
00265 //  speedup trick already accelerates this situation with no overhead.
00266 //
00267 //  However, a queue size of 100 to 200 has been seen to save time in a
00268 //  planning application with "non-coherent" placements of models.
00269 //
00270 //----------------------------------------------------------------------------
00271 
00272 int 
00273 PQP_Distance(PQP_DistanceResult *result, 
00274              PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1,
00275              PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2,
00276              PQP_REAL rel_err, PQP_REAL abs_err,
00277              int qsize = 2);
00278 
00279 //----------------------------------------------------------------------------
00280 //
00281 //  PQP_ToleranceResult
00282 //
00283 //  This saves and reports results from a tolerance query.  
00284 //
00285 //----------------------------------------------------------------------------
00286 //
00287 //  struct PQP_ToleranceResult - declaration contained in PQP_Internal.h
00288 //  {
00289 //    // statistics
00290 //  
00291 //    int NumBVTests(); 
00292 //    int NumTriTests();
00293 //    PQP_REAL QueryTimeSecs();
00294 //  
00295 //    // If the models are closer than ( <= ) tolerance, these points 
00296 //    // and distance were what established this.  Otherwise, 
00297 //    // distance and point values are not meaningful.
00298 //  
00299 //    PQP_REAL Distance();
00300 //    const PQP_REAL *P1();
00301 //    const PQP_REAL *P2();
00302 //  
00303 //    // boolean says whether models are closer than tolerance distance
00304 //  
00305 //    int CloserThanTolerance();
00306 //  };
00307 
00308 //----------------------------------------------------------------------------
00309 //
00310 // PQP_Tolerance() - checks if distance between PQP_Models is <= tolerance
00311 //
00312 //
00313 // Declare a PQP_ToleranceResult and pass its pointer to collect
00314 // tolerance information.
00315 //
00316 // The algorithm returns whether the true distance is <= or >
00317 // "tolerance".  This routine does not simply compute true distance
00318 // and compare to the tolerance - models can often be shown closer or
00319 // farther than the tolerance more trivially.  In most cases this
00320 // query should run faster than a distance query would on the same
00321 // models and configurations.
00322 // 
00323 // "qsize" again controls the size of a priority queue used for
00324 // searching.  Not setting qsize is the current recommendation, since
00325 // increasing it has only slowed down our applications.
00326 //
00327 //----------------------------------------------------------------------------
00328 
00329 int
00330 PQP_Tolerance(PQP_ToleranceResult *res, 
00331               PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1,
00332               PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2,
00333               PQP_REAL tolerance,
00334               int qsize = 2);
00335 
00336 #endif 
00337 #endif
00338 
00339 
00340 
00341 
00342 
00343 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:18