swri_geometry_util: test_geometry

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/geometry_util.h>
00033 
00034 TEST(GeometryUtilTests, ClosestPointToLinesInvalid)
00035 {
00036   tf::Vector3 point;
00037   ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
00038     tf::Vector3(1, 0, 0),
00039     tf::Vector3(1, 0, 0),
00040     tf::Vector3(0, 1, 0),
00041     tf::Vector3(1, 0, 1),
00042     point));
00043  
00044   ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
00045     tf::Vector3(1, 0, 1),
00046     tf::Vector3(0, 1, 0),
00047     tf::Vector3(0, 0, 1),
00048     tf::Vector3(0, 0, 1),
00049     point));
00050 
00051   ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
00052     tf::Vector3(0, 0, 0),
00053     tf::Vector3(10, 0, 0),
00054     tf::Vector3(20, 0, 0),
00055     tf::Vector3(30, 0, 0),
00056     point));
00057 
00058   ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
00059     tf::Vector3(0, 0, 0),
00060     tf::Vector3(10, 0, 0),
00061     tf::Vector3(30, 0, 0),
00062     tf::Vector3(10, 0, 0),
00063     point));
00064 
00065   ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
00066     tf::Vector3(0, 0, 0),
00067     tf::Vector3(10, 0, 0),
00068     tf::Vector3(20, 10, 10),
00069     tf::Vector3(30, 10, 10),
00070     point));
00071 
00072 
00073   ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
00074     tf::Vector3(0, 0, 0),
00075     tf::Vector3(10, 0, 0),
00076     tf::Vector3(30, 10, 10),
00077     tf::Vector3(10, 10, 10),
00078     point));
00079 }
00080 
00081 TEST(GeometryUtilTests, ClosestPointToLines)
00082 {
00083   tf::Vector3 point;
00084   ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
00085     tf::Vector3(0, 0, 0),
00086     tf::Vector3(10, 0, 0),
00087     tf::Vector3(0, 0, 0),
00088     tf::Vector3(0, 10, 0),
00089     point));
00090   EXPECT_FLOAT_EQ(point.x(), 0);
00091   EXPECT_FLOAT_EQ(point.y(), 0);
00092   EXPECT_FLOAT_EQ(point.z(), 0);
00093 
00094   ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
00095     tf::Vector3(0, 0, 0),
00096     tf::Vector3(10, 0, 0),
00097     tf::Vector3(0, 5, 0),
00098     tf::Vector3(0, 10, 0),
00099     point));
00100   EXPECT_FLOAT_EQ(point.x(), 0);
00101   EXPECT_FLOAT_EQ(point.y(), 0);
00102   EXPECT_FLOAT_EQ(point.z(), 0);
00103 
00104   ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
00105     tf::Vector3(5, 0, 0),
00106     tf::Vector3(10, 0, 0),
00107     tf::Vector3(0, 5, 0),
00108     tf::Vector3(0, 10, 0),
00109     point));
00110   EXPECT_FLOAT_EQ(point.x(), 0);
00111   EXPECT_FLOAT_EQ(point.y(), 0);
00112   EXPECT_FLOAT_EQ(point.z(), 0);
00113 
00114   ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
00115     tf::Vector3(0, 0, 0),
00116     tf::Vector3(10, 0, 0),
00117     tf::Vector3(0, -5, 0),
00118     tf::Vector3(0, 10, 0),
00119     point));
00120   EXPECT_FLOAT_EQ(point.x(), 0);
00121   EXPECT_FLOAT_EQ(point.y(), 0);
00122   EXPECT_FLOAT_EQ(point.z(), 0);
00123 
00124   ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
00125     tf::Vector3(0, 0, 0),
00126     tf::Vector3(10, 0, 0),
00127     tf::Vector3(0, 0, 20),
00128     tf::Vector3(0, 10, 20),
00129     point));
00130   EXPECT_FLOAT_EQ(point.x(), 0);
00131   EXPECT_FLOAT_EQ(point.y(), 0);
00132   EXPECT_FLOAT_EQ(point.z(), 10);
00133 
00134   ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
00135     tf::Vector3(0, 0, 0),
00136     tf::Vector3(10, 0, 0),
00137     tf::Vector3(0, 10, 20),
00138     tf::Vector3(0, 0, 20),
00139     point));
00140   EXPECT_FLOAT_EQ(point.x(), 0);
00141   EXPECT_FLOAT_EQ(point.y(), 0);
00142   EXPECT_FLOAT_EQ(point.z(), 10);
00143 }
00144 
00145 TEST(GeometryUtilTests, ProjectPointToLineSegment3D)
00146 {
00147   tf::Vector3 p1(0, 0, 0);
00148   tf::Vector3 p2(10, 0, 0);
00149   tf::Vector3 p3(2, 0, 0);
00150   tf::Vector3 p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00151   EXPECT_FLOAT_EQ(p3.x(), p4.x());
00152   EXPECT_FLOAT_EQ(p3.y(), p4.y());
00153   EXPECT_FLOAT_EQ(p3.z(), p4.z());
00154 
00155   p4 = swri_geometry_util::ProjectToLineSegment(p2, p1, p3);
00156   EXPECT_FLOAT_EQ(p3.x(), p4.x());
00157   EXPECT_FLOAT_EQ(p3.y(), p4.y());
00158   EXPECT_FLOAT_EQ(p3.z(), p4.z());
00159 
00160   p3 = tf::Vector3(0, 0, 0);
00161   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00162   EXPECT_FLOAT_EQ(p3.x(), p4.x());
00163   EXPECT_FLOAT_EQ(p3.y(), p4.y());
00164   EXPECT_FLOAT_EQ(p3.z(), p4.z());
00165 
00166   p3 = tf::Vector3(0, 1, 0);
00167   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00168   EXPECT_FLOAT_EQ(0, p4.x());
00169   EXPECT_FLOAT_EQ(0, p4.y());
00170   EXPECT_FLOAT_EQ(0, p4.z());
00171 
00172   p3 = tf::Vector3(5, -1, 0);
00173   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00174   EXPECT_FLOAT_EQ(5, p4.x());
00175   EXPECT_FLOAT_EQ(0, p4.y());
00176   EXPECT_FLOAT_EQ(0, p4.z());
00177 
00178   p3 = tf::Vector3(5, -1, 10);
00179   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00180   EXPECT_FLOAT_EQ(5, p4.x());
00181   EXPECT_FLOAT_EQ(0, p4.y());
00182   EXPECT_FLOAT_EQ(0, p4.z());
00183 
00184   p3 = tf::Vector3(-5, -1, 10);
00185   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00186   EXPECT_FLOAT_EQ(0, p4.x());
00187   EXPECT_FLOAT_EQ(0, p4.y());
00188   EXPECT_FLOAT_EQ(0, p4.z());
00189 
00190   p3 = tf::Vector3(15, -1, 10);
00191   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00192   EXPECT_FLOAT_EQ(10, p4.x());
00193   EXPECT_FLOAT_EQ(0, p4.y());
00194   EXPECT_FLOAT_EQ(0, p4.z());
00195 
00196   p1 = tf::Vector3(0, 0, 0);
00197   p2 = tf::Vector3(10, 10, 10);
00198   p3 = tf::Vector3(1, 1, 1);
00199   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00200   EXPECT_FLOAT_EQ(1, p4.x());
00201   EXPECT_FLOAT_EQ(1, p4.y());
00202   EXPECT_FLOAT_EQ(1, p4.z());
00203 
00204   p2 = tf::Vector3(10, 10, 0);
00205   p3 = tf::Vector3(0, 10, 0);
00206   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00207   EXPECT_FLOAT_EQ(5, p4.x());
00208   EXPECT_FLOAT_EQ(5, p4.y());
00209   EXPECT_FLOAT_EQ(0, p4.z());
00210 
00211   p4 = swri_geometry_util::ProjectToLineSegment(p2, p1, p3);
00212   EXPECT_FLOAT_EQ(5, p4.x());
00213   EXPECT_FLOAT_EQ(5, p4.y());
00214   EXPECT_FLOAT_EQ(0, p4.z());
00215 
00216   p3 = tf::Vector3(-100, 10, 0);
00217   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00218   EXPECT_FLOAT_EQ(0, p4.x());
00219   EXPECT_FLOAT_EQ(0, p4.y());
00220   EXPECT_FLOAT_EQ(0, p4.z());
00221 }
00222 
00223 TEST(GeometryUtilTests, ProjectPointToLineSegment2D)
00224 {
00225   cv::Vec2d p1(0, 0);
00226   cv::Vec2d p2(10, 0);
00227   cv::Vec2d p3(2, 0);
00228   cv::Vec2d p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00229   EXPECT_FLOAT_EQ(p3[0], p4[0]);
00230   EXPECT_FLOAT_EQ(p3[1], p4[1]);
00231 
00232   p4 = swri_geometry_util::ProjectToLineSegment(p2, p1, p3);
00233   EXPECT_FLOAT_EQ(p3[0], p4[0]);
00234   EXPECT_FLOAT_EQ(p3[1], p4[1]);
00235 
00236   p3 = cv::Vec2d(0, 0);
00237   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00238   EXPECT_FLOAT_EQ(p3[0], p4[0]);
00239   EXPECT_FLOAT_EQ(p3[1], p4[1]);
00240 
00241   p3 = cv::Vec2d(0, 1);
00242   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00243   EXPECT_FLOAT_EQ(0, p4[0]);
00244   EXPECT_FLOAT_EQ(0, p4[1]);
00245 
00246   p3 = cv::Vec2d(5, -1);
00247   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00248   EXPECT_FLOAT_EQ(5, p4[0]);
00249   EXPECT_FLOAT_EQ(0, p4[1]);
00250 
00251   p3 = cv::Vec2d(-5, -1);
00252   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00253   EXPECT_FLOAT_EQ(0, p4[0]);
00254   EXPECT_FLOAT_EQ(0, p4[1]);
00255 
00256   p3 = cv::Vec2d(15, -1);
00257   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00258   EXPECT_FLOAT_EQ(10, p4[0]);
00259   EXPECT_FLOAT_EQ(0, p4[1]);
00260 
00261   p1 = cv::Vec2d(0, 0);
00262   p2 = cv::Vec2d(10, 10);
00263   p3 = cv::Vec2d(1, 1);
00264   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00265   EXPECT_FLOAT_EQ(1, p4[0]);
00266   EXPECT_FLOAT_EQ(1, p4[1]);
00267 
00268   p2 = cv::Vec2d(10, 10);
00269   p3 = cv::Vec2d(0, 10);
00270   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00271   EXPECT_FLOAT_EQ(5, p4[0]);
00272   EXPECT_FLOAT_EQ(5, p4[1]);
00273 
00274   p4 = swri_geometry_util::ProjectToLineSegment(p2, p1, p3);
00275   EXPECT_FLOAT_EQ(5, p4[0]);
00276   EXPECT_FLOAT_EQ(5, p4[1]);
00277 
00278   p3 = cv::Vec2d(-100, 10);
00279   p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
00280   EXPECT_FLOAT_EQ(0, p4[0]);
00281   EXPECT_FLOAT_EQ(0, p4[1]);
00282 }
00283 
00284 TEST(GeometryUtilTests, DistanceFromLineSegment3D)
00285 {
00286   tf::Vector3 p1(0, 0, 0);
00287   tf::Vector3 p2(10, 0, 0);
00288   tf::Vector3 p3(2, 0, 0);
00289   EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
00290   EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00291 
00292   p3 = tf::Vector3(0, 0, 0);
00293   EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00294 
00295   p3 = tf::Vector3(0, 1, 0);
00296   EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00297 
00298   p3 = tf::Vector3(5, -1, 0);
00299   EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00300 
00301   p3 = tf::Vector3(5, -1, 10);
00302   EXPECT_FLOAT_EQ(std::sqrt(101), swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00303 
00304   p3 = tf::Vector3(-5, -1, 10);
00305   EXPECT_FLOAT_EQ(std::sqrt(126), swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00306 
00307   p3 = tf::Vector3(15, -1, 10);
00308   EXPECT_FLOAT_EQ(std::sqrt(126), swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00309 
00310   p1 = tf::Vector3(0, 0, 0);
00311   p2 = tf::Vector3(10, 10, 10);
00312   p3 = tf::Vector3(1, 1, 1);
00313   EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00314 
00315   p2 = tf::Vector3(10, 10, 0);
00316   p3 = tf::Vector3(0, 10, 0);
00317   EXPECT_FLOAT_EQ(std::sqrt(50), swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00318   EXPECT_FLOAT_EQ(std::sqrt(50), swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
00319 
00320   p3 = tf::Vector3(-100, 10, 0);
00321   EXPECT_FLOAT_EQ(std::sqrt(10100), swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
00322 }
00323 
00324 TEST(GeometryUtilTests, DistanceFromLineSegment2D)
00325 {
00326   cv::Vec2d p1(0, 0);
00327   cv::Vec2d p2(10, 0);
00328   cv::Vec2d p3(2, 0);
00329   EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
00330   EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00331 
00332   p3 = cv::Vec2d(0, 0);
00333   EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00334 
00335   p3 = cv::Vec2d(0, 1);
00336   EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00337 
00338   p3 = cv::Vec2d(5, -1);
00339   EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00340 
00341   p1 = cv::Vec2d(0, 0);
00342   p2 = cv::Vec2d(10, 10);
00343   p3 = cv::Vec2d(1, 1);
00344   EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00345 
00346   p2 = cv::Vec2d(10, 10);
00347   p3 = cv::Vec2d(0, 10);
00348   EXPECT_FLOAT_EQ(std::sqrt(50), swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
00349   EXPECT_FLOAT_EQ(std::sqrt(50), swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
00350 
00351   p3 = cv::Vec2d(-100, 10);
00352   EXPECT_FLOAT_EQ(std::sqrt(10100), swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
00353 }
00354 
00355 
00356 TEST(GeometryUtilTests, PointInPolygon)
00357 {
00358   std::vector<cv::Vec2d> polygon1;
00359   polygon1.push_back(cv::Vec2d(1, 1));
00360   polygon1.push_back(cv::Vec2d(1, -1));
00361   polygon1.push_back(cv::Vec2d(-1, -1));
00362   polygon1.push_back(cv::Vec2d(-1, 1));
00363   EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(0, 0)));
00364   EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(0.99, 0.99)));
00365   EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(-0.99, 0.99)));
00366   EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(-0.99, -0.99)));
00367   EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(0.99, -0.99)));
00368   EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(2, 0)));
00369   EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(-2, 0)));
00370   EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(2, -2)));
00371 
00372   std::vector<cv::Vec2d> polygon2;
00373   polygon2.push_back(cv::Vec2d(1, 1));
00374   polygon2.push_back(cv::Vec2d(1, -1));
00375   polygon2.push_back(cv::Vec2d(-1, -1));
00376   polygon2.push_back(cv::Vec2d(-1, 1));
00377   polygon2.push_back(cv::Vec2d(1, 1));
00378   polygon2.push_back(cv::Vec2d(1, 1));
00379   EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(0, 0)));
00380   EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(0.99, 0.99)));
00381   EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(-0.99, 0.99)));
00382   EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(-0.99, -0.99)));
00383   EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(0.99, -0.99)));
00384   EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(2, 0)));
00385   EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(-2, 0)));
00386   EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(2, -2)));
00387 }
00388 
00389 TEST(GeometryUtilTests, DistanceFromPolygon)
00390 {
00391   std::vector<cv::Vec2d> polygon1;
00392   polygon1.push_back(cv::Vec2d(1, 1));
00393   polygon1.push_back(cv::Vec2d(1, -1));
00394   polygon1.push_back(cv::Vec2d(-1, -1));
00395   polygon1.push_back(cv::Vec2d(-1, 1));
00396   EXPECT_FLOAT_EQ(1.0, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(0, 0)));
00397   EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(0.99, 0.99)));
00398   EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(-0.99, 0.99)));
00399   EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(-0.99, -0.99)));
00400   EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(0.99, -0.99)));
00401   EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(2, 0)));
00402   EXPECT_FLOAT_EQ(1,swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(-2, 0)));
00403   EXPECT_FLOAT_EQ(std::sqrt(2),swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(2, -2)));
00404 
00405   std::vector<cv::Vec2d> polygon2;
00406   polygon2.push_back(cv::Vec2d(1, 1));
00407   polygon2.push_back(cv::Vec2d(1, -1));
00408   polygon2.push_back(cv::Vec2d(-1, -1));
00409   polygon2.push_back(cv::Vec2d(-1, 1));
00410   polygon2.push_back(cv::Vec2d(1, 1));
00411   polygon2.push_back(cv::Vec2d(1, 1));
00412   EXPECT_FLOAT_EQ(1.0, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(0, 0)));
00413   EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(0.99, 0.99)));
00414   EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(-0.99, 0.99)));
00415   EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(-0.99, -0.99)));
00416   EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(0.99, -0.99)));
00417   EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(2, 0)));
00418   EXPECT_FLOAT_EQ(1,swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(-2, 0)));
00419   EXPECT_FLOAT_EQ(std::sqrt(2),swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(2, -2)));
00420 }
00421 
00422 // Run all the tests that were declared with TEST()
00423 int main(int argc, char **argv)
00424 {
00425   testing::InitGoogleTest(&argc, argv);
00426 
00427   return RUN_ALL_TESTS();
00428 }