Implement plane mapper struct and use it in clipping functions

KeepersCompound.Lightmapper/MathUtils.cs

@@ -54,27 +54,79 @@ public static class MathUtils
         return Math.Abs(Vector3.Dot(plane.Normal, point) + plane.D) / plane.Normal.Length();
     }
 
+    public record PlanePointMapper
+    {
+        Vector3 _origin;
+        Vector3 _xAxis;
+        Vector3 _yAxis;
+
+        public PlanePointMapper(Vector3 normal, Vector3 p0, Vector3 p1)
+        {
+            _origin = p0;
+            _xAxis = p1 - _origin;
+            _yAxis = Vector3.Cross(normal, _xAxis);
+
+            _xAxis = Vector3.Normalize(_xAxis);
+            _yAxis = Vector3.Normalize(_yAxis);
+        }
+
+        public Vector2 MapTo2d(Vector3 point)
+        {
+            var offset = point - _origin;
+            var x = Vector3.Dot(offset, _xAxis);
+            var y = Vector3.Dot(offset, _yAxis);
+            return new Vector2(x, y);
+        }
+
+        public Vector2[] MapTo2d(Vector3[] points)
+        {
+            var points2d = new Vector2[points.Length];
+            for (var i = 0; i < points.Length; i++)
+            {
+                points2d[i] = MapTo2d(points[i]);
+            }
+            return points2d;
+        }
+
+        public Vector3 MapTo3d(Vector2 point)
+        {
+            return _origin + point.X * _xAxis + point.Y * _yAxis;
+        }
+
+        public Vector3[] MapTo3d(Vector2[] points)
+        {
+            var points3d = new Vector3[points.Length];
+            for (var i = 0; i < points.Length; i++)
+            {
+                points3d[i] = MapTo3d(points[i]);
+            }
+            return points3d;
+        }
+    }
+
     /// <summary>
     /// Expects poly to be convex. Given a point
     /// </summary>
     public static Vector3 ClipPointToPoly3d(Vector3 point, Vector3[] vertices, Plane projectionPlane)
     {
-        // TODO: Shouldn't need to pass 3d. We can just pass the luxel coord, and then we only need the 
+        var mapper = new PlanePointMapper(projectionPlane.Normal, vertices[0], vertices[1]);
-        var (p2d, v2ds) = LocalPlaneCoords(point, vertices, projectionPlane);
 
-        // !HACK: Replace this shit
+        var v2ds = mapper.MapTo2d(vertices);
-        var origin = vertices[0];
+        var p2d = mapper.MapTo2d(point);
-        var locX = vertices[1] - origin;
+        p2d = ClipPointToPoly2d(p2d, v2ds);
-        var locY = Vector3.Cross(projectionPlane.Normal, locX);
-        locX = Vector3.Normalize(locX);
-        locY = Vector3.Normalize(locY);
 
-        for (var i = 0; i < v2ds.Length; i++)
+        return mapper.MapTo3d(p2d);
+    }
+
+    public static Vector2 ClipPointToPoly2d(Vector2 point, Vector2[] vertices)
     {
-            var a = v2ds[i];
+        var vertexCount = vertices.Length;
-            var b = v2ds[(i + 1) % v2ds.Length];
+        for (var i = 0; i < vertexCount; i++)
+        {
+            var a = vertices[i];
+            var b = vertices[(i + 1) % vertexCount];
             var segment = b - a;
-            var offset = p2d - a;
+            var offset = point - a;
             var norm = Vector2.Normalize(new Vector2(-segment.Y, segment.X));
             var side = Vector2.Dot(norm, offset);
             if (side >= -Epsilon)
@@ -82,32 +134,10 @@ public static class MathUtils
                 // We apply epsilon so that we push slightly into the poly. If we only
                 // push to the edge then Embree sometimes misses casts. The reason
                 // it's 2 epsilon is so Side == -Epsilon still gets pushed in properly
-                p2d -= norm * (side + 2 * Epsilon);
+                point -= norm * (side + 2 * Epsilon);
             }
         }
 
-        return origin + p2d.X * locX + p2d.Y * locY;
+        return point;
-    }
-
-    // TODO: Only do this once per poly
-    public static (Vector2, Vector2[]) LocalPlaneCoords(Vector3 point, Vector3[] ps, Plane plane)
-    {
-        var origin = ps[0];
-        var locX = ps[1] - origin;
-        var locY = Vector3.Cross(plane.Normal, locX);
-
-        locX = Vector3.Normalize(locX);
-        locY = Vector3.Normalize(locY);
-
-        var offset = point - origin;
-        var p2d = new Vector2(Vector3.Dot(offset, locX), Vector3.Dot(offset, locY));
-        var p2ds = new Vector2[ps.Length];
-        for (var i = 0; i < ps.Length; i++)
-        {
-            var p = ps[i] - origin;
-            p2ds[i] = new Vector2(Vector3.Dot(p, locX), Vector3.Dot(p, locY));
-        }
-
-        return (p2d, p2ds);
     }
 }