--- a/ai/steer.d	Wed Mar 25 11:28:25 2009 -0400
+++ b/ai/steer.d	Wed Mar 25 14:44:47 2009 -0400
@@ -32,7 +32,11 @@
  */
 module openmelee.ai.steer;
 
+import tango.io.Stdout : Stdout;
+
 import blaze.common.bzMath: bzDot, bzClamp, bzVec2;
+import blaze.collision.shapes.bzShape : bzShape;
+import tango.math.Math : sqrt;
 import blaze.dynamics.bzBody: bzBody;
 
 import openmelee.ships.ship : Ship, State;
@@ -43,7 +47,16 @@
     // Constructor: initializes state
     this (Ship ship)
     {
-        m_ship = ship;
+        m_ship = ship;
+        m_body = ship.rBody;
+        m_radius = 0.0f;
+        
+        // Find radius of body
+        for (bzShape s = m_body.shapeList; s; s = s.next) {
+            if(s.sweepRadius > m_radius) {
+                m_radius = s.sweepRadius;
+            }
+        }
     }
     
 	struct PathIntersection
@@ -135,37 +148,95 @@
         bzVec2 avoidance;
         PathIntersection nearest, next;
         float minDistanceToCollision = minTimeToCollision * m_speed;
-
+        
         next.intersect = false;
         nearest.intersect = false;
 
         // test all obstacles for intersection with my forward axis,
         // select the one whose point of intersection is nearest
-        for (bzBody o; o; o = o.next) {
+        for (bzBody o = obstacles; o; o = o.next) {
+            
+            if(o is m_body) continue;
+            
             // This code which presumes the obstacle is spherical
-            //findNextIntersectionWithSphere (o, next);
+            findNextIntersectionWithSphere(o, next);
 
             if (!nearest.intersect || (next.intersect && next.distance < nearest.distance)) {
 					nearest = next;
             }
         }
-
+        
         // when a nearest intersection was found
         if (nearest.intersect && (nearest.distance < minDistanceToCollision)) {
+            Stdout("Distance: ")(nearest.distance).newline;
             // compute avoidance steering force: take offset from obstacle to me,
             // take the component of that which is lateral (perpendicular to my
             // forward direction), set length to maxForce, add a bit of forward
             // component (in capture the flag, we never want to slow down)
             bzVec2 offset = m_position - nearest.obstacle.position;
-            //avoidance = Vector3Helpers.PerpendicularComponent(offset, this.Forward);
+            avoidance = perpendicularComponent(offset, m_forward);
             avoidance.normalize;
-            avoidance *= m_maxForce;
-            avoidance += m_forward * m_maxForce * 0.75f;
+            //avoidance *= m_maxForce;
+            //avoidance += m_forward * m_maxForce * 0.75f;
         }
 
         return avoidance;
     }
 
+		void findNextIntersectionWithSphere(bzBody obs, 
+                                            inout PathIntersection intersection) {
+                                                
+			// This routine is based on the Paul Bourke's derivation in:
+			//   Intersection of a Line and a Sphere (or circle)
+			//   http://www.swin.edu.au/astronomy/pbourke/geometry/sphereline/
+
+			float b, c, d, p, q, s;
+			bzVec2 lc;
+
+			// initialize pathIntersection object
+			intersection.intersect = false;
+			intersection.obstacle = obs;
+
+			// find "local center" (lc) of sphere in boid's coordinate space
+			lc = m_body.localPoint(obs.position);
+
+			// computer line-sphere intersection parameters
+			b = 0;
+            
+            // Find radius of obstacle
+            float obsRadius = 0;
+            for (bzShape shape = obs.shapeList; shape; shape = shape.next) {
+                if(shape.sweepRadius > obsRadius) {
+                    obsRadius = shape.sweepRadius;
+                }
+            }
+        
+			c = square(lc.x) + square(lc.y) - square(obsRadius + m_radius);
+			d = (b * b) - (4 * c);
+            
+			// when the path does not intersect the sphere
+			if (d < 0) return;
+
+			// otherwise, the path intersects the sphere in two points with
+			// parametric coordinates of "p" and "q".
+			// (If "d" is zero the two points are coincident, the path is tangent)
+			s = sqrt(d);
+			p = (-b + s) * 0.5f;
+			q = (-b - s) * 0.5f;
+
+			// both intersections are behind us, so no potential collisions
+			if ((p < 0) && (q < 0)) return;
+
+			// at least one intersection is in front of us
+			intersection.intersect = true;
+			intersection.distance =
+				((p > 0) && (q > 0)) ?
+				// both intersections are in front of us, find nearest one
+				((p < q) ? p : q) :
+				// otherwise only one intersections is in front, select it
+				((p > 0) ? p : q);
+		}
+
     /*
     // ------------------------------------------------------------------------
     // Unaligned collision avoidance behavior: avoid colliding with other
@@ -463,6 +534,8 @@
     bzVec2 m_up;
     bzVec2 m_side;
     bzVec2 m_forward;
+    float m_radius;
+    bzBody m_body;
 	
 	float m_speed = 0;
 	float m_maxForce = 0;