added commented test. Removed pnpoly method which is not used in the …

…project
ADVRHumanoids · Feb 18, 2024 · b67ffdb · b67ffdb
1 parent 061e160
commit b67ffdb
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 47 deletions.
diff --git a/include/OpenSoT/utils/cartesian_utils.h b/include/OpenSoT/utils/cartesian_utils.h
@@ -267,43 +267,6 @@ template<class _Matrix_Type_> class SVDPseudoInverse
 class cartesian_utils
 {
 public:
-    /**
-     * @brief pnpoly this code is EXACTLY the code of the PNPOLY - Point Inclusion in Polygon Test
-        W. Randolph Franklin (WRF) to test if a point is inside a plygon
-        (https://www.ecse.rpi.edu/~wrf/Research/Short_Notes/pnpoly.html)
-     * @param nvert Number of vertices in the polygon
-     * @param vertx Arrays containing the x-coordinates of the polygon's vertices
-     * @param verty Arrays containing the y-coordinates of the polygon's vertices
-     * @param testx X-coordinate of the test point
-     * @param testy Y-coordinate of the test point
-     * @return
-     *          -1 if the point is outside of the
-     *           0 if the point is on an edge or at a vertes
-     *           1 if the point is inside of the polygon
-     *
-     * REMARKS:
-     *      The vertices may be listed clockwise or anticlockwise.
-     *      The first may optionally be repeated, if so nvert may optionally be increased by 1.
-     *      The input polygon may be a compound polygon constisting of several separate
-     *      subpolygons. If so, the first vertex of each subpolygon must be repeated, and when calculating
-     *      nvert, these first vertices must be counted twice.
-     *      Written by Randolph Franklin, University of Ottawa, 7/70.
-     *
-     * METHOD:
-     *      A vertical line is drawn thru the point in question. If it crosses the polygon an odd number
-     *      of times, then the point is inside of the polygon.
-     */
-    static int pnpoly(int nvert, float *vertx, float *verty, float testx, float testy)
-    {
-        int i, j, c = 0;
-          for (i = 0, j = nvert-1; i < nvert; j = i++) {
-            if ( ((verty[i]>testy) != (verty[j]>testy)) &&
-             (testx < (vertx[j]-vertx[i]) * (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i]) )
-               c = !c;
-          }
-          return c;
-    }
-
     /**
      * @brief computePanTiltMatrix given a gaze vector computes the Homogeneous Matrix to control the
      * YAW-PITCH angles.

diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
@@ -96,12 +96,12 @@ TARGET_LINK_LIBRARIES(testl1HQP ${TestLibs})
 add_dependencies(testl1HQP   OpenSoT)
 add_test(NAME OpenSoT_solvers_l1HQP COMMAND testl1HQP)
 
-# if(${OPENSOT_SOTH_FRONT_END})
-#     ADD_EXECUTABLE(testSOTH     solvers/TestSOTH.cpp)
-#     TARGET_LINK_LIBRARIES(testSOTH ${TestLibs})
-#     add_dependencies(testSOTH OpenSoT)
-#     add_test(NAME OpenSoT_solvers_soth COMMAND testSOTH)
-# endif()
+if(${OPENSOT_SOTH_FRONT_END})
+    ADD_EXECUTABLE(testSOTH     solvers/TestSOTH.cpp)
+    TARGET_LINK_LIBRARIES(testSOTH ${TestLibs})
+    add_dependencies(testSOTH OpenSoT)
+    add_test(NAME OpenSoT_solvers_soth COMMAND testSOTH)
+endif()
 
 ADD_EXECUTABLE(testBilateralConstraint     constraints/TestBilateralConstraint.cpp)
 TARGET_LINK_LIBRARIES(testBilateralConstraint ${TestLibs})
@@ -223,10 +223,10 @@ add_test(NAME OpenSoT_task_velocity_Postural COMMAND testPosturalVelocityTask)
 #     add_test(NAME OpenSoT_constraints_velocity_CollisionAvoidanceEnvironment COMMAND testCollisionAvoidanceEnvironmentVelocityConstraint)
 # endif()
 
-# ADD_EXECUTABLE(testCartesianUtils utils/cartesian_utils_test.cpp)
-# TARGET_LINK_LIBRARIES(testCartesianUtils ${TestLibs})
-# add_dependencies(testCartesianUtils   OpenSoT)
-# add_test(NAME OpenSoT_utils_cartesian_utils COMMAND testCartesianUtils)
+ ADD_EXECUTABLE(testCartesianUtils utils/cartesian_utils_test.cpp)
+ TARGET_LINK_LIBRARIES(testCartesianUtils ${TestLibs})
+ add_dependencies(testCartesianUtils   OpenSoT)
+ add_test(NAME OpenSoT_utils_cartesian_utils COMMAND testCartesianUtils)
 
  ADD_EXECUTABLE(testQPOases_SetActiveStack solvers/TestQPOases_SetActiveStack.cpp)
  TARGET_LINK_LIBRARIES(testQPOases_SetActiveStack ${TestLibs})