collision_checking: collision_checking: collision

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #include "collision_checking/collision_primitive.h"
00038 
00039 namespace collision_checking
00040 {
00041 
00042 BVH_CollideResult::BVH_CollideResult()
00043 {
00044   num_bv_tests = 0;
00045   num_tri_tests = 0;
00046   num_vf_tests = 0;
00047   num_ee_tests = 0;
00048 
00049   query_time_seconds = 0;
00050 
00051   num_pairs = num_pairs_allocated = 0;
00052   pairs = NULL;
00053 
00054   num_max_contacts = 0;
00055 }
00056 
00057 
00058 BVH_CollideResult::~BVH_CollideResult()
00059 {
00060   delete [] pairs;
00061 }
00062 
00063 
00064 void BVH_CollideResult::sizeTo(int n)
00065 {
00066   BVHCollisionPair* temp;
00067 
00068   if(n < num_pairs) return;
00069   temp = new BVHCollisionPair[n];
00070   memcpy(temp, pairs, sizeof(BVHCollisionPair) * num_pairs);
00071   delete [] pairs;
00072   pairs = temp;
00073   num_pairs_allocated = n;
00074   return;
00075 }
00076 
00077 void BVH_CollideResult::add(int id1, int id2, BVH_REAL time)
00078 {
00079   if(num_pairs >= num_pairs_allocated)
00080     sizeTo(num_pairs_allocated * 2 + 8);
00081 
00082   pairs[num_pairs].id1 = id1;
00083   pairs[num_pairs].id2 = id2;
00084   pairs[num_pairs].collide_time = time;
00085   num_pairs++;
00086 }
00087 
00088 void BVH_CollideResult::add(int id1, int id2, Vec3f contact_point, BVH_REAL penetration_depth, const Vec3f& normal, BVH_REAL time)
00089 {
00090   if(num_pairs >= num_pairs_allocated)
00091     sizeTo(num_pairs_allocated * 2 + 8);
00092 
00093   pairs[num_pairs].id1 = id1;
00094   pairs[num_pairs].id2 = id2;
00095   pairs[num_pairs].collide_time = time;
00096   pairs[num_pairs].penetration_depth = penetration_depth;
00097   pairs[num_pairs].normal = normal;
00098   pairs[num_pairs].contact_point = contact_point;
00099   num_pairs++;
00100 }
00101 
00102 
00103 void collideRecurse(BVNode<OBB>* tree1, BVNode<OBB>* tree2,
00104                     const Vec3f R[3], const Vec3f& T,
00105                     int b1, int b2,
00106                     Vec3f* vertices1, Vec3f* vertices2,
00107                     Triangle* tri_indices1, Triangle* tri_indices2,
00108                     BVH_CollideResult* res, BVHFrontList* front_list)
00109 {
00110   BVNode<OBB>* node1 = tree1 + b1;
00111   BVNode<OBB>* node2 = tree2 + b2;
00112 
00113   bool l1 = node1->isLeaf();
00114   bool l2 = node2->isLeaf();
00115 
00116   if(l1 && l2)
00117   {
00118     if(front_list) front_list->push_back(BVHFrontNode(b1, b2));
00119 
00120     res->num_bv_tests++;
00121     if(!overlap(R, T, node1->bv, node2->bv)) return;
00122 
00123     res->num_tri_tests++;
00124 
00125     const Triangle& tri_id1 = tri_indices1[-node1->first_child - 1];
00126     const Triangle& tri_id2 = tri_indices2[-node2->first_child - 1];
00127 
00128     const Vec3f& p1 = vertices1[tri_id1[0]];
00129     const Vec3f& p2 = vertices1[tri_id1[1]];
00130     const Vec3f& p3 = vertices1[tri_id1[2]];
00131 
00132     const Vec3f& q1 = vertices2[tri_id2[0]];
00133     const Vec3f& q2 = vertices2[tri_id2[1]];
00134     const Vec3f& q3 = vertices2[tri_id2[2]];
00135 
00136     BVH_REAL penetration;
00137     Vec3f normal;
00138     int n_contacts;
00139     Vec3f contacts[2];
00140 
00141 
00142     if(res->num_max_contacts == 0) // only interested in collision or not
00143     {
00144       if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
00145                                        R, T))
00146       {
00147           res->add(-node1->first_child - 1, -node2->first_child - 1);
00148       }
00149     }
00150     else  // need compute the contact information
00151     {
00152       if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
00153                                        R, T,
00154                                        contacts,
00155                                        (unsigned int*)&n_contacts,
00156                                        &penetration,
00157                                        &normal))
00158       {
00159         for(int i = 0; i < n_contacts; ++i)
00160         {
00161           if(res->num_max_contacts <= res->numPairs()) break;
00162           res->add(-node1->first_child - 1, -node2->first_child - 1, contacts[i], penetration, normal);
00163         }
00164       }
00165     }
00166 
00167 
00168 
00169     return;
00170   }
00171 
00172   res->num_bv_tests++;
00173   if(!overlap(R, T, node1->bv, node2->bv))
00174   {
00175     if(front_list) front_list->push_back(BVHFrontNode(b1, b2));
00176     return;
00177   }
00178 
00179   BVH_REAL sz1 = node1->bv.size();
00180   BVH_REAL sz2 = node2->bv.size();
00181 
00182   if(l2 || (!l1 && (sz1 > sz2)))
00183   {
00184     int c1 = node1->first_child;
00185     int c2 = c1 + 1;
00186 
00187     collideRecurse(tree1, tree2, R, T, c1, b2, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00188 
00189     if(res->numPairs() > 0 && ((res->num_max_contacts == 0) || (res->num_max_contacts <= res->numPairs()))) return;
00190 
00191     collideRecurse(tree1, tree2, R, T, c2, b2, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00192   }
00193   else
00194   {
00195     int c1 = node2->first_child;
00196     int c2 = c1 + 1;
00197 
00198     collideRecurse(tree1, tree2, R, T, b1, c1, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00199 
00200     if(res->numPairs() > 0 && ((res->num_max_contacts == 0) || (res->num_max_contacts <= res->numPairs()))) return;
00201 
00202     collideRecurse(tree1, tree2, R, T, b1, c2, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00203   }
00204 }
00205 
00206 void collideRecurse(BVNode<RSS>* tree1, BVNode<RSS>* tree2,
00207                     const Vec3f R[3], const Vec3f& T,
00208                     int b1, int b2,
00209                     Vec3f* vertices1, Vec3f* vertices2,
00210                     Triangle* tri_indices1, Triangle* tri_indices2,
00211                     BVH_CollideResult* res, BVHFrontList* front_list)
00212 {
00213   BVNode<RSS>* node1 = tree1 + b1;
00214   BVNode<RSS>* node2 = tree2 + b2;
00215 
00216   bool l1 = node1->isLeaf();
00217   bool l2 = node2->isLeaf();
00218 
00219   if(l1 && l2)
00220   {
00221     if(front_list) front_list->push_back(BVHFrontNode(b1, b2));
00222 
00223     res->num_bv_tests++;
00224     if(!overlap(R, T, node1->bv, node2->bv)) return;
00225 
00226     res->num_tri_tests++;
00227 
00228     const Triangle& tri_id1 = tri_indices1[-node1->first_child - 1];
00229     const Triangle& tri_id2 = tri_indices2[-node2->first_child - 1];
00230 
00231     const Vec3f& p1 = vertices1[tri_id1[0]];
00232     const Vec3f& p2 = vertices1[tri_id1[1]];
00233     const Vec3f& p3 = vertices1[tri_id1[2]];
00234 
00235     const Vec3f& q1 = vertices2[tri_id2[0]];
00236     const Vec3f& q2 = vertices2[tri_id2[1]];
00237     const Vec3f& q3 = vertices2[tri_id2[2]];
00238 
00239     BVH_REAL penetration;
00240     Vec3f normal;
00241     int n_contacts;
00242     Vec3f contacts[2];
00243 
00244 
00245     if(res->num_max_contacts == 0) // only interested in collision or not
00246     {
00247       if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
00248                                        R, T))
00249       {
00250           res->add(-node1->first_child - 1, -node2->first_child - 1);
00251       }
00252     }
00253     else // need compute the contact information
00254     {
00255       if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
00256                                        R, T,
00257                                        contacts,
00258                                        (unsigned int*)&n_contacts,
00259                                        &penetration,
00260                                        &normal))
00261       {
00262         for(int i = 0; i < n_contacts; ++i)
00263         {
00264           if(res->num_max_contacts <= res->numPairs()) break;
00265           res->add(-node1->first_child - 1, -node2->first_child - 1, contacts[i], penetration, normal);
00266         }
00267       }
00268     }
00269 
00270 
00271     return;
00272   }
00273 
00274   res->num_bv_tests++;
00275   if(!overlap(R, T, node1->bv, node2->bv))
00276   {
00277     if(front_list) front_list->push_back(BVHFrontNode(b1, b2));
00278     return;
00279   }
00280 
00281   BVH_REAL sz1 = node1->bv.size();
00282   BVH_REAL sz2 = node2->bv.size();
00283 
00284   if(l2 || (!l1 && (sz1 > sz2)))
00285   {
00286     int c1 = node1->first_child;
00287     int c2 = c1 + 1;
00288 
00289     collideRecurse(tree1, tree2, R, T, c1, b2, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00290 
00291     if(res->numPairs() > 0 && ((res->num_max_contacts == 0) || (res->num_max_contacts <= res->numPairs()))) return;
00292 
00293     collideRecurse(tree1, tree2, R, T, c2, b2, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00294   }
00295   else
00296   {
00297     int c1 = node2->first_child;
00298     int c2 = c1 + 1;
00299 
00300     collideRecurse(tree1, tree2, R, T, b1, c1, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00301 
00302     if(res->numPairs() > 0 && ((res->num_max_contacts == 0) || (res->num_max_contacts <= res->numPairs()))) return;
00303 
00304     collideRecurse(tree1, tree2, R, T, b1, c2, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00305   }
00306 }
00307 
00308 void propagateBVHFrontList(BVNode<OBB>* tree1, BVNode<OBB>* tree2,
00309                            Vec3f R[3], const Vec3f& T,
00310                            Vec3f* vertices1, Vec3f* vertices2,
00311                            Triangle* tri_indices1, Triangle* tri_indices2,
00312                            BVH_CollideResult* res,
00313                            BVHFrontList* front_list)
00314 {
00315   BVHFrontList::iterator front_iter;
00316   BVHFrontList append;
00317   for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter)
00318   {
00319     int b1 = front_iter->left;
00320     int b2 = front_iter->right;
00321     BVNode<OBB>* node1 = tree1 + b1;
00322     BVNode<OBB>* node2 = tree2 + b2;
00323 
00324 
00325     bool l1 = node1->isLeaf();
00326     bool l2 = node2->isLeaf();
00327 
00328     if(l1 & l2)
00329     {
00330       front_iter->valid = false; // the front node is no longer valid, in collideRecurse will add again.
00331       collideRecurse(tree1, tree2, R, T, b1, b2, vertices1, vertices2, tri_indices1, tri_indices2, res, &append);
00332     }
00333     else
00334     {
00335       res->num_bv_tests++;
00336       if(!overlap(R, T, node1->bv, node2->bv))
00337       {
00338         front_iter->valid = false; // the front node is no longer valid
00339 
00340         BVH_REAL sz1 = node1->bv.size();
00341         BVH_REAL sz2 = node2->bv.size();
00342 
00343         if(l2 || (!l1 && (sz1 > sz2)))
00344         {
00345           int c1 = node1->first_child;
00346           int c2 = c1 + 1;
00347 
00348           collideRecurse(tree1, tree2, R, T, c1, b2, vertices1, vertices2, tri_indices1, tri_indices2, res, &append);
00349 
00350           collideRecurse(tree1, tree2, R, T, c2, b2, vertices1, vertices2, tri_indices1, tri_indices2, res, &append);
00351         }
00352         else
00353         {
00354           int c1 = node2->first_child;
00355           int c2 = c1 + 1;
00356 
00357           collideRecurse(tree1, tree2, R, T, b1, c1, vertices1, vertices2, tri_indices1, tri_indices2, res, &append);
00358 
00359           collideRecurse(tree1, tree2, R, T, b1, c2, vertices1, vertices2, tri_indices1, tri_indices2, res, &append);
00360         }
00361       }
00362     }
00363   }
00364 
00365 
00366   // clean the old front list (remove invalid node)
00367   for(front_iter = front_list->begin(); front_iter != front_list->end();)
00368   {
00369     if(!front_iter->valid)
00370       front_iter = front_list->erase(front_iter);
00371     else
00372       ++front_iter;
00373   }
00374 
00375   for(front_iter = append.begin(); front_iter != append.end(); ++front_iter)
00376   {
00377     front_list->push_back(*front_iter);
00378   }
00379 }
00380 
00381 
00382 void propagateBVHFrontList(BVNode<RSS>* tree1, BVNode<RSS>* tree2,
00383                            Vec3f R[3], const Vec3f& T,
00384                            Vec3f* vertices1, Vec3f* vertices2,
00385                            Triangle* tri_indices1, Triangle* tri_indices2,
00386                            BVH_CollideResult* res,
00387                            BVHFrontList* front_list)
00388 {
00389   BVHFrontList::iterator front_iter;
00390   BVHFrontList append;
00391   for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter)
00392   {
00393     int b1 = front_iter->left;
00394     int b2 = front_iter->right;
00395     BVNode<RSS>* node1 = tree1 + b1;
00396     BVNode<RSS>* node2 = tree2 + b2;
00397 
00398 
00399     bool l1 = node1->isLeaf();
00400     bool l2 = node2->isLeaf();
00401 
00402     if(l1 & l2)
00403     {
00404       front_iter->valid = false; // the front node is no longer valid, in collideRecurse will add again.
00405       collideRecurse(tree1, tree2, R, T, b1, b2, vertices1, vertices2, tri_indices1, tri_indices2, res, &append);
00406     }
00407     else
00408     {
00409       res->num_bv_tests++;
00410       if(!overlap(R, T, node1->bv, node2->bv))
00411       {
00412         front_iter->valid = false; // the front node is no longer valid
00413 
00414         BVH_REAL sz1 = node1->bv.size();
00415         BVH_REAL sz2 = node2->bv.size();
00416 
00417         if(l2 || (!l1 && (sz1 > sz2)))
00418         {
00419           int c1 = node1->first_child;
00420           int c2 = c1 + 1;
00421 
00422           collideRecurse(tree1, tree2, R, T, c1, b2, vertices1, vertices2, tri_indices1, tri_indices2, res, &append);
00423 
00424           collideRecurse(tree1, tree2, R, T, c2, b2, vertices1, vertices2, tri_indices1, tri_indices2, res, &append);
00425         }
00426         else
00427         {
00428           int c1 = node2->first_child;
00429           int c2 = c1 + 1;
00430 
00431           collideRecurse(tree1, tree2, R, T, b1, c1, vertices1, vertices2, tri_indices1, tri_indices2, res, &append);
00432 
00433           collideRecurse(tree1, tree2, R, T, b1, c2, vertices1, vertices2, tri_indices1, tri_indices2, res, &append);
00434         }
00435       }
00436     }
00437   }
00438 
00439 
00440   // clean the old front list (remove invalid node)
00441   for(front_iter = front_list->begin(); front_iter != front_list->end();)
00442   {
00443     if(!front_iter->valid)
00444       front_iter = front_list->erase(front_iter);
00445     else
00446       ++front_iter;
00447   }
00448 
00449   for(front_iter = append.begin(); front_iter != append.end(); ++front_iter)
00450   {
00451     front_list->push_back(*front_iter);
00452   }
00453 }
00454 
00455 }
collision_primitive.cpp
