1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the copyright holder(s) nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $Id$
35 *
36 */
37 #include <pcl/common/distances.h>
38
39 void
lineToLineSegment(const Eigen::VectorXf & line_a,const Eigen::VectorXf & line_b,Eigen::Vector4f & pt1_seg,Eigen::Vector4f & pt2_seg)40 pcl::lineToLineSegment (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b,
41 Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg)
42 {
43 // point + direction = 2nd point
44 Eigen::Vector4f p1 = Eigen::Vector4f::Zero ();
45 Eigen::Vector4f p2 = Eigen::Vector4f::Zero ();
46 Eigen::Vector4f dir1 = Eigen::Vector4f::Zero ();
47 p1.head<3> () = line_a.head<3> ();
48 dir1.head<3> () = line_a.segment<3> (3);
49 p2 = p1 + dir1;
50
51 // point + direction = 2nd point
52 Eigen::Vector4f q1 = Eigen::Vector4f::Zero ();
53 Eigen::Vector4f q2 = Eigen::Vector4f::Zero ();
54 Eigen::Vector4f dir2 = Eigen::Vector4f::Zero ();
55 q1.head<3> () = line_b.head<3> ();
56 dir2.head<3> () = line_b.segment<3> (3);
57 q2 = q1 + dir2;
58
59 // a = x2 - x1 = line_a[1] - line_a[0]
60 Eigen::Vector4f u = dir1;
61 // b = x4 - x3 = line_b[1] - line_b[0]
62 Eigen::Vector4f v = dir2;
63 // c = x2 - x3 = line_a[1] - line_b[0]
64 Eigen::Vector4f w = p2 - q1;
65
66 float a = u.dot (u);
67 float b = u.dot (v);
68 float c = v.dot (v);
69 float d = u.dot (w);
70 float e = v.dot (w);
71 float denominator = a*c - b*b;
72 float sc, tc;
73 // Compute the line parameters of the two closest points
74 if (denominator < 1e-5) // The lines are almost parallel
75 {
76 sc = 0.0;
77 tc = (b > c ? d / b : e / c); // Use the largest denominator
78 }
79 else
80 {
81 sc = (b*e - c*d) / denominator;
82 tc = (a*e - b*d) / denominator;
83 }
84 // Get the closest points
85 pt1_seg = Eigen::Vector4f::Zero ();
86 pt1_seg = p2 + sc * u;
87
88 pt2_seg = Eigen::Vector4f::Zero ();
89 pt2_seg = q1 + tc * v;
90 }
91
92