swri_geometry_util: test_intersection.cpp Source File

Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #include <gtest/gtest.h>
00031 
00032 #include <swri_geometry_util/intersection.h>
00033 
00034 TEST(IntersectionTests, Intersects)
00035 {
00036   cv::Vec2d c;
00037   ASSERT_TRUE(swri_geometry_util::LineIntersection(
00038       cv::Vec2d(0, 5),
00039       cv::Vec2d(10, 5),
00040       cv::Vec2d(5, 0),
00041       cv::Vec2d(5, 10),
00042       c));
00043 
00044   EXPECT_FLOAT_EQ(5.0, c[0]);
00045   EXPECT_FLOAT_EQ(5.0, c[1]);
00046 
00047   ASSERT_TRUE(swri_geometry_util::LineIntersection(
00048       cv::Vec2d(0, 6),
00049       cv::Vec2d(10, 6),
00050       cv::Vec2d(5, 0),
00051       cv::Vec2d(5, 10),
00052       c));
00053 
00054   EXPECT_FLOAT_EQ(5.0, c[0]);
00055   EXPECT_FLOAT_EQ(6.0, c[1]);
00056 
00057   ASSERT_TRUE(swri_geometry_util::LineIntersection(
00058       cv::Vec2d(10, 6),
00059       cv::Vec2d(0, 6),
00060       cv::Vec2d(5, 10),
00061       cv::Vec2d(5, 0),
00062       c));
00063 
00064   EXPECT_FLOAT_EQ(5.0, c[0]);
00065   EXPECT_FLOAT_EQ(6.0, c[1]);
00066 
00067   ASSERT_TRUE(swri_geometry_util::LineIntersection(
00068       cv::Vec2d(5, 10),
00069       cv::Vec2d(5, 0),
00070       cv::Vec2d(10, 6),
00071       cv::Vec2d(0, 6),
00072       c));
00073 
00074   EXPECT_FLOAT_EQ(5.0, c[0]);
00075   EXPECT_FLOAT_EQ(6.0, c[1]);
00076 
00077   ASSERT_TRUE(swri_geometry_util::LineIntersection(
00078       cv::Vec2d(10, 10),
00079       cv::Vec2d(0, 0),
00080       cv::Vec2d(0, 10),
00081       cv::Vec2d(10, 0),
00082       c));
00083 
00084   EXPECT_FLOAT_EQ(5.0, c[0]);
00085   EXPECT_FLOAT_EQ(5.0, c[1]);
00086 
00087   ASSERT_TRUE(swri_geometry_util::LineIntersection(
00088       cv::Vec2d(-10, -10),
00089       cv::Vec2d(0, 0),
00090       cv::Vec2d(0, -10),
00091       cv::Vec2d(-10, 0),
00092       c));
00093 
00094   EXPECT_FLOAT_EQ(-5.0, c[0]);
00095   EXPECT_FLOAT_EQ(-5.0, c[1]);
00096 
00097   ASSERT_TRUE(swri_geometry_util::LineIntersection(
00098       cv::Vec2d(0, 5),
00099       cv::Vec2d(-10, 5),
00100       cv::Vec2d(5, 0),
00101       cv::Vec2d(5, -10),
00102       c));
00103 
00104   EXPECT_FLOAT_EQ(5.0, c[0]);
00105   EXPECT_FLOAT_EQ(5.0, c[1]);
00106 }
00107 
00108 TEST(IntersectionTests, Parallel)
00109 {
00110   cv::Vec2d c;
00111   EXPECT_FALSE(swri_geometry_util::LineIntersection(
00112       cv::Vec2d(0, 0),
00113       cv::Vec2d(0, 0),
00114       cv::Vec2d(0, 0),
00115       cv::Vec2d(0, 0),
00116       c));
00117 
00118   EXPECT_FALSE(swri_geometry_util::LineIntersection(
00119       cv::Vec2d(0, 0),
00120       cv::Vec2d(10, 0),
00121       cv::Vec2d(0, 0),
00122       cv::Vec2d(10, 0),
00123       c));
00124 
00125   EXPECT_FALSE(swri_geometry_util::LineIntersection(
00126       cv::Vec2d(0, 0),
00127       cv::Vec2d(10, 0),
00128       cv::Vec2d(25, 0),
00129       cv::Vec2d(15, 0),
00130       c));
00131 
00132   EXPECT_FALSE(swri_geometry_util::LineIntersection(
00133       cv::Vec2d(0, 0),
00134       cv::Vec2d(10, 0),
00135       cv::Vec2d(0, -1),
00136       cv::Vec2d(10, -1),
00137       c));
00138 }
00139 
00140 TEST(IntersectionTests, SegmentsIntersect)
00141 {
00142   cv::Vec2d c;
00143   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00144       cv::Vec2d(0, 5),
00145       cv::Vec2d(10, 5),
00146       cv::Vec2d(5, 0),
00147       cv::Vec2d(5, 10),
00148       c));
00149 
00150   EXPECT_FLOAT_EQ(5.0, c[0]);
00151   EXPECT_FLOAT_EQ(5.0, c[1]);
00152 
00153   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00154       cv::Vec2d(0, 6),
00155       cv::Vec2d(10, 6),
00156       cv::Vec2d(5, 0),
00157       cv::Vec2d(5, 10),
00158       c));
00159 
00160   EXPECT_FLOAT_EQ(5.0, c[0]);
00161   EXPECT_FLOAT_EQ(6.0, c[1]);
00162 
00163   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00164       cv::Vec2d(10, 6),
00165       cv::Vec2d(0, 6),
00166       cv::Vec2d(5, 10),
00167       cv::Vec2d(5, 0),
00168       c));
00169 
00170   EXPECT_FLOAT_EQ(5.0, c[0]);
00171   EXPECT_FLOAT_EQ(6.0, c[1]);
00172 
00173   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00174       cv::Vec2d(5, 10),
00175       cv::Vec2d(5, 0),
00176       cv::Vec2d(10, 6),
00177       cv::Vec2d(0, 6),
00178       c));
00179 
00180   EXPECT_FLOAT_EQ(5.0, c[0]);
00181   EXPECT_FLOAT_EQ(6.0, c[1]);
00182 
00183   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00184       cv::Vec2d(10, 10),
00185       cv::Vec2d(0, 0),
00186       cv::Vec2d(0, 10),
00187       cv::Vec2d(10, 0),
00188       c));
00189 
00190   EXPECT_FLOAT_EQ(5.0, c[0]);
00191   EXPECT_FLOAT_EQ(5.0, c[1]);
00192 
00193   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00194       cv::Vec2d(-10, -10),
00195       cv::Vec2d(0, 0),
00196       cv::Vec2d(0, -10),
00197       cv::Vec2d(-10, 0),
00198       c));
00199 
00200   EXPECT_FLOAT_EQ(-5.0, c[0]);
00201   EXPECT_FLOAT_EQ(-5.0, c[1]);
00202 
00203 
00204   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00205       cv::Vec2d(0, 2),
00206       cv::Vec2d(2, 0),
00207       cv::Vec2d(0, 0),
00208       cv::Vec2d(2, 2),
00209       c));
00210 
00211   EXPECT_FLOAT_EQ(1.0, c[0]);
00212   EXPECT_FLOAT_EQ(1.0, c[1]);
00213 
00214   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00215       cv::Vec2d(0, 2),
00216       cv::Vec2d(2, 0),
00217       cv::Vec2d(0, 0),
00218       cv::Vec2d(2, 0),
00219       c));
00220 
00221   EXPECT_FLOAT_EQ(2.0, c[0]);
00222   EXPECT_FLOAT_EQ(0.0, c[1]);
00223 
00224   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00225       cv::Vec2d(3, 0),
00226       cv::Vec2d(0, 0),
00227       cv::Vec2d(0, 2),
00228       cv::Vec2d(2, 0),
00229       c));
00230   EXPECT_FLOAT_EQ(2.0, c[0]);
00231   EXPECT_FLOAT_EQ(0.0, c[1]);
00232 
00233   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00234       cv::Vec2d(0, 2),
00235       cv::Vec2d(2, 0),
00236       cv::Vec2d(0, 0),
00237       cv::Vec2d(3, 0),
00238       c));
00239   EXPECT_FLOAT_EQ(2.0, c[0]);
00240   EXPECT_FLOAT_EQ(0.0, c[1]);
00241 
00242   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00243       cv::Vec2d(0, 2),
00244       cv::Vec2d(2, 0),
00245       cv::Vec2d(3, 0),
00246       cv::Vec2d(0, 0),
00247       c));
00248   EXPECT_FLOAT_EQ(2.0, c[0]);
00249   EXPECT_FLOAT_EQ(0.0, c[1]);
00250 
00251   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00252       cv::Vec2d(0, 2),
00253       cv::Vec2d(2, 0),
00254       cv::Vec2d(2, 0),
00255       cv::Vec2d(0, 0),
00256       c));
00257   EXPECT_FLOAT_EQ(2.0, c[0]);
00258   EXPECT_FLOAT_EQ(0.0, c[1]);
00259 
00260   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00261       cv::Vec2d(0, 2),
00262       cv::Vec2d(2, 0),
00263       cv::Vec2d(0, 2),
00264       cv::Vec2d(0, 0),
00265       c));
00266 
00267   EXPECT_FLOAT_EQ(0.0, c[0]);
00268   EXPECT_FLOAT_EQ(2.0, c[1]);
00269 
00270   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00271       cv::Vec2d(0, 2),
00272       cv::Vec2d(2, 0),
00273       cv::Vec2d(1, 1),
00274       cv::Vec2d(0, 0),
00275       c));
00276 
00277   EXPECT_FLOAT_EQ(1.0, c[0]);
00278   EXPECT_FLOAT_EQ(1.0, c[1]);
00279 
00280   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00281       cv::Vec2d(200, 0),
00282       cv::Vec2d(0, 0),
00283       cv::Vec2d(33.3, 0),
00284       cv::Vec2d(10, 10),
00285       c));
00286   EXPECT_FLOAT_EQ(33.3, c[0]);
00287   EXPECT_FLOAT_EQ(0.0, c[1]);
00288 
00289   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00290       cv::Vec2d(0, 0),
00291       cv::Vec2d(0, 0),
00292       cv::Vec2d(0, 0),
00293       cv::Vec2d(0, 0),
00294       c));
00295   EXPECT_FLOAT_EQ(0.0, c[0]);
00296   EXPECT_FLOAT_EQ(0.0, c[1]);
00297 
00298   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00299       cv::Vec2d(0, 2),
00300       cv::Vec2d(2, 0),
00301       cv::Vec2d(1, 1),
00302       cv::Vec2d(1, 1),
00303       c));
00304   EXPECT_FLOAT_EQ(1.0, c[0]);
00305   EXPECT_FLOAT_EQ(1.0, c[1]);
00306 
00307   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00308       cv::Vec2d(1, 1),
00309       cv::Vec2d(1, 1),
00310       cv::Vec2d(0, 2),
00311       cv::Vec2d(2, 0),
00312       c));
00313   EXPECT_FLOAT_EQ(1.0, c[0]);
00314   EXPECT_FLOAT_EQ(1.0, c[1]);
00315 
00316   ASSERT_TRUE(swri_geometry_util::LineSegmentIntersection(
00317       cv::Vec2d(1065.8687582537791058712173253,-1053.2999883795632740657310933),
00318       cv::Vec2d(1065.5686875431010776082985103,-1051.8000590902413478033849970),
00319       cv::Vec2d(1066.1686875431009866588283330,-1053.0000590902413932781200856),
00320       cv::Vec2d(1065.5686875431010776082985103,-1051.8000590902413478033849970),
00321       c));
00322   EXPECT_FLOAT_EQ(1065.5686875431010776082985103, c[0]);
00323   EXPECT_FLOAT_EQ(-1051.8000590902413478033849970, c[1]);
00324 }
00325 
00326 TEST(IntersectionTests, SegmentsDontIntersect)
00327 {
00328   cv::Vec2d c;
00329   ASSERT_FALSE(swri_geometry_util::LineSegmentIntersection(
00330       cv::Vec2d(0, 5),
00331       cv::Vec2d(-10, 5),
00332       cv::Vec2d(5, 0),
00333       cv::Vec2d(5, -10),
00334       c));
00335 
00336   ASSERT_FALSE(swri_geometry_util::LineSegmentIntersection(
00337       cv::Vec2d(0, 2),
00338       cv::Vec2d(2, 0),
00339       cv::Vec2d(2, 1),
00340       cv::Vec2d(5, 2),
00341       c));
00342 
00343   ASSERT_FALSE(swri_geometry_util::LineSegmentIntersection(
00344       cv::Vec2d(0, 2),
00345       cv::Vec2d(2, 0),
00346       cv::Vec2d(0, 0),
00347       cv::Vec2d(-5, -5),
00348       c));
00349 
00350   ASSERT_FALSE(swri_geometry_util::LineSegmentIntersection(
00351       cv::Vec2d(0, 2),
00352       cv::Vec2d(2, 0),
00353       cv::Vec2d(0.999, 0.999),
00354       cv::Vec2d(0, 0),
00355       c));
00356 
00357   ASSERT_FALSE(swri_geometry_util::LineSegmentIntersection(
00358       cv::Vec2d(200, 0),
00359       cv::Vec2d(0, 0),
00360       cv::Vec2d(33.3, 0.0000001),
00361       cv::Vec2d(10, 10),
00362       c));
00363 
00364   ASSERT_FALSE(swri_geometry_util::LineSegmentIntersection(
00365       cv::Vec2d(1.00001, 1.00001),
00366       cv::Vec2d(1.00001, 1.00001),
00367       cv::Vec2d(0, 2),
00368       cv::Vec2d(2, 0),
00369       c));
00370 
00371   ASSERT_FALSE(swri_geometry_util::LineSegmentIntersection(
00372       cv::Vec2d(0, 2),
00373       cv::Vec2d(2, 0),
00374       cv::Vec2d(1.00001, 1.00001),
00375       cv::Vec2d(1.00001, 1.00001),
00376       c));
00377 }
00378 
00379 TEST(IntersectionTests, ParallelSegments)
00380 {
00381   cv::Vec2d c;
00382   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00383       cv::Vec2d(0, 0),
00384       cv::Vec2d(10, 0),
00385       cv::Vec2d(0, 0),
00386       cv::Vec2d(10, 0),
00387       c));
00388   EXPECT_FLOAT_EQ(0.0, c[0]);
00389   EXPECT_FLOAT_EQ(0.0, c[1]);
00390 
00391   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00392       cv::Vec2d(0, 0),
00393       cv::Vec2d(10, 0),
00394       cv::Vec2d(10, 0),
00395       cv::Vec2d(0, 0),
00396       c));
00397   EXPECT_FLOAT_EQ(0.0, c[0]);
00398   EXPECT_FLOAT_EQ(0.0, c[1]);
00399 
00400   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00401       cv::Vec2d(10, 0),
00402       cv::Vec2d(0, 0),
00403       cv::Vec2d(10, 0),
00404       cv::Vec2d(0, 0),
00405       c));
00406   EXPECT_FLOAT_EQ(10.0, c[0]);
00407   EXPECT_FLOAT_EQ(0.0, c[1]);
00408 
00409   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00410       cv::Vec2d(10, 0),
00411       cv::Vec2d(0, 0),
00412       cv::Vec2d(0, 0),
00413       cv::Vec2d(10, 0),
00414       c));
00415   EXPECT_FLOAT_EQ(10.0, c[0]);
00416   EXPECT_FLOAT_EQ(0.0, c[1]);
00417 
00418   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00419       cv::Vec2d(10, 0),
00420       cv::Vec2d(0, 0),
00421       cv::Vec2d(10, 0),
00422       cv::Vec2d(20, 0),
00423       c));
00424   EXPECT_FLOAT_EQ(10.0, c[0]);
00425   EXPECT_FLOAT_EQ(0.0, c[1]);
00426 
00427   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00428       cv::Vec2d(10, 0),
00429       cv::Vec2d(0, 0),
00430       cv::Vec2d(5, 0),
00431       cv::Vec2d(10, 0),
00432       c));
00433   EXPECT_FLOAT_EQ(10.0, c[0]);
00434   EXPECT_FLOAT_EQ(0.0, c[1]);
00435 
00436   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00437       cv::Vec2d(10, 0),
00438       cv::Vec2d(0, 0),
00439       cv::Vec2d(10, 0),
00440       cv::Vec2d(5, 0),
00441       c));
00442   EXPECT_FLOAT_EQ(10.0, c[0]);
00443   EXPECT_FLOAT_EQ(0.0, c[1]);
00444 
00445   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00446       cv::Vec2d(0, 0),
00447       cv::Vec2d(10, 0),
00448       cv::Vec2d(5, 0),
00449       cv::Vec2d(10, 0),
00450       c));
00451   EXPECT_FLOAT_EQ(5.0, c[0]);
00452   EXPECT_FLOAT_EQ(0.0, c[1]);
00453 
00454   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00455       cv::Vec2d(0, 0),
00456       cv::Vec2d(10, 0),
00457       cv::Vec2d(10, 0),
00458       cv::Vec2d(5, 0),
00459       c));
00460   EXPECT_FLOAT_EQ(5.0, c[0]);
00461   EXPECT_FLOAT_EQ(0.0, c[1]);
00462 
00463   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00464       cv::Vec2d(0, 0),
00465       cv::Vec2d(10, 0),
00466       cv::Vec2d(5, 0),
00467       cv::Vec2d(6, 0),
00468       c));
00469   EXPECT_FLOAT_EQ(5.0, c[0]);
00470   EXPECT_FLOAT_EQ(0.0, c[1]);
00471 
00472   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00473       cv::Vec2d(0, 0),
00474       cv::Vec2d(10, 0),
00475       cv::Vec2d(6, 0),
00476       cv::Vec2d(5, 0),
00477       c));
00478   EXPECT_FLOAT_EQ(5.0, c[0]);
00479   EXPECT_FLOAT_EQ(0.0, c[1]);
00480 
00481   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00482       cv::Vec2d(10, 0),
00483       cv::Vec2d(0, 0),
00484       cv::Vec2d(5, 0),
00485       cv::Vec2d(6, 0),
00486       c));
00487   EXPECT_FLOAT_EQ(6.0, c[0]);
00488   EXPECT_FLOAT_EQ(0.0, c[1]);
00489 
00490   EXPECT_TRUE(swri_geometry_util::LineSegmentIntersection(
00491       cv::Vec2d(10, 0),
00492       cv::Vec2d(0, 0),
00493       cv::Vec2d(6, 0),
00494       cv::Vec2d(5, 0),
00495       c));
00496   EXPECT_FLOAT_EQ(6.0, c[0]);
00497   EXPECT_FLOAT_EQ(0.0, c[1]);
00498 
00499   EXPECT_FALSE(swri_geometry_util::LineSegmentIntersection(
00500       cv::Vec2d(0, 0),
00501       cv::Vec2d(10, 0),
00502       cv::Vec2d(0, 1),
00503       cv::Vec2d(10, 1),
00504       c));
00505 
00506   EXPECT_FALSE(swri_geometry_util::LineSegmentIntersection(
00507       cv::Vec2d(0, 0),
00508       cv::Vec2d(10, 0),
00509       cv::Vec2d(0, .00001),
00510       cv::Vec2d(10, .00001),
00511       c));
00512 
00513   EXPECT_FALSE(swri_geometry_util::LineSegmentIntersection(
00514       cv::Vec2d(0, 0),
00515       cv::Vec2d(10, 0),
00516       cv::Vec2d(11, 0),
00517       cv::Vec2d(20, 0),
00518       c));
00519 
00520   EXPECT_FALSE(swri_geometry_util::LineSegmentIntersection(
00521       cv::Vec2d(0, 0),
00522       cv::Vec2d(10, 0),
00523       cv::Vec2d(10.00001, 0),
00524       cv::Vec2d(20, 0),
00525       c));
00526 }
00527 
00528 // Run all the tests that were declared with TEST()
00529 int main(int argc, char **argv)
00530 {
00531   testing::InitGoogleTest(&argc, argv);
00532 
00533   return RUN_ALL_TESTS();
00534 }