VclipLinkPair.cpp
Go to the documentation of this file.
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 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:19