From af5d39695681238b35c404eec93dbb28a49dec48 Mon Sep 17 00:00:00 2001
From: Chris Ertel <chris@kedagital.com>
Date: Wed, 10 Jul 2019 14:29:41 -0400
Subject: [PATCH] Start adding triangle routines.

---
 lib/collision/intersection/capsule_capsule.ex |  4 +-
 lib/geometry/triangle.ex                      | 90 +++++++++++++++++++
 test/geometry_test.exs                        |  1 +
 3 files changed, 93 insertions(+), 2 deletions(-)
 create mode 100644 lib/geometry/triangle.ex

diff --git a/lib/collision/intersection/capsule_capsule.ex b/lib/collision/intersection/capsule_capsule.ex
index ed595d4..b8e2fc9 100644
--- a/lib/collision/intersection/capsule_capsule.ex
+++ b/lib/collision/intersection/capsule_capsule.ex
@@ -278,14 +278,14 @@ defmodule ElixirRigidPhysics.Collision.Intersection.CapsuleCapsule do
                 contacts:
                   {ContactPoint.contact_point(
                      world_point:
-                     direction
+                       direction
                        |> Vec3.scale(-(cr_a - penetration_depth / 2))
                        |> Vec3.add(clipped_nearest_1),
                      depth: penetration_depth
                    ),
                    ContactPoint.contact_point(
                      world_point:
-                     direction
+                       direction
                        |> Vec3.scale(-(cr_a - penetration_depth / 2))
                        |> Vec3.add(clipped_nearest_2),
                      depth: penetration_depth
diff --git a/lib/geometry/triangle.ex b/lib/geometry/triangle.ex
new file mode 100644
index 0000000..cac4738
--- /dev/null
+++ b/lib/geometry/triangle.ex
@@ -0,0 +1,90 @@
+defmodule ElixirRigidPhysics.Geometry.Triangle do
+  @moduledoc """
+  Module for handling queries related to planar 3D triangles
+  """
+  alias Graphmath.Vec3
+
+  require Record
+  Record.defrecord(:triangle, a: {0.0, 0.0, 0.0}, b: {0.0, 1.0, 0.0}, c: {0.0, 0.0, 1.0})
+  @type triangle :: record(:triangle, a: Vec3.vec3(), b: Vec3.vec3(), c: Vec3.vec3())
+
+  require ElixirRigidPhysics.Geometry.Plane, as: Plane
+
+  @doc """
+  Creates a triangle given three points.
+
+  ## Examples
+    iex> # IO.puts "Test basic triangle creation from points"
+    iex> require ElixirRigidPhysics.Geometry.Triangle, as: Triangle
+    iex> a = {1.0, 0.0, 1.0}
+    iex> b = {0.0, 0.0, 0.0}
+    iex> c = {3.0, 0.0, 1.0}
+    iex> Triangle.create_from_points( a, b, c )
+    {:triangle, {1.0, 0.0, 1.0}, {0.0, 0.0, 0.0}, {3.0, 0.0, 1.0}}
+  """
+  @spec create_from_points(Vec3.vec3(), Vec3.vec3(), Vec3.vec3()) :: triangle
+  def create_from_points(a, b, c) do
+    triangle(a: a, b: b, c: c)
+  end
+
+  @doc """
+  Creates plane from triangle.
+
+  ## Examples
+    iex> # IO.puts "Test plane creation from triangle"
+    iex> require ElixirRigidPhysics.Geometry.Triangle, as: Triangle
+    iex> a = {0.0, 0.0, 0.0}
+    iex> b = {1.0, 0.0, 0.0}
+    iex> c = {0.0, 0.0, 1.0}
+    iex> t= Triangle.create_from_points( a, b, c )
+    iex> Triangle.to_plane( t )
+    {:plane, 0.0, -1.0, 0.0, 0.0}
+
+    iex> # IO.puts "Test plane creation from scaled triangle"
+    iex> require ElixirRigidPhysics.Geometry.Triangle, as: Triangle
+    iex> a = {0.0, 0.0, 0.0}
+    iex> b = {2.0, 0.0, 0.0}
+    iex> c = {0.0, 0.0, 2.0}
+    iex> t= Triangle.create_from_points( a, b, c )
+    iex> Triangle.to_plane( t )
+    {:plane, 0.0, -1.0, 0.0, 0.0}
+
+    iex> # IO.puts "Test plane creation from flipped triangle"
+    iex> require ElixirRigidPhysics.Geometry.Triangle, as: Triangle
+    iex> a = {0.0, 0.0, 0.0}
+    iex> b = {1.0, 0.0, 0.0}
+    iex> c = {0.0, 0.0, 1.0}
+    iex> t= Triangle.create_from_points( a, c, b  )
+    iex> Triangle.to_plane( t )
+    {:plane, 0.0, 1.0, 0.0, 0.0}
+
+    iex> # IO.puts "Test plane creation from 3D triangle"
+    iex> require ElixirRigidPhysics.Geometry.Triangle, as: Triangle
+    iex> a = {0.0, 2.0, 0.0}
+    iex> b = {1.0, 2.0, 0.0}
+    iex> c = {0.0, 2.0, 1.0}
+    iex> t= Triangle.create_from_points( a, b, c )
+    iex> Triangle.to_plane( t )
+    {:plane, 0.0, -1.0, 0.0, 2.0}
+
+    iex> # IO.puts "Test plane creation from triangle"
+    iex> require ElixirRigidPhysics.Geometry.Triangle, as: Triangle
+    iex> a = {0.0, 0.0, 1.0}
+    iex> b = {0.0, 1.0, 0.0}
+    iex> c = {1.0, 0.0, 0.0}
+    iex> sqrt_3_over_3 = :math.sqrt(3)/3.0
+    iex> t= Triangle.create_from_points( a, b, c )
+    iex> {:plane, pa, pb, pc, pd} = Triangle.to_plane( t )
+    iex> Graphmath.Vec3.equal({pa,pb,pc},{-sqrt_3_over_3,-sqrt_3_over_3,-sqrt_3_over_3}, 0.000005)
+    true
+    iex> Float.round(pd - sqrt_3_over_3) == 0.0
+    true
+  """
+  @spec to_plane(triangle) :: Plane.plane()
+  def to_plane(triangle(a: a, b: b, c: c)) do
+    ab = Vec3.subtract(b, a)
+    ac = Vec3.subtract(c, a)
+    n = Vec3.cross(ab, ac) |> Vec3.normalize()
+    Plane.create(n, a)
+  end
+end
diff --git a/test/geometry_test.exs b/test/geometry_test.exs
index b77d586..8ecea4b 100644
--- a/test/geometry_test.exs
+++ b/test/geometry_test.exs
@@ -5,4 +5,5 @@ defmodule ElixirRigidPhysicsTest.GeometryTest do
   doctest ElixirRigidPhysics.Geometry.Sphere
   doctest ElixirRigidPhysics.Geometry.Util
   doctest ElixirRigidPhysics.Geometry.Plane
+  doctest ElixirRigidPhysics.Geometry.Triangle
 end