distances.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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 the copyright holder(s) 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  * $Id$
00035  *
00036  */
00037 #include <pcl/common/distances.h>
00038 
00039 void
00040 pcl::lineToLineSegment (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, 
00041                         Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg)
00042 {
00043   // point + direction = 2nd point
00044   Eigen::Vector4f p1 = Eigen::Vector4f::Zero ();
00045   Eigen::Vector4f p2 = Eigen::Vector4f::Zero ();
00046   Eigen::Vector4f dir1 = Eigen::Vector4f::Zero ();
00047   p1.head<3> () = line_a.head<3> ();
00048   dir1.head<3> () = line_a.segment<3> (3);
00049   p2 = p1 + dir1;
00050 
00051   // point + direction = 2nd point
00052   Eigen::Vector4f q1 = Eigen::Vector4f::Zero ();
00053   Eigen::Vector4f q2 = Eigen::Vector4f::Zero ();
00054   Eigen::Vector4f dir2 = Eigen::Vector4f::Zero ();
00055   q1.head<3> () = line_b.head<3> ();
00056   dir2.head<3> () = line_b.segment<3> (3);
00057   q2 = q1 + dir2;
00058 
00059   // a = x2 - x1 = line_a[1] - line_a[0]
00060   Eigen::Vector4f u = dir1;
00061   // b = x4 - x3 = line_b[1] - line_b[0]
00062   Eigen::Vector4f v = dir2;
00063   // c = x2 - x3 = line_a[1] - line_b[0]
00064   Eigen::Vector4f w = p2 - q1;
00065 
00066   float a = u.dot (u);
00067   float b = u.dot (v);
00068   float c = v.dot (v);
00069   float d = u.dot (w);
00070   float e = v.dot (w);
00071   float denominator = a*c - b*b;
00072   float sc, tc;
00073   // Compute the line parameters of the two closest points
00074   if (denominator < 1e-5)          // The lines are almost parallel
00075   {
00076     sc = 0.0;
00077     tc = (b > c ? d / b : e / c);  // Use the largest denominator
00078   }
00079   else
00080   {
00081     sc = (b*e - c*d) / denominator;
00082     tc = (a*e - b*d) / denominator;
00083   }
00084   // Get the closest points
00085   pt1_seg = Eigen::Vector4f::Zero ();
00086   pt1_seg = p2 + sc * u;
00087 
00088   pt2_seg = Eigen::Vector4f::Zero ();
00089   pt2_seg = q1 + tc * v;
00090 }
00091 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:22