00001 #include "VclipLinkPair.h" 00002 00003 VclipLinkPair::VclipLinkPair(hrp::Link* link0, Vclip::Polyhedron* vclip_model0, hrp::Link* link1, Vclip::Polyhedron* vclip_model1, double tolerance) 00004 { 00005 links_[0] = link0; 00006 links_[1] = link1; 00007 Vclip_Model1 = vclip_model0; 00008 Vclip_Model2 = vclip_model1; 00009 tolerance_ = tolerance; 00010 Feature_Pair.first = (const Vclip::Feature *)new Vclip::Vertex(Vclip_Model1->verts().front()); 00011 Feature_Pair.second = (const Vclip::Feature *)new Vclip::Vertex(Vclip_Model2->verts().front()); 00012 } 00013 VclipLinkPair::~VclipLinkPair() 00014 { 00015 } 00016 00017 bool VclipLinkPair::checkCollision() 00018 { 00019 double p1[3], p2[3]; 00020 double len = computeDistance(p1,p2); 00021 if ( len < tolerance_ ) { 00022 return true; 00023 } 00024 return false; 00025 } 00026 00027 double VclipLinkPair::computeDistance(double *q1, double *q2) 00028 { 00029 Vclip::Mat3 R1, R2; 00030 Vclip::Vect3 T1, T2; 00031 Vclip::VclipPose P1, P2; 00032 const hrp::Vector3& p1 = links_[0]->p; 00033 hrp::Matrix33 r1 = links_[0]->attitude(); 00034 const hrp::Vector3& p2 = links_[1]->p; 00035 hrp::Matrix33 r2 = links_[1]->attitude(); 00036 R1.xrow().set(r1(0,0), r1(0,1), r1(0,2)); 00037 R1.yrow().set(r1(1,0), r1(1,1), r1(1,2)); 00038 R1.zrow().set(r1(2,0), r1(2,1), r1(2,2)); 00039 R2.xrow().set(r2(0,0), r2(0,1), r2(0,2)); 00040 R2.yrow().set(r2(1,0), r2(1,1), r2(1,2)); 00041 R2.zrow().set(r2(2,0), r2(2,1), r2(2,2)); 00042 T1.set(p1(0), p1(1), p1(2)); 00043 T2.set(p2(0), p2(1), p2(2)); 00044 P1.set(R1, T1); 00045 P2.set(R2, T2); 00046 Vclip::VclipPose X12, X21; 00047 X12.invert(P2); 00048 X12.postmult(P1); 00049 X21.invert(X12); 00050 Vclip::Vect3 cp1, cp2; 00051 double len = Vclip::Polyhedron::vclip(Vclip_Model1, Vclip_Model2, X12, X21, Feature_Pair.first, Feature_Pair.second, cp1, cp2, 0); 00052 Vclip::Vect3 cp1g, cp2g; 00053 P1.xformPoint(cp1, cp1g); 00054 P2.xformPoint(cp2, cp2g); 00055 q1[0] = cp1g.x; q1[1] = cp1g.y; q1[2] = cp1g.z; 00056 q2[0] = cp2g.x; q2[1] = cp2g.y; q2[2] = cp2g.z; 00057 00058 return len; 00059 } 00060 00061