From b54f8939423cddc838ffc156f855330dd8fcc2ab Mon Sep 17 00:00:00 2001
From: Eric Wasylishen <ewasylishen@gmail.com>
Date: Sat, 11 Feb 2017 01:58:18 -0700
Subject: [PATCH] common: add GLM_InterpolateNormal

---
 common/mathlib.cc         | 57 ++++++++++++++++++++++++++++++++++-----
 include/common/mathlib.hh |  5 +++-
 light/test_light.cc       | 43 +++++++++++++++++++++++++++++
 3 files changed, 97 insertions(+), 8 deletions(-)

diff --git a/common/mathlib.cc b/common/mathlib.cc
index dda75a5d..13748d8b 100644
--- a/common/mathlib.cc
+++ b/common/mathlib.cc
@@ -314,6 +314,20 @@ glm::vec4 GLM_PolyPlane(const std::vector<glm::vec3> &points)
     return vec4(normal, dist);
 }
 
+std::pair<bool, vec4>
+GLM_MakeEdgePlane(const vec3 &v0, const vec3 &v1, const vec3 &faceNormal)
+{
+    const float v0v1len = length(v1-v0);
+    if (v0v1len < POINT_EQUAL_EPSILON)
+        return make_pair(false, vec4(0));
+    
+    const vec3 edgedir = (v1 - v0) / v0v1len;
+    const vec3 edgeplane_normal = cross(edgedir, faceNormal);
+    const float edgeplane_dist = dot(edgeplane_normal, v0);
+    
+    return make_pair(true, vec4(edgeplane_normal, edgeplane_dist));
+}
+
 vector<vec4>
 GLM_MakeInwardFacingEdgePlanes(std::vector<vec3> points)
 {
@@ -331,15 +345,11 @@ GLM_MakeInwardFacingEdgePlanes(std::vector<vec3> points)
         const vec3 v0 = points.at(i);
         const vec3 v1 = points.at((i+1) % points.size());
         
-        const float v0v1len = length(v1-v0);
-        if (v0v1len < POINT_EQUAL_EPSILON)
+        const auto edgeplane = GLM_MakeEdgePlane(v0, v1, faceNormal);
+        if (!edgeplane.first)
             continue;
         
-        const vec3 edgedir = (v1 - v0) / v0v1len;
-        const vec3 edgeplane_normal = cross(edgedir, faceNormal);
-        const float edgeplane_dist = dot(edgeplane_normal, v0);
-        
-        result.push_back(vec4(edgeplane_normal, edgeplane_dist));
+        result.push_back(edgeplane.second);
     }
     
     return result;
@@ -436,4 +446,37 @@ std::pair<int, glm::vec3> GLM_ClosestPointOnPolyBoundary(const std::vector<glm::
     return make_pair(bestI, bestPointOnPoly);
 }
 
+std::pair<bool, glm::vec3> GLM_InterpolateNormal(const std::vector<glm::vec3> &points,
+                                                 const std::vector<glm::vec3> &normals,
+                                                 const glm::vec3 &point)
+{
+    Q_assert(points.size() == normals.size());
+    
+    // Step through the triangles, being careful to handle zero-size ones
 
+    const vec3 &p0 = points.at(0);
+    const vec3 &n0 = normals.at(0);
+    
+    const int N = points.size();
+    for (int i=2; i<N; i++) {
+        const vec3 &p1 = points.at(i-1);
+        const vec3 &n1 = normals.at(i-1);
+        const vec3 &p2 = points.at(i);
+        const vec3 &n2 = normals.at(i);
+     
+        const auto edgeplanes = GLM_MakeInwardFacingEdgePlanes({p0, p1, p2});
+        if (edgeplanes.empty())
+            continue;
+        
+        if (GLM_EdgePlanes_PointInside(edgeplanes, point)) {
+            // Found the correct triangle
+            
+            const vec3 bary = Barycentric_FromPoint(point, make_tuple(p0, p1, p2));
+            const vec3 interpolatedNormal = Barycentric_ToPoint(bary, make_tuple(n0, n1, n2));
+            
+            return make_pair(true, interpolatedNormal);
+        }
+    }
+    
+    return make_pair(false, vec3(0));
+}
diff --git a/include/common/mathlib.hh b/include/common/mathlib.hh
index ca62f267..7ce4a6ac 100644
--- a/include/common/mathlib.hh
+++ b/include/common/mathlib.hh
@@ -328,5 +328,8 @@ glm::vec3 GLM_PolyCentroid(const std::vector<glm::vec3> &points);
 glm::vec4 GLM_PolyPlane(const std::vector<glm::vec3> &points);
 /// Returns the index of the polygon edge, and the closest point on that edge, to the given point
 std::pair<int, glm::vec3> GLM_ClosestPointOnPolyBoundary(const std::vector<glm::vec3> &poly, const glm::vec3 &point);
-
+/// Returns `true` and the interpolated normal if `point` is in the polygon, otherwise returns false.
+std::pair<bool, glm::vec3> GLM_InterpolateNormal(const std::vector<glm::vec3> &points,
+                                                 const std::vector<glm::vec3> &normals,
+                                                 const glm::vec3 &point);
 #endif /* __COMMON_MATHLIB_H__ */
diff --git a/light/test_light.cc b/light/test_light.cc
index f8fc4f25..3b84c776 100644
--- a/light/test_light.cc
+++ b/light/test_light.cc
@@ -6,6 +6,7 @@
 #include <glm/vec3.hpp>
 #include <glm/vec4.hpp>
 #include <glm/gtx/string_cast.hpp>
+#include <glm/gtc/epsilon.hpp>
 
 using namespace glm;
 using namespace std;
@@ -368,3 +369,45 @@ TEST(mathlib, BarycentricRandom) {
         EXPECT_FLOAT_EQ(0.0f, GLM_DistAbovePlane(plane, point));
     }
 }
+
+TEST(mathlib, InterpolateNormals) {
+    // This test relies on the way GLM_InterpolateNormal is implemented
+    
+    // o--o--o
+    // | / / |
+    // |//   |
+    // o-----o
+    
+    const vector<vec3> poly {
+        { 0,0,0 },
+        { 0,64,0 },
+        { 32,64,0 }, // colinear
+        { 64,64,0 },
+        { 64,0,0 }
+    };
+    
+    const vector<vec3> normals {
+        { 1,0,0 },
+        { 0,1,0 },
+        { 0,0,1 }, // colinear
+        { 0,0,0 },
+        { -1,0,0 }
+    };
+    
+    // First try all the known points
+    for (int i=0; i<poly.size(); i++) {
+        const auto res = GLM_InterpolateNormal(poly, normals, poly.at(i));
+        EXPECT_EQ(true, res.first);
+        EXPECT_TRUE(all(epsilonEqual(normals.at(i), res.second, vec3(POINT_EQUAL_EPSILON))));
+    }
+    
+    {
+        const vec3 firstTriCentroid = (poly[0] + poly[1] + poly[2]) / 3.0f;
+        const auto res = GLM_InterpolateNormal(poly, normals, firstTriCentroid);
+        EXPECT_EQ(true, res.first);
+        EXPECT_TRUE(all(epsilonEqual(vec3(1/3.0f), res.second, vec3(POINT_EQUAL_EPSILON))));
+    }
+    
+    // Outside poly
+    EXPECT_FALSE(GLM_InterpolateNormal(poly, normals, vec3(-0.1, 0, 0)).first);
+}