From a53f9913a838240ae35ac1fcfee81fc11a6f24df Mon Sep 17 00:00:00 2001
From: jeff <jeffbelg@efi.com>
Date: Wed, 6 Nov 2013 18:08:37 -0800
Subject: [PATCH] ray-plane and ray-sphere intersection

---
 src/cgmath/plane.rs  | 11 ++++++++++-
 src/cgmath/point.rs  |  2 +-
 src/cgmath/sphere.rs | 29 ++++++++++++++++++++++++++++-
 src/tests/plane.rs   | 16 ++++++++++++++++
 src/tests/sphere.rs  | 19 +++++++++++++++++++
 src/tests/test.rs    |  2 +-
 6 files changed, 75 insertions(+), 4 deletions(-)
 create mode 100644 src/tests/sphere.rs

diff --git a/src/cgmath/plane.rs b/src/cgmath/plane.rs
index b744a08..4e691d3 100644
--- a/src/cgmath/plane.rs
+++ b/src/cgmath/plane.rs
@@ -15,6 +15,7 @@
 
 use std::cast::transmute;
 use std::fmt;
+use std::num::Zero;
 
 use intersect::Intersect;
 use point::{Point, Point3};
@@ -22,6 +23,7 @@ use ray::Ray3;
 use vector::{Vec3, Vec4};
 use vector::{Vector, EuclideanVector};
 
+
 /// A 3-dimendional plane formed from the equation: `a*x + b*y + c*z - d = 0`.
 ///
 /// # Fields
@@ -86,7 +88,14 @@ impl<S: Float> Plane<S> {
 
 impl<S: Float> Intersect<Option<Point3<S>>> for (Plane<S>, Ray3<S>) {
     fn intersection(&self) -> Option<Point3<S>> {
-        fail!("Not yet implemented");
+        match *self {
+            (ref p, ref r) =>
+            {
+                  let t = -(p.d + r.origin.dot(&p.n)) / r.direction.dot(&p.n);
+                  if t < Zero::zero() { None }
+                  else { Some(r.origin.add_v(&r.direction.mul_s(t))) }
+            }
+        }
     }
 }
 
diff --git a/src/cgmath/point.rs b/src/cgmath/point.rs
index 388d7be..df75e2c 100644
--- a/src/cgmath/point.rs
+++ b/src/cgmath/point.rs
@@ -27,7 +27,7 @@ use vector::*;
 #[deriving(Eq, Zero, Clone)]
 pub struct Point2<S> { x: S, y: S }
 
-/// A point in 2-dimensional space.
+/// A point in 3-dimensional space.
 #[deriving(Eq, Zero, Clone)]
 pub struct Point3<S> { x: S, y: S, z: S }
 
diff --git a/src/cgmath/sphere.rs b/src/cgmath/sphere.rs
index f1e1035..2be7bd9 100644
--- a/src/cgmath/sphere.rs
+++ b/src/cgmath/sphere.rs
@@ -15,10 +15,37 @@
 
 //! Bounding sphere
 
-use point::Point3;
+use intersect::Intersect;
+use point::{Point, Point3};
+use ray::Ray3;
+use vector::Vector;
+
+use std::num::NumCast;
+use std::num;
+
+fn cast<T: NumCast, U: NumCast>(n: T) -> U {
+            num::cast(n).unwrap()
+}
 
 #[deriving(Clone, Eq)]
 pub struct Sphere<S> {
     center: Point3<S>,
     radius: S,
 }
+
+impl<S: Float> Intersect<Option<Point3<S>>> for (Sphere<S>, Ray3<S>) {
+    fn intersection(&self) -> Option<Point3<S>> {
+        match *self {
+            (ref s, ref r) => 
+            {
+                let l = s.center.sub_p(&r.origin);
+                let tca = l.dot(&r.direction);
+                if tca < cast(0.0) { return None; }
+                let d2 = l.dot(&l) - tca*tca;
+                if (d2 > s.radius*s.radius) { return None; }
+                let thc = num::sqrt(s.radius*s.radius - d2);
+                Some(r.origin.add_v(&r.direction.mul_s(tca - thc)))
+            }
+        }
+    }
+}
diff --git a/src/tests/plane.rs b/src/tests/plane.rs
index 422e352..b0e4a2b 100644
--- a/src/tests/plane.rs
+++ b/src/tests/plane.rs
@@ -15,6 +15,9 @@
 
 use cgmath::plane::*;
 use cgmath::point::*;
+use cgmath::vector::*;
+use cgmath::ray::*;
+use cgmath::intersect::Intersect;
 
 #[test]
 fn test_from_points() {
@@ -28,3 +31,16 @@ fn test_from_points() {
                                   Point3::new(0.0, 5.0,  5.0)),
         None);     // The points are parallel
 }
+
+#[test]
+fn test_ray_intersection() {
+    let p0 = Plane::from_abcd(1f64, 0f64, 0f64, -7f64);
+    let r0 = Ray3::new(Point3::new(2f64, 3f64, 4f64), Vec3::new(1f64, 1f64, 1f64).normalize());
+    assert_eq!((p0, r0).intersection(), Some(Point3::new(7f64, 8f64, 9f64)));
+
+    let p1 = Plane::from_points(Point3::new(5f64, 0f64,  5f64),
+                                  Point3::new(5f64, 5f64,  5f64),
+                                  Point3::new(5f64, 0f64, -1f64)).unwrap();
+    let r1 = Ray3::new(Point3::new(0f64, 0f64, 0f64), Vec3::new(-1f64, 0f64, 0f64).normalize());
+    assert_eq!((p1, r1).intersection(), None); // r1 points away from p1
+}
diff --git a/src/tests/sphere.rs b/src/tests/sphere.rs
new file mode 100644
index 0000000..9e84081
--- /dev/null
+++ b/src/tests/sphere.rs
@@ -0,0 +1,19 @@
+use cgmath::sphere::*;
+use cgmath::point::*;
+use cgmath::vector::*;
+use cgmath::ray::*;
+use cgmath::intersect::Intersect;
+use std::num;
+
+#[test]
+fn test_intersection() {
+    let sphere = Sphere {center: Point3::new(0f64,0f64,0f64), radius: 1f64};
+    let r0 = Ray3::new(Point3::new(0f64, 0f64, 5f64), Vec3::new(0f64, 0f64, -5f64).normalize());
+    let r1 = Ray3::new(Point3::new(num::cos(1f64), 0f64, 5f64), Vec3::new(0f64, 0f64, -5f64).normalize());
+    let r2 = Ray3::new(Point3::new(1f64, 0f64, 5f64), Vec3::new(0f64, 0f64, -5f64).normalize());
+    let r3 = Ray3::new(Point3::new(2f64, 0f64, 5f64), Vec3::new(0f64, 0f64, -5f64).normalize());
+    assert_eq!((sphere,r0).intersection(), Some(Point3::new(0f64, 0f64, 1f64)));
+    assert_approx_eq!((sphere,r1).intersection().unwrap(), Point3::new(num::cos(1f64), 0f64, num::sin(1f64)));
+    assert_eq!((sphere,r2).intersection(), Some(Point3::new(1f64, 0f64, 0f64)));
+    assert_eq!((sphere,r3).intersection(), None);
+}
diff --git a/src/tests/test.rs b/src/tests/test.rs
index 1621767..8a23c9b 100644
--- a/src/tests/test.rs
+++ b/src/tests/test.rs
@@ -37,4 +37,4 @@ pub mod transform;
 // pub mod frustum;
 // pub mod intersect;
 // pub mod obb;
-// pub mod sphere;
+pub mod sphere;