From d37f53e190e237aa9fce34d6d72128a6fc89db03 Mon Sep 17 00:00:00 2001
From: EvilSpirit <anandamide@mail.ru>
Date: Sat, 19 Nov 2016 16:21:11 +0700
Subject: [PATCH] Improve performance of face picking.

This commit implements two improvements. First, it rewrites
SMesh::FirstIntersectionWith() to use an optimal (as currently known)
ray tracing algorithm. Second, it rejects triangles without
an associated face entity outright.
---
 src/mesh.cpp    | 33 ++++++++++++---------------------
 src/polygon.cpp | 41 +++++++++++++++++++++++++++++++++++++++++
 src/polygon.h   |  2 ++
 3 files changed, 55 insertions(+), 21 deletions(-)

diff --git a/src/mesh.cpp b/src/mesh.cpp
index c909f8b4..4231a198 100644
--- a/src/mesh.cpp
+++ b/src/mesh.cpp
@@ -327,32 +327,23 @@ bool SMesh::IsEmpty() const {
 }
 
 uint32_t SMesh::FirstIntersectionWith(Point2d mp) const {
-    Vector p0 = Vector::From(mp.x, mp.y, 0);
-    Vector gn = Vector::From(0, 0, 1);
+    Vector rayPoint = SS.GW.UnProjectPoint3(Vector::From(mp.x, mp.y, 0.0));
+    Vector rayDir = SS.GW.UnProjectPoint3(Vector::From(mp.x, mp.y, 1.0)).Minus(rayPoint);
 
-    double maxT = -1e12;
     uint32_t face = 0;
+    double faceT = VERY_NEGATIVE;
+    for(int i = 0; i < l.n; i++) {
+        const STriangle &tr = l.elem[i];
+        if(tr.meta.face == 0) continue;
 
-    int i;
-    for(i = 0; i < l.n; i++) {
-        STriangle tr = l.elem[i];
-        tr.a = SS.GW.ProjectPoint3(tr.a);
-        tr.b = SS.GW.ProjectPoint3(tr.b);
-        tr.c = SS.GW.ProjectPoint3(tr.c);
-
-        Vector n = tr.Normal();
-
-        if(n.Dot(gn) < LENGTH_EPS) continue; // back-facing or on edge
-
-        if(tr.ContainsPointProjd(gn, p0)) {
-            // Let our line have the form r(t) = p0 + gn*t
-            double t = -(n.Dot((tr.a).Minus(p0)))/(n.Dot(gn));
-            if(t > maxT) {
-                maxT = t;
-                face = tr.meta.face;
-            }
+        double t;
+        if(!tr.Raytrace(rayPoint, rayDir, &t, NULL)) continue;
+        if(t > faceT) {
+            face  = tr.meta.face;
+            faceT = t;
         }
     }
+
     return face;
 }
 
diff --git a/src/polygon.cpp b/src/polygon.cpp
index ca7afc82..20a8614f 100644
--- a/src/polygon.cpp
+++ b/src/polygon.cpp
@@ -42,6 +42,47 @@ bool STriangle::ContainsPointProjd(Vector n, Vector p) const {
     return true;
 }
 
+bool STriangle::Raytrace(const Vector &rayPoint, const Vector &rayDir,
+                         double *t, Vector *inters) const {
+    // Algorithm from: "Fast, Minimum Storage Ray/Triangle Intersection" by
+    // Tomas Moeller and Ben Trumbore.
+
+    // Find vectors for two edges sharing vertex A.
+    Vector edge1 = b.Minus(a);
+    Vector edge2 = c.Minus(a);
+
+    // Begin calculating determinant - also used to calculate U parameter.
+    Vector pvec = rayDir.Cross(edge2);
+
+    // If determinant is near zero, ray lies in plane of triangle.
+    // Also, cull back facing triangles here.
+    double det = edge1.Dot(pvec);
+    if(-det < LENGTH_EPS) return false;
+    double inv_det = 1.0f / det;
+
+    // Calculate distance from vertex A to ray origin.
+    Vector tvec = rayPoint.Minus(a);
+
+    // Calculate U parameter and test bounds.
+    double u = tvec.Dot(pvec) * inv_det;
+    if (u < 0.0f || u > 1.0f) return false;
+
+    // Prepare to test V parameter.
+    Vector qvec = tvec.Cross(edge1);
+
+    // Calculate V parameter and test bounds.
+    double v = rayDir.Dot(qvec) * inv_det;
+    if (v < 0.0f || u + v > 1.0f) return false;
+
+    // Calculate t, ray intersects triangle.
+    *t = edge2.Dot(qvec) * inv_det;
+
+    // Calculate intersection point.
+    if(inters != NULL) *inters = rayPoint.Plus(rayDir.ScaledBy(*t));
+
+    return true;
+}
+
 void STriangle::FlipNormal() {
     swap(a, b);
     swap(an, bn);
diff --git a/src/polygon.h b/src/polygon.h
index 8136860e..9d6ec659 100644
--- a/src/polygon.h
+++ b/src/polygon.h
@@ -188,6 +188,8 @@ public:
     bool ContainsPoint(Vector p) const;
     bool ContainsPointProjd(Vector n, Vector p) const;
     STriangle Transform(Vector o, Vector u, Vector v) const;
+    bool Raytrace(const Vector &rayPoint, const Vector &rayDir,
+                  double *t, Vector *inters) const;
 };
 
 class SBsp2 {