Added CircleIntersects()

1 files changed, 87 insertions, 2 deletions

diff --git a/algorithm.mod/algorithm.bmx b/algorithm.mod/algorithm.bmx
index 187d2bb..1cac89f 100644
--- a/algorithm.mod/algorithm.bmx
+++ b/algorithm.mod/algorithm.bmx
@@ -15,7 +15,7 @@ Import brl.math
 Import brl.linkedlist
 
 Rem
-bbdoc: Used to return points from algorithms.
+bbdoc: Used to return integer points from algorithms.
 EndRem
 Type TAlgoPoint
 	Rem
@@ -42,6 +42,38 @@ Type TAlgoPoint
 End Type
 
 Rem
+bbdoc: Used to return 3D points from algorithms.
+EndRem
+Type TAlgoPoint3D
+	Rem
+	bbdoc: The X co-ordinate of the point
+	EndRem
+	Field	x:Double
+	Rem
+	bbdoc: The Y co-ordinate of the point
+	EndRem
+	Field	y:Double
+	Rem
+	bbdoc: The Z co-ordinate of the point
+	EndRem
+	Field	z:Double
+	
+	Rem
+	bbdoc: Create a point.
+	returns: Newly created point.
+	about: @x, @y and @z are initialised with the passed parameters.
+	EndRem
+	Function Create:TAlgoPoint3D(x:Double, y:Double, z:Double)
+		Local o:TAlgoPoint3D=New TAlgoPoint3D
+		o.x=x
+		o.y=y
+		o.z=z
+		Return o
+	End Function
+	
+End Type
+
+Rem
 bbdoc: Implements a Bresenham style line.
 returns: A list of points making up the line.
 about: @px1, @py1, @px2 and @py2 are the end points of the line.
@@ -136,6 +168,55 @@ Function DoCircle:TAlgoPoint[](x:Int, y:Int, r:Int)
 	Return p
 End Function
 
+
+Rem
+bbdoc: Returns the points where a sphere intersects a 3D line.
+returns: null if they don't intersect, one point if it intersects as a tangent, two points otherwise.
+about: @x1, @y1, @z1, @x2, @y2 and @z2 describe the line.  @cx, @cy, @cz and @rad describe the sphere.
+about: Leave the Z co-ords as zero to do a 2D check.
+EndRem
+Function CircleIntersects:TAlgoPoint3D[](x1:Double, y1:Double, z1:Double, x2:Double, y2:Double, z2:Double, cx:Double, cy:Double, cz:Double, rad:Double)
+	Local a:Double = Square(x2-x1) + Square(y2-y1) + Square(z2-z1)
+	Local b:Double = 2 * ( (x2-x1)*(x1-cx) + (y2-y1)*(y1-cy) + (z2-z1)*(z1-cz) )
+	Local c:Double = Square(x1) + Square(y1) + Square(z1) + Square(cx) + Square(cy) + Square(cz) - 2 * ( cx*x1 + cy*y1 + cz*z1 ) - Square(rad)
+	Local i:Double = b*b-4*a*c
+	
+	If i<0
+		Return Null
+	ElseIf i=1.0
+		Local m:Double = -b/(2*a)
+		If m<0.0 Or m>1.0
+			Return Null
+		EndIf
+		Local p:TAlgoPoint3D[]=New TAlgoPoint3D[1]
+		p[0]=TAlgoPoint3D.Create( x1+m*(x2-x1), y1+m*(y2-y1), z1 + m*(z2-z1))
+		Return p
+	Else
+		Local e:Double = Sqr(Square(b) - 4*a*c)
+		Local m1:Double = (-b + e) / (2*a)
+		Local m2:Double = (-b - e) / (2*a)
+		
+		If (m2<0.0 Or m2>1.0) And (m1<0.0 Or m1>1.0)
+			Return Null
+		ElseIf (m1<0.0 Or m1>1.0)
+			Local p:TAlgoPoint3D[]=New TAlgoPoint3D[1]
+			p[0]=TAlgoPoint3D.Create( x1+m2*(x2-x1), y1+m2*(y2-y1), z1+m2*(z2-z1))
+			Return p
+		ElseIf m2<0.0 Or m2>1.0
+			Local p:TAlgoPoint3D[]=New TAlgoPoint3D[1]
+			p[0]=TAlgoPoint3D.Create( x1+m1*(x2-x1), y1+m1*(y2-y1), z1+m1*(z2-z1))
+			Return p
+		Else
+			Local p:TAlgoPoint3D[]=New TAlgoPoint3D[2]
+			p[0]=TAlgoPoint3D.Create( x1+m1*(x2-x1), y1+m1*(y2-y1), z1+m1*(z2-z1))
+			p[1]=TAlgoPoint3D.Create( x1+m2*(x2-x1), y1+m2*(y2-y1), z1+m2*(z2-z1))
+			Return p
+		EndIf
+	EndIf
+
+End Function
+
+
 Private
 
 Type AlgoStatic
@@ -156,4 +237,8 @@ Type AlgoStatic
 			done=True
 		EndIf
 	End Function
-End Type
\ No newline at end of file
+End Type
+
+Function Square:Double(x:Double)
+	Return x*x
+End Function