Add function for projecting a point onto a plane. (#521)

swri-robotics · Sep 10, 2018 · ec0705e · ec0705e
1 parent 51b7ce5
commit ec0705e
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 1 deletion.
diff --git a/swri_geometry_util/include/swri_geometry_util/geometry_util.h b/swri_geometry_util/include/swri_geometry_util/geometry_util.h
@@ -48,11 +48,16 @@ namespace swri_geometry_util
       const tf::Vector3& plane_point,
       const tf::Vector3& point);
 
+  tf::Vector3 ProjectPointToPlane(
+      const tf::Vector3& plane_normal,
+      const tf::Vector3& plane_point,
+      const tf::Vector3& point);
+
   double DistanceFromLineSegment(
       const tf::Vector3& line_start,
       const tf::Vector3& line_end,
       const tf::Vector3& point);
-      
+
   tf::Vector3 ProjectToLineSegment(
       const tf::Vector3& line_start,
       const tf::Vector3& line_end,

diff --git a/swri_geometry_util/src/geometry_util.cpp b/swri_geometry_util/src/geometry_util.cpp
@@ -39,6 +39,15 @@ namespace swri_geometry_util
     return plane_normal.normalized().dot(point - plane_point);
   }
 
+  tf::Vector3 ProjectPointToPlane(
+      const tf::Vector3& plane_normal,
+      const tf::Vector3& plane_point,
+      const tf::Vector3& point)
+  {
+    double d = DistanceFromPlane(plane_normal, plane_point, point);
+    return point - plane_normal * d;
+  }
+
   double DistanceFromLineSegment(
       const tf::Vector3& line_start,
       const tf::Vector3& line_end,