CollisionPairInserter.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  * General Robotix Inc. 
00009  */
00010 
00011 #include "CollisionPairInserter.h"
00012 #include "ColdetModelSharedDataSet.h"
00013 #include "Opcode/Opcode.h"
00014 #include <cstdio>
00015 #include <iostream>
00016 #include <vector>
00017 
00018 using namespace std;
00019 using namespace Opcode;
00020 using namespace hrp;
00021 
00022 HRP_COLLISION_EXPORT int tri_tri_overlap(
00023     const Vector3& P1,
00024     const Vector3& P2,
00025     const Vector3& P3,
00026     const Vector3& Q1,
00027     const Vector3& Q2,
00028     const Vector3& Q3,
00029     collision_data* col_p,
00030     CollisionPairInserterBase* collisionPairInserter);
00031 
00032 namespace {
00033     const bool COLLIDE_DEBUG = false;
00034     // �R���p�C�����Ɂ@-DDEPTH_CHECK�Ƃ���΁Adepth�l�ɂ��ڐG�_�I�����L��ɂȂ�܂��B�@�@//
00035 #ifdef  DEPTH_CHECK
00036     const double MAX_DEPTH = 0.1;
00037 #endif
00038     const int CD_OK = 0;
00039     const int CD_ALL_CONTACTS = 1;
00040     const int CD_FIRST_CONTACT = 2;
00041     const int CD_ERR_COLLIDE_OUT_OF_MEMORY = 2;
00042     
00043     enum {
00044         FV = 1,
00045         VF,
00046         EE
00047     };
00048 }
00049 
00050 
00051 CollisionPairInserter::CollisionPairInserter()
00052 {
00053 
00054 }
00055 
00056 
00057 CollisionPairInserter::~CollisionPairInserter()
00058 {
00059 
00060 }
00061 
00062 
00063 void CollisionPairInserter::copy_tri(col_tri* t1, tri* t2)
00064 {
00065     t1->p1 = t2->p1;
00066     t1->p2 = t2->p2;
00067     t1->p3 = t2->p3;
00068 }
00069 
00070 
00071 void CollisionPairInserter::copy_tri(col_tri* t1, col_tri* t2)
00072 {
00073     t1->p1 = t2->p1;
00074     t1->p2 = t2->p2;
00075     t1->p3 = t2->p3;
00076 
00077     if(t2->n[0] && t2->n[1] && t2->n[2]){
00078         t1->n = t2->n;
00079     }
00080 }
00081 
00082 
00083 void CollisionPairInserter::calc_normal_vector(col_tri* t)
00084 {
00085     if(t->status == 0){
00086         const Vector3 e1(t->p2 - t->p1);
00087         const Vector3 e2(t->p3 - t->p2);
00088         t->n = e1.cross(e2).normalized();
00089         t->status = 1;
00090     }
00091 }
00092 
00093 
00094 int CollisionPairInserter::is_convex_neighbor(col_tri* t1, col_tri* t2)
00095 {
00096     const double EPS = 1.0e-12; // a small number
00097         
00098     calc_normal_vector(t2);
00099         
00100     // printf("normal vector1 = %f %f %f\n", t1->n[0], t1->n[1], t1->n[2]);
00101     // printf("normal vector2 = %f %f %f\n", t2->n[0], t2->n[1], t2->n[2]);
00102         
00103     const Vector3 vec1(t1->p1 - t2->p1);
00104     const Vector3 vec2(t1->p2 - t2->p2);
00105     const Vector3 vec3(t1->p3 - t2->p3);
00106         
00107     // printf("is_convex_neighbor = %f %f %f\n",innerProd(t1->n,vec1),innerProd(t1->n,vec2),innerProd(t1->n,vec3));
00108         
00109     if(t2->n.dot(vec1) < EPS && t2->n.dot(vec2) < EPS && t2->n.dot(vec3) < EPS){
00110         return 1;
00111     } else {
00112         return 0;
00113     }
00114 }
00115 
00116 void CollisionPairInserter::triangleIndexToPoint(ColdetModelSharedDataSet* model, int id, col_tri& tri){
00117     IceMaths::IndexedTriangle indextriangle = model->triangles[id];
00118     IceMaths::Point point0 = model->vertices[indextriangle.mVRef[0]];
00119     IceMaths::Point point1 = model->vertices[indextriangle.mVRef[1]];
00120     IceMaths::Point point2 = model->vertices[indextriangle.mVRef[2]];
00121     tri.p1[0] = point0.x; tri.p1[1] = point0.y; tri.p1[2] = point0.z; 
00122     tri.p2[0] = point1.x; tri.p2[1] = point1.y; tri.p2[2] = point1.z; 
00123     tri.p3[0] = point2.x; tri.p3[1] = point2.y; tri.p3[2] = point2.z; 
00124 }
00125 
00126 void CollisionPairInserter::get_triangles_in_convex_neighbor(ColdetModelSharedDataSet* model, int id, col_tri* tri_convex_neighbor, std::vector<int>& foundTriangles, int& count){
00127     int k;
00128     for(unsigned int i=0; i<foundTriangles.size(); i++)
00129         if(foundTriangles[i] == id){
00130             k = i;
00131             break;
00132         }
00133 
00134     for(int i=0; i<3; i++){
00135         int nei = model->neighbor[id].triangles[i];
00136         if(nei < 0)
00137             continue;
00138         unsigned int j=0;
00139         for(; j<foundTriangles.size(); j++)
00140             if(foundTriangles[j] == nei)
00141                 break;
00142         if(j<foundTriangles.size())
00143             continue;
00144 
00145         col_tri tri_nei;
00146         triangleIndexToPoint(model, nei, tri_nei); 
00147 
00148         if(is_convex_neighbor( &tri_nei, &tri_convex_neighbor[k])){
00149             if(k!=0){
00150                 Vector3 p1 = tri_nei.p1 - tri_convex_neighbor[0].p1;
00151                 if(p1.dot(tri_convex_neighbor[0].n) > 0)
00152                     continue;
00153                 Vector3 p2 = tri_nei.p2 - tri_convex_neighbor[0].p1;
00154                 if(p2.dot(tri_convex_neighbor[0].n) > 0)
00155                     continue;
00156                 Vector3 p3 = tri_nei.p3 - tri_convex_neighbor[0].p1;
00157                 if(p3.dot(tri_convex_neighbor[0].n) > 0)
00158                     continue;
00159             }
00160             foundTriangles.push_back(nei);
00161             tri_convex_neighbor[count].status = 0;
00162             copy_tri(&tri_convex_neighbor[count++], &tri_nei);
00163         }
00164     }
00165 }
00166 
00167 int CollisionPairInserter::get_triangles_in_convex_neighbor(ColdetModelSharedDataSet* model, int id, col_tri* tri_convex_neighbor, int min_num){
00168 
00169     std::vector<int> foundTriangles;
00170     int count=0;
00171     triangleIndexToPoint(model, id, tri_convex_neighbor[count++]);
00172     tri_convex_neighbor[0].status = 0;
00173     foundTriangles.push_back(id);
00174     
00175     int start = 0;
00176     int end = 1;
00177 
00178     int j=0;
00179     while(count < min_num && j<2){
00180         for(int i=start; i< end; i++)
00181             get_triangles_in_convex_neighbor(model, foundTriangles[i], tri_convex_neighbor, foundTriangles, count);
00182         start = end;
00183         end  = count;
00184         j++;
00185     }
00186 
00187     return count;
00188 }
00189 
00190 
00191 void CollisionPairInserter::examine_normal_vector(int id1, int id2, int ctype)
00192 {
00193     check_separability(id1, id2, ctype);
00194 }
00195 
00196 void CollisionPairInserter::check_separability(int id1, int id2, int ctype){
00197     int contactIndex = cdContact.size() - 1;
00198     Vector3 signed_distance;
00199     Vector3 signed_distance1(99999999.0,99999999.0,99999999.0);
00200     Vector3 signed_distance2(-99999999.0,-99999999.0,-99999999.0);
00201 
00202     ColdetModelSharedDataSet* model0 = ((CollisionPairInserterBase*)this)->models[0];
00203     ColdetModelSharedDataSet* model1 = ((CollisionPairInserterBase*)this)->models[1];
00204     find_signed_distance(signed_distance1, model0, id1, contactIndex, ctype, 1);
00205     find_signed_distance(signed_distance2, model1, id2, contactIndex, ctype, 2);
00206 
00207     int max = (2 < ctype) ? ctype : 2;
00208     
00209     for(int i=0; i < max; ++i){
00210         signed_distance[i] = signed_distance1[i] - signed_distance2[i];
00211         if(COLLIDE_DEBUG) printf("signed distance %d = %f\n", i, signed_distance[i]);
00212     }
00213 
00214     switch(ctype){
00215 
00216     case FV:
00217         if(signed_distance[0] < signed_distance[1]){
00218             cdContact[contactIndex].n_vector = cdContact[contactIndex].m;
00219             cdContact[contactIndex].depth = fabs(signed_distance[1]);
00220             if(COLLIDE_DEBUG) printf("normal replaced\n");
00221         } else {
00222             cdContact[contactIndex].depth = fabs(signed_distance[0]);
00223         }
00224         break;
00225         
00226     case VF:
00227         if(signed_distance[0] < signed_distance[1]){
00228             cdContact[contactIndex].n_vector = - cdContact[contactIndex].n;
00229             cdContact[contactIndex].depth = fabs(signed_distance[1]);
00230             if(COLLIDE_DEBUG) printf("normal replaced\n");
00231         } else{
00232             cdContact[contactIndex].depth = fabs(signed_distance[0]);
00233         }
00234         break;
00235         
00236     case EE:
00237         cdContact[contactIndex].num_of_i_points = 1;
00238         if(signed_distance[0] < signed_distance[1] && signed_distance[2] <= signed_distance[1]){
00239             cdContact[contactIndex].n_vector = cdContact[contactIndex].m;
00240             cdContact[contactIndex].depth = fabs(signed_distance[1]);
00241             if(COLLIDE_DEBUG) printf("normal replaced\n");
00242         } else if(signed_distance[0] < signed_distance[2] && signed_distance[1] < signed_distance[2]){
00243             cdContact[contactIndex].n_vector = - cdContact[contactIndex].n;
00244             cdContact[contactIndex].depth = fabs(signed_distance[2]);
00245             if(COLLIDE_DEBUG) printf("normal replaced\n");
00246         } else {
00247             cdContact[contactIndex].depth = fabs(signed_distance[0]);
00248             // cout << "depth in InsertCollisionPair.cpp = " << signed_distance[0] << endl;
00249         }
00250         cdContact[contactIndex].i_points[0] += cdContact[contactIndex].i_points[1];
00251         cdContact[contactIndex].i_points[0] *= 0.5;
00252         break;
00253     }
00254     
00255     if(COLLIDE_DEBUG){
00256         printf("final normal = %f %f %f\n", cdContact[contactIndex].n_vector[0],
00257                cdContact[contactIndex].n_vector[1], cdContact[contactIndex].n_vector[2]);
00258     }
00259     if(COLLIDE_DEBUG){
00260         for(int i=0; i < cdContact[contactIndex].num_of_i_points; ++i){
00261             cout << "final depth = " << cdContact[contactIndex].depth << endl;
00262             cout << "final i_point = " << cdContact[contactIndex].i_points[i][0] << " "
00263                  << cdContact[contactIndex].i_points[i][1] << " " << cdContact[contactIndex].i_points[i][2]
00264                  << endl;
00265         }
00266     }
00267     
00268     if(COLLIDE_DEBUG) cout << endl;
00269 }
00270 
00271 void CollisionPairInserter::find_signed_distance(
00272     Vector3& signed_distance, col_tri* trp, int nth, int ctype, int obj)
00273 {
00274     find_signed_distance(signed_distance, trp->p1, nth, ctype, obj);
00275     find_signed_distance(signed_distance, trp->p2, nth, ctype, obj);
00276     find_signed_distance(signed_distance, trp->p3, nth, ctype, obj);
00277 }
00278 
00279 void CollisionPairInserter::find_signed_distance(
00280     Vector3& signed_distance, const Vector3& vert, int nth, int ctype, int obj)
00281 {
00282     Vector3 vert_w;
00283     if(obj==1){
00284         vert_w = CD_s1 * (CD_Rot1 * vert + CD_Trans1);
00285     } else {
00286         vert_w = CD_s2 * (CD_Rot2 * vert + CD_Trans2);
00287     }
00288         
00289     if(COLLIDE_DEBUG) printf("vertex = %f %f %f\n", vert_w[0], vert_w[1], vert_w[2]);
00290         
00291     // use the first intersecting point to find the distance
00292     const Vector3 vec(vert_w - cdContact[nth].i_points[0]);
00293     //vecNormalize(cdContact[nth].n_vector);
00294     cdContact[nth].n_vector.normalize();
00295         
00296     double dis0 = cdContact[nth].n_vector.dot(vec);
00297         
00298 #if 0
00299     switch(ctype){
00300     case FV:
00301         if(dot(cdContact[nth].n_vector, cdContact[nth].n) > 0.0) dis0 = - dis0;
00302         break;
00303     case VF:
00304         if(dot(cdContact[nth].n_vector, cdContact[nth].m) < 0.0) dis0 = - dis0;
00305         break;
00306     case EE:
00307         if(dot(cdContact[nth].n_vector, cdContact[nth].n) > 0.0 ||
00308            dot(cdContact[nth].n_vector, cdContact[nth].m) < 0.0)
00309             dis0 = - dis0;
00310     }
00311 #endif
00312         
00313     if(COLLIDE_DEBUG) printf("dis0 = %f\n", dis0);
00314         
00315     double dis1 = dis0;
00316     double dis2 = dis0;
00317         
00318     switch(ctype){
00319     case FV:
00320         dis1 =   cdContact[nth].m.dot(vec);
00321         if(COLLIDE_DEBUG) printf("dis1 = %f\n", dis1);
00322         break;
00323     case VF:
00324         dis1 = - cdContact[nth].n.dot(vec);
00325         if(COLLIDE_DEBUG) printf("dis1 = %f\n", dis1);
00326         break;
00327     case EE:
00328         dis1 =   cdContact[nth].m.dot(vec);
00329         dis2 = - cdContact[nth].n.dot(vec);
00330         if(COLLIDE_DEBUG){
00331             printf("dis1 = %f\n", dis1);
00332             printf("dis2 = %f\n", dis2);
00333         }
00334     }
00335 
00336     if(COLLIDE_DEBUG) printf("obj = %d\n", obj);
00337     if(obj == 1){
00338         if(dis0 < signed_distance[0]) signed_distance[0] = dis0;
00339         if(dis1 < signed_distance[1]) signed_distance[1] = dis1;
00340         if(ctype==EE)
00341             if(dis2 < signed_distance[2]) signed_distance[2] = dis2;
00342     }
00343     else{
00344         if(signed_distance[0] < dis0) signed_distance[0] = dis0;
00345         if(signed_distance[1] < dis1) signed_distance[1] = dis1;
00346         if(ctype==EE)
00347             if(signed_distance[2] < dis2) signed_distance[2] = dis2;
00348     }
00349 }
00350 
00351 void CollisionPairInserter::find_signed_distance(
00352     Vector3& signed_distance,
00353     ColdetModelSharedDataSet* model,
00354     int id, 
00355     int contactIndex, 
00356     int ctype, 
00357     int obj)
00358 {
00359     const int MIN_NUM_NEIGHBOR = 10;
00360     col_tri* tri_convex_neighbor = new col_tri[22];
00361     int num = get_triangles_in_convex_neighbor(model, id, tri_convex_neighbor, MIN_NUM_NEIGHBOR);
00362 
00363     for(int i=0; i<num; ++i){
00364         find_signed_distance(signed_distance, &tri_convex_neighbor[i], contactIndex, ctype, obj);
00365     }
00366 
00367     delete [] tri_convex_neighbor;
00368 }
00369 
00370 int CollisionPairInserter::new_point_test(int k)
00371 {
00372     const double eps = 1.0e-12; // 1 micro meter to judge two contact points are identical
00373 
00374     int last = cdContact.size()-1;
00375     
00376     for(int i=0; i < last; ++i){
00377         for(int j=0; j < cdContact[i].num_of_i_points; ++j){
00378             Vector3 dv(cdContact[i].i_points[j] - cdContact[last].i_points[k]);
00379             double d = cdContact[i].depth - cdContact[last].depth;
00380             if(dv.dot(dv) < eps && d*d < eps) return 0;
00381         }
00382     }
00383     return 1;
00384 }
00385 
00386 
00387 //
00388 // obsolute signatures
00389 //
00390 int CollisionPairInserter::apply(
00391     const Opcode::AABBCollisionNode* b1,
00392     const Opcode::AABBCollisionNode* b2,
00393     int id1, int id2,
00394     int num_of_i_points,
00395     Vector3 i_points[4],
00396     Vector3& n_vector,
00397     double depth,
00398     Vector3& n1,
00399     Vector3& m1,
00400     int ctype,
00401     Opcode::MeshInterface* mesh1,
00402     Opcode::MeshInterface* mesh2)
00403 {
00404     cdContact.push_back(collision_data());
00405     collision_data& contact = cdContact.back();
00406     contact.id1 = id1;
00407     contact.id2 = id2;
00408     contact.depth = depth;
00409     contact.num_of_i_points = num_of_i_points;
00410 
00411     if(COLLIDE_DEBUG) printf("num_of_i_points = %d\n", num_of_i_points);
00412 
00413     for(int i=0; i < num_of_i_points; ++i){
00414         contact.i_points[i].noalias() = CD_s2 * ((CD_Rot2 * i_points[i]) + CD_Trans2);
00415     }
00416 
00417     contact.n_vector.noalias() = CD_Rot2 * n_vector;
00418     contact.n.noalias() = CD_Rot2 * n1;
00419     contact.m.noalias() = CD_Rot2 * m1;
00420         examine_normal_vector(id1, id2, ctype);
00421 
00422 #ifdef DEPTH_CHECK
00423     // analyze_neighborhood_of_i_point(b1, b2, cdContactsCount, ctype);
00424     // remove the intersecting point if depth is deeper than MAX_DEPTH meter
00425     if(fabs(contact.depth) < MAX_DEPTH){
00426         for(int i=0; i < num_of_i_points; ++i){
00427             contact.i_point_new[i] = new_point_test(i);
00428         }
00429     } else {
00430         for(int i=0; i < num_of_i_points; ++i){
00431             contact.i_point_new[i] = 0;
00432         }
00433     }
00434 #else
00435     for(int i=0; i < num_of_i_points; ++i){
00436         contact.i_point_new[i] = 1;
00437     }
00438 #endif
00439 
00440     return CD_OK;
00441 }
00442 
00443 
00444 int CollisionPairInserter::detectTriTriOverlap(
00445     const Vector3& P1,
00446     const Vector3& P2,
00447     const Vector3& P3,
00448     const Vector3& Q1,
00449     const Vector3& Q2,
00450     const Vector3& Q3,
00451     collision_data* col_p)
00452 {
00453     return tri_tri_overlap(P1, P2, P3, Q1, Q2, Q3, col_p, this);
00454 }


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:53