util3d: added intersectRayMesh() and intersectRayTriangle() functions

corelib/include/rtabmap/core/impl/util3d_surface.hpp

@@ -317,6 +317,42 @@ void denseMeshPostProcessing(
 	}
 }
 
+template<typename PointT>
+bool intersectRayMesh(
+		const Eigen::Vector3f & origin,
+		const Eigen::Vector3f & dir,
+		const typename pcl::PointCloud<PointT> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		bool ignoreBackFaces,
+		float & distance,
+		Eigen::Vector3f & normal,
+		int & index)
+{
+	bool intersect = false;
+	distance = std::numeric_limits<float>::max();
+	for (size_t i = 0; i < polygons.size(); ++i)
+	{
+		const pcl::Vertices & vert = polygons.at(i);
+		UASSERT(vert.vertices.size()==3);
+		float d;
+		Eigen::Vector3f n;
+		if (intersectRayTriangle(origin, dir,
+				cloud.at(vert.vertices.at(0)).getVector3fMap(),
+				cloud.at(vert.vertices.at(1)).getVector3fMap(),
+				cloud.at(vert.vertices.at(2)).getVector3fMap(), d, n) &&
+			d < distance &&
+			(!ignoreBackFaces || n.dot(dir)<0))
+		{
+			distance = d;
+			index = i;
+			normal = n;
+			intersect |= true;
+		}
+	}
+	normal.normalize();
+	return intersect;
+}
+
 }
 
 }

corelib/include/rtabmap/core/util3d_surface.h

@@ -422,6 +422,49 @@ void denseMeshPostProcessing(
 		int minClusterSize = 50,           // Remove small polygon clusters after the mesh has been cleaned (0=disabled)
 		ProgressState * progressState = 0);
 
+/**
+ * intersectRayTriangle(): find the 3D intersection of a ray with a triangle
+ *     Input:   p = origin of the ray
+ *              dir = direction of the ray
+ *              v0 = point 0 of the triangle
+ *              v1 = point 1 of the triangle
+ *              v2 = point 2 of the triangle
+ *     Output:  distance = distance from origin along ray direction
+ *              normal = normal of the triangle (not normalized)
+ *     Return: true = intersect in unique point inside the triangle
+ *
+ *     Intersection point can be computed with "I = p + dir*distance"
+ *
+ *  Copyright 2001 softSurfer, 2012 Dan Sunday
+ *  This code may be freely used and modified for any purpose
+ *  providing that this copyright notice is included with it.
+ *  SoftSurfer makes no warranty for this code, and cannot be held
+ *  liable for any real or imagined damage resulting from its use.
+ *  Users of this code must verify correctness for their application.
+ *
+ *   Mathieu:  Adapted for PCL format
+ */
+bool RTABMAP_EXP intersectRayTriangle(
+		const Eigen::Vector3f & p,
+		const Eigen::Vector3f & dir,
+		const Eigen::Vector3f & v0,
+		const Eigen::Vector3f & v1,
+		const Eigen::Vector3f & v2,
+		float & distance,
+		Eigen::Vector3f & normal);
+
+template<typename PointT>
+bool intersectRayMesh(
+		const Eigen::Vector3f & origin,
+		const Eigen::Vector3f & dir,
+		const typename pcl::PointCloud<PointT> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		bool ignoreBackFaces,
+		float & distance,
+		Eigen::Vector3f & normal,
+		int & index);
+
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/src/util3d_surface.cpp

@@ -3386,6 +3386,53 @@ pcl::PolygonMesh::Ptr meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float f
 	return output;
 }
 
+bool intersectRayTriangle(
+		const Eigen::Vector3f & p,
+		const Eigen::Vector3f & dir,
+		const Eigen::Vector3f & v0,
+		const Eigen::Vector3f & v1,
+		const Eigen::Vector3f & v2,
+		float & distance,
+		Eigen::Vector3f & normal)
+{
+    // get triangle edge cv::Vec3fs and plane normal
+    const Eigen::Vector3f u = v1-v0;
+    const Eigen::Vector3f v = v2-v0;
+    normal = u.cross(v);              // cross product
+    if (normal == Eigen::Vector3f(0,0,0))   // triangle is degenerate
+        return false;                 // do not deal with this case
+
+    const float denomimator = normal.dot(dir);
+    if (fabs(denomimator) < 10e-9)    // ray is  parallel to triangle plane
+       return false;
+
+    // get intersect of ray with triangle plane
+    distance = normal.dot(v0 - p) / denomimator;
+    if (distance < 0.0)                      // ray goes away from triangle
+        return false;
+
+    // is I inside T?
+    float    uu, uv, vv, wu, wv, D;
+    uu = u.dot(u);
+    uv = u.dot(v);
+    vv = v.dot(v);
+    const Eigen::Vector3f w = p + dir * distance - v0;
+    wu = w.dot(u);
+    wv = w.dot(v);
+    D = uv * uv - uu * vv;
+
+    // get and test parametric coords
+    float s, t;
+    s = (uv * wv - vv * wu) / D;
+    if (s < 0.0 || s > 1.0)         // I is outside T
+        return false;
+    t = (uv * wu - uu * wv) / D;
+    if (t < 0.0 || (s + t) > 1.0)   // I is outside T
+        return false;
+
+    return true;                    // I is in T
+}
+
 }
 
 }