PQP_Internal.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 #include "Tri.h"
00042 #include "BV.h"
00043 
00044 class PQP_Model
00045 {
00046 
00047 public:
00048 
00049   int build_state;
00050 
00051   Tri *tris;  
00052   int num_tris;
00053   int num_tris_alloced;
00054 
00055   BV *b;
00056   int num_bvs;
00057   int num_bvs_alloced;
00058 
00059   Tri *last_tri;       // closest tri on this model in last distance test
00060   
00061   BV *child(int n) { return &b[n]; }
00062 
00063   PQP_Model();
00064   ~PQP_Model();
00065 
00066   int BeginModel(int num_tris = 8); // preallocate for num_tris triangles;
00067                                     // the parameter is optional, since
00068                                     // arrays are reallocated as needed
00069   int AddTri(const PQP_REAL *p1, const PQP_REAL *p2, const PQP_REAL *p3,
00070 // added by yamane ->
00071                          const PQP_REAL *n1, const PQP_REAL *n2, const PQP_REAL *n3,
00072                          int _vertex_id[3], int _neighbor_id[3],
00073 // <-
00074              int id);
00075   int EndModel();
00076   int MemUsage(int msg);  // returns model mem usage.  
00077                           // prints message to stderr if msg == TRUE
00078 };
00079 
00080 struct CollisionPair
00081 {
00082   int id1;
00083   int id2;
00084   PQP_REAL depth;
00085   PQP_REAL position1[3];
00086   int vertex_id1;
00087   PQP_REAL position2[3];
00088   int vertex_id2;
00089   PQP_REAL normal[3];
00090 };
00091 
00092 struct PQP_CollideResult  
00093 {
00094   // stats
00095 
00096   int num_bv_tests;
00097   int num_tri_tests;
00098   double query_time_secs;
00099 
00100   // xform from model 1 to model 2
00101 
00102   PQP_REAL RR[3][3];
00103   PQP_REAL T[3];
00104 
00105   int num_pairs_alloced;
00106   int num_pairs;
00107   CollisionPair *pairs;
00108 
00109   void SizeTo(int n);    
00110   void Add(int i1, int i2, PQP_REAL _depth, PQP_REAL position1[3], int _vertex_id1, PQP_REAL position2[3], int _vertex_id2, PQP_REAL normal[3]);
00111         void Remove(int id);
00112 
00113         int have_vertex1(int vertex_id) {
00114                 if(vertex_id < 0) return -1;
00115                 for(int i=0; i<num_pairs; i++)
00116                 {
00117                         if(pairs[i].vertex_id1 == vertex_id) return i;
00118                 }
00119                 return -1;
00120         }
00121         int have_vertex2(int vertex_id) {
00122                 if(vertex_id < 0) return -1;
00123                 for(int i=0; i<num_pairs; i++)
00124                 {
00125                         if(pairs[i].vertex_id2 == vertex_id) return i;
00126                 }
00127                 return -1;
00128         }
00129         
00130 
00131   PQP_CollideResult();
00132   ~PQP_CollideResult();
00133 
00134   // statistics
00135 
00136   int NumBVTests() { return num_bv_tests; }
00137   int NumTriTests() { return num_tri_tests; }
00138   double QueryTimeSecs() { return query_time_secs; }
00139 
00140   // free the list of contact pairs; ordinarily this list is reused
00141   // for each query, and only deleted in the destructor.
00142 
00143   void FreePairsList(); 
00144 
00145   // query results
00146 
00147   int Colliding() { return (num_pairs > 0); }
00148   int NumPairs() { return num_pairs; }
00149   int Id1(int k) { return pairs[k].id1; }
00150   int Id2(int k) { return pairs[k].id2; }
00151         PQP_REAL Depth(int k) {
00152                 return pairs[k].depth;
00153         }
00154         void Position1(int k, PQP_REAL _pos[3]) {
00155                 _pos[0] = pairs[k].position1[0];
00156                 _pos[1] = pairs[k].position1[1];
00157                 _pos[2] = pairs[k].position1[2];
00158         }
00159         void Position2(int k, PQP_REAL _pos[3]) {
00160                 _pos[0] = pairs[k].position2[0];
00161                 _pos[1] = pairs[k].position2[1];
00162                 _pos[2] = pairs[k].position2[2];
00163         }
00164         void Normal(int k, PQP_REAL _norm[3]) {
00165                 _norm[0] = pairs[k].normal[0];
00166                 _norm[1] = pairs[k].normal[1];
00167                 _norm[2] = pairs[k].normal[2];
00168         }
00169         
00170 };
00171 
00172 #if PQP_BV_TYPE & RSS_TYPE // distance/tolerance are only available with RSS
00173 
00174 struct PQP_DistanceResult 
00175 {
00176   // stats
00177 
00178   int num_bv_tests;
00179   int num_tri_tests;
00180   double query_time_secs;
00181 
00182   // xform from model 1 to model 2
00183 
00184   PQP_REAL RR[3][3];
00185   PQP_REAL T[3];
00186 
00187   PQP_REAL rel_err; 
00188   PQP_REAL abs_err; 
00189 
00190   PQP_REAL distance;
00191   PQP_REAL p1[3]; 
00192   PQP_REAL p2[3];
00193   int qsize;
00194         int id1;
00195         int id2;
00196   
00197   // statistics
00198 
00199   int NumBVTests() { return num_bv_tests; }
00200   int NumTriTests() { return num_tri_tests; }
00201   double QueryTimeSecs() { return query_time_secs; }
00202 
00203   // The following distance and points established the minimum distance
00204   // for the models, within the relative and absolute error bounds 
00205   // specified.
00206   // Points are defined: PQP_REAL p1[3], p2[3];
00207 
00208   PQP_REAL Distance() { return distance; }
00209   const PQP_REAL *P1() { return p1; }
00210   const PQP_REAL *P2() { return p2; }
00211         int ID1() { return id1; }
00212         int ID2() { return id2; }
00213 
00214 };
00215 
00216 struct PQP_ToleranceResult 
00217 {
00218   // stats
00219 
00220   int num_bv_tests;
00221   int num_tri_tests;
00222   double query_time_secs;
00223 
00224   // xform from model 1 to model 2
00225 
00226   PQP_REAL RR[3][3];
00227   PQP_REAL T[3];
00228 
00229   int    closer_than_tolerance;   
00230   PQP_REAL tolerance;      
00231 
00232   PQP_REAL distance;
00233   PQP_REAL p1[3]; 
00234   PQP_REAL p2[3]; 
00235   int qsize;
00236 
00237   // statistics
00238 
00239   int NumBVTests() { return num_bv_tests; }
00240   int NumTriTests() { return num_tri_tests; }
00241   double QueryTimeSecs() { return query_time_secs; }
00242 
00243   // If the models are closer than ( <= ) tolerance, these points 
00244   // and distance were what established this.  Otherwise, 
00245   // distance and point values are not meaningful.
00246 
00247   PQP_REAL Distance() { return distance; }
00248   const PQP_REAL *P1() { return p1; }
00249   const PQP_REAL *P2() { return p2; }
00250 
00251   // boolean says whether models are closer than tolerance distance
00252 
00253   int CloserThanTolerance() { return closer_than_tolerance; }
00254 };
00255 
00256 #endif


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:56