import from local directory

2 files changed, 159 insertions, 0 deletions

diff --git a/src/main/scala/sims/collision/broadphase/BroadPhaseDetector.scala b/src/main/scala/sims/collision/broadphase/BroadPhaseDetector.scala
new file mode 100644
index 0000000..637da7a
--- /dev/null
+++ b/src/main/scala/sims/collision/broadphase/BroadPhaseDetector.scala
@@ -0,0 +1,39 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision.broadphase
+
+import sims.collision._
+import scala.collection.mutable.ArrayBuffer
+
+abstract class BroadPhaseDetector[A <: Collidable: ClassManifest] {
+	
+	protected var _items = new ArrayBuffer[A]
+	
+	/** Collidable items managed by this collision detector. */
+	def items: Seq[A] = _items
+	
+	/** Adds an item to this collision detector. */
+	def +=(item: A) = _items += item
+	
+	/** Adds a collection of items to this collision detector. */
+	def ++=(items: Iterable[A]) = for (i <- items) this += i
+	
+	/**Removes an item from this collision detector. */
+	def -=(item: A) = _items -= item
+	
+	/**Removes a collection of items from this collision detector. */
+	def --=(items: Iterable[A]) = for (i <- items) this -= i
+	
+	/**Removes all items from this collision detector. */
+	def clear() = _items.clear
+	
+	/** Applies a given function to every potentially colliding pair.
+	  * @param f function applied to every potentially colliding pair */
+	def foreach(f: ((A, A)) => Unit)
+
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/broadphase/SAP.scala b/src/main/scala/sims/collision/broadphase/SAP.scala
new file mode 100644
index 0000000..664f620
--- /dev/null
+++ b/src/main/scala/sims/collision/broadphase/SAP.scala
@@ -0,0 +1,120 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision.broadphase
+
+import sims.collision._
+import scala.collection.mutable.ArrayBuffer
+
+/** A broadphase collision detector implementing the "Sweep and Prune" algorithm.
+  *  
+  * The implementation of the broadphase algorithm was adapted from
+  * Real-Time Collision Detection by Christer Ericson, published by Morgan Kaufmann Publishers, (c) 2005 Elsevier Inc */
+class SAP[A <: Collidable: ClassManifest] extends BroadPhaseDetector[A]{
+
+	/*ordering along `axis`
+	 * x axis => 0
+	 * y axis => 1 */
+	private var axis = 0
+	
+	//use insert sort
+	private var almostSorted = false
+	private var sortedCount = 0
+	private val threshold = 3
+	
+	private implicit def ordering: Ordering[A] = new Ordering[A] {
+		def compare(x: A, y: A) = {
+			val delta = x.aabb.minVertex.components(axis) - y.aabb.minVertex.components(axis)
+			if (delta < 0) -1
+			else if(delta > 0) 1
+			else 0
+		}
+	}
+	
+	private def insertionSort(a: ArrayBuffer[A])(implicit ord: Ordering[A]) = {
+    import ord._
+		val length = a.length
+		var i = 1; while(i < length) {
+      var j = i
+      val t = a(j);
+      while (j>0 && a(j-1) > t) {
+      	a(j)=a(j-1)
+      	j -= 1
+      }
+      a(j)=t;
+      i += 1
+		}
+	}
+	
+	def foreach(f: ((A, A)) => Unit): Unit = {
+		
+		if (almostSorted)
+			insertionSort(_items)
+		else
+			_items = _items.sorted //quicksort
+	
+		var sumX, sumY = 0.0
+		var sumX2, sumY2 = 0.0
+		var varianceX, varianceY = 0.0
+	
+		var i = 0; while (i < _items.length) {
+		
+			//center point
+			val px = (_items(i).aabb.minVertex.x + _items(i).aabb.maxVertex.x) / 2
+			val py = (_items(i).aabb.minVertex.y + _items(i).aabb.maxVertex.y) / 2
+		
+			//update sum and sum2 for computing variance of AABB centers
+			sumX += px; sumY += py
+			sumX2 += px * px; sumY2 += py * py
+		
+			//collision test
+			var j = i + 1; var break = false; while(!break && j < _items.length) {
+			
+				//stop when tested AABBs are beyond the end of current AABB
+				if (_items(j).aabb.minVertex.components(axis) > _items(i).aabb.maxVertex.components(axis))
+				break = true
+				
+				//collision test here
+				else if (
+						(_items(i).fixed == false || _items(j).fixed == false) &&
+						(_items(i).aabb overlaps _items(j).aabb)
+				) f(_items(i), _items(j))
+			
+				j += 1
+			}
+			
+			i += 1
+		}
+	
+		varianceX = sumX2 - sumX * sumX
+		varianceY = sumY2 - sumY * sumY
+	
+		//choose sorting axis with greatest variance
+		var newAxis = 0
+		if (varianceX < varianceY) newAxis = 1
+		
+		if (axis == newAxis)
+			sortedCount += 1
+		else sortedCount = 0
+		
+		almostSorted = sortedCount > threshold 
+		//if sorting axis changes, items will no longer be almost sorted
+		//and thus quicksort should be used to reorder them
+		//almostSorted = axis == newAxis
+		
+		//update sorting axis
+		axis = newAxis
+		
+	}
+	
+}
+
+object SAP {
+	
+	def apply[A <: Collidable: ClassManifest] = new SAP[A]
+	
+}
\ No newline at end of file