import from local directory

77 files changed, 4288 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..7e07ecf
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+target/
+project/boot/
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..4257d21
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,24 @@
+Copyright (c) 2010-2011 Jakob Odersky
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+1. Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+3. The name of the author may not be used to endorse or promote products
+   derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/README b/README
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/README
diff --git a/build.sbt b/build.sbt
new file mode 100644
index 0000000..7c993ee
--- /dev/null
+++ b/build.sbt
@@ -0,0 +1,7 @@
+name := "SiMS2"
+
+normalizedName := "sims2"
+
+version := "2.0"
+
+scalaVersion := "2.9.0-1"
diff --git a/lib/processing/core.jar b/lib/processing/core.jar
new file mode 100644
index 0000000..8d5f0c8
--- /dev/null
+++ b/lib/processing/core.jar
diff --git a/src/main/scala/sims/collision/AABB.scala b/src/main/scala/sims/collision/AABB.scala
new file mode 100644
index 0000000..68782d1
--- /dev/null
+++ b/src/main/scala/sims/collision/AABB.scala
@@ -0,0 +1,55 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import sims.math._
+
+/* 
+ * y
+ * ^
+ * |
+ * |  +-------+
+ * |  |    max|
+ * |  |       |
+ * |  |min    |
+ * |  +-------+
+ * |
+ * 0-------------->x
+ * 
+ */
+
+/** Axis Aligned Bounding Boxes (AABBs) are rectangles that frame a shape.
+  * Their X-axis and Y-axis orientation makes it easy to test two AABBs for overlap.
+  * @param minVertex Position vector of the bottom-left vertex
+  * @param maxVertex Position vector of the upper-right vertex */
+case class AABB(val minVertex: Vector2D,
+                val maxVertex: Vector2D) {
+	
+	/** Diagonal vector from `minVertex` to `maxVertex`. */
+	def diagonal = maxVertex - minVertex
+	
+	/** Width of this AABB. */
+	def width = maxVertex.x - minVertex.x
+	
+	/** Height of this AABB. */
+	def height = maxVertex.y - minVertex.y
+	
+	/** Checks if the given point is located within this AABB. */
+	def contains(point: Vector2D): Boolean = minVertex.x <= point.x && point.x <= maxVertex.x && minVertex.y <= point.y && point.y <= maxVertex.y
+	
+	/** Checks if the given AABB is located within this AABB. */
+	def contains(box: AABB): Boolean = contains(box.minVertex) && contains(box.maxVertex)
+	
+  /** Checks this AABB with <code>box</code> for overlap.
+    * @param box AABB with which to check for overlap */
+  def overlaps(box: AABB): Boolean = {
+    val d1 = box.minVertex - maxVertex
+    val d2 = minVertex - box.maxVertex
+    !(d1.x > 0 || d1.y > 0 || d2.x > 0 || d2.y > 0)
+  }
+}
diff --git a/src/main/scala/sims/collision/CachedCollidable.scala b/src/main/scala/sims/collision/CachedCollidable.scala
new file mode 100644
index 0000000..d3522a4
--- /dev/null
+++ b/src/main/scala/sims/collision/CachedCollidable.scala
@@ -0,0 +1,30 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+/** Implements features of a collidable object to be evaluated lazily and only
+  * to be be recomputed when the collidable object moves. */
+trait CachedCollidable extends Collidable {
+	
+	/** Invalidates all features and forces their evaluation on next call.
+	  * Should be called by clients when this object moves. */
+	def move() = {
+		_aabbValid = false
+	}
+	
+	private var _aabb: AABB = null
+	private var _aabbValid = false
+	abstract override def aabb = {
+		if (! _aabbValid) {
+			_aabb = super.aabb
+			_aabbValid = true
+		}
+		_aabb
+	}
+
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/Circle.scala b/src/main/scala/sims/collision/Circle.scala
new file mode 100644
index 0000000..f40da14
--- /dev/null
+++ b/src/main/scala/sims/collision/Circle.scala
@@ -0,0 +1,41 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import sims.math.Vector2D
+
+/** Properties implemented by a collidable circle.
+  * Note: this class does not define any physical properties, see sims.dynamics.Circle for that.
+  * @see sims.dynamics.Circle */
+trait Circle extends Collidable {
+	
+	/** Position of this circle. */
+	def position: Vector2D
+	
+	/** Radius of this circle. */
+	def radius: Double
+	
+	/** Returns this circle's axis aligned bounding box.
+    * @see collision.AABB */
+	override def aabb = new AABB(Vector2D(position.x - radius, position.y - radius), Vector2D(position.x + radius, position.y + radius))
+	
+	/** Checks if the point `point` is contained in this circle. */
+	override def contains(point: Vector2D) = (point - position).length <= radius
+	
+	/** Returns the projection of this polygon onto the line given by the directional vector `axis`. */
+	override def project(axis: Vector2D) = {
+		val dir = axis.unit
+		new Projection(
+				axis,
+				(position - dir * radius) dot dir,
+				(position + dir * radius) dot dir
+				)
+	}
+	
+	override def support(direction: Vector2D) = position + direction.unit * radius
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/Collidable.scala b/src/main/scala/sims/collision/Collidable.scala
new file mode 100644
index 0000000..951ef0e
--- /dev/null
+++ b/src/main/scala/sims/collision/Collidable.scala
@@ -0,0 +1,32 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import sims.math._
+
+/** A base trait for all collidable objects. */
+trait Collidable extends AnyRef {
+	
+	/** Returns an axis aligned box, bounding this collidable object. */
+	def aabb: AABB
+	
+	/** Projects this collidable object onto the given axis. */
+	def project(axis: Vector2D): Projection
+	
+	/** Checks if the point `point` is contained in this collidable object. */
+	def contains(point: Vector2D): Boolean
+	
+	/** Returns the farthest vertex of this collidable in the given direction. */
+	def support(direction: Vector2D): Vector2D
+		
+	/** A fixed collidable object cannot collide with other fixed collidable objects.
+	  * This is useful in improving collision detection performance, since a pair of fixed objects will
+	  * be eliminated in the broadphase. */
+	def fixed = false
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/Collision.scala b/src/main/scala/sims/collision/Collision.scala
new file mode 100644
index 0000000..29af3be
--- /dev/null
+++ b/src/main/scala/sims/collision/Collision.scala
@@ -0,0 +1,19 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import sims.math.Vector2D
+
+/** Contains information on the collision between two collidable items. */
+abstract class Collision[A <: Collidable] {
+	val item1: A
+	val item2: A
+	def normal: Vector2D
+	def points: Seq[Vector2D]
+	def overlap: Double
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/ContactPoint.scala b/src/main/scala/sims/collision/ContactPoint.scala
new file mode 100644
index 0000000..1ebe787
--- /dev/null
+++ b/src/main/scala/sims/collision/ContactPoint.scala
@@ -0,0 +1,11 @@
+package sims.collision
+
+import sims.math._
+
+class ContactPoint[A <: Collidable](
+		val item1: A,
+		val item2: A,
+		val normal: Vector2D,
+		val overlap: Double,
+		val point: Vector2D
+	)
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/ConvexPolygon.scala b/src/main/scala/sims/collision/ConvexPolygon.scala
new file mode 100644
index 0000000..3d7a81b
--- /dev/null
+++ b/src/main/scala/sims/collision/ConvexPolygon.scala
@@ -0,0 +1,60 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import sims.math._
+
+/** Common properties of all convex polygons.
+  * '''Note: convex polygons are not verified to be convex. It is up to the client to ensure this.''' */
+trait ConvexPolygon extends Polygon {
+	
+	   /**Returns the projection of this polygon onto the line given by the directional vector <code>axis</code>.
+   * @param axis directional vector of the line
+   * @return projection of this polygon*/
+  override def project(axis: Vector2D) = {
+  	val dir = axis.unit
+  	var min = vertices(0) dot dir
+  	var max = vertices(0) dot dir
+  	
+  	for (v <- vertices) {
+  		val d = v dot dir
+  		if (d < min) min = d
+  		if (d > max) max = d
+  	}
+  	
+    new Projection(axis, min, max)
+  }
+  
+  /**Checks if the point <code>point</code> is contained in this polygon.
+   * <p>
+   * A ray is created, originating from the point and following an arbitrary direction (X-Axis was chosen).
+   * The number of intersections between the ray and this polygon's sides (including vertices) is counted.
+   * The amount of intersections with vertices is subtracted form the previous number.
+   * If the latter number is odd, the point is contained in the polygon.*/
+  override def contains(point: Vector2D) = {
+    val r = new Ray(point, Vector2D.i)
+    var intersections = 0
+    for (s <- sides; if !(r intersection s).isEmpty) intersections += 1
+    for (v <- vertices; if (r contains v)) intersections -= 1
+    intersections % 2 != 0
+  }
+  
+  override def support(direction: Vector2D) = {
+  	var maxDistance = vertices(0) dot direction
+		var maxPoint = vertices(0)
+		for (v <- vertices) {
+			val s = v dot direction
+			if (s > maxDistance) {
+				maxDistance = s
+				maxPoint = v
+			}
+		}
+		maxPoint
+	}
+  
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/Detector.scala b/src/main/scala/sims/collision/Detector.scala
new file mode 100644
index 0000000..6b0559a
--- /dev/null
+++ b/src/main/scala/sims/collision/Detector.scala
@@ -0,0 +1,81 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import sims.collision.broadphase._
+import sims.collision.narrowphase._
+import scala.collection.mutable.ArrayBuffer
+
+/** Collision detectors are used to compute collisions between
+  * a given collection of items.
+  * They use a [[sims.collision.BroadPhaseDetector]] to determine potentially
+  * colliding pairs of items.
+  * These pairs are then examined with a [[sims.collision.NarrowPhaseDetector]]
+  * to compute the final collisions.
+  * 
+  * @param broadphase a broadphase collision detector
+  * @param narrowphase a narrowphase collision detector */
+class Detector[A <: Collidable: ClassManifest](
+		broadphase: BroadPhaseDetector[A],
+		narrowphase: NarrowPhaseDetector[A]
+		) {
+
+	/** Collidable items managed by this collision detector. */
+	def items: Seq[A] = broadphase.items
+	
+	/** Adds an item to this collision detector. */
+	def +=(item: A): Unit = broadphase +=  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): Unit = broadphase -= 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(): Unit = broadphase.clear
+	
+	/** Indicates the valid state of this collision detector. */
+	private var valid = false
+	
+	/** Invalidates this detector. The next time `collisions()` is called, all collisions will be
+	  * recomputed. */
+	def invalidate() = valid = false
+	
+	/** Cache of collisions. */
+	private var _collisions = new ArrayBuffer[Collision[A]]
+	
+	/** Returns a cached sequence of collisions between all items managed by this collision detector.
+	  * If no collisions were calculated since the last time `invalidate()` was called, the collisions
+	  * will be calculated. */
+	def collisions(): Seq[Collision[A]] = {
+		
+		if (!valid) {
+			
+			_collisions.clear()
+			
+			for (pair <- broadphase) {
+				val collision = narrowphase.collision(pair)
+				if (collision != None) _collisions += collision.get
+			}
+			
+			valid = true
+		}
+		
+		_collisions
+		
+	}
+	
+}
+
+class DetectorConstructor[A <: Collidable: ClassManifest] (broadphase: BroadPhaseDetector[A]) {
+	def narrowedBy(narrowphase: NarrowPhaseDetector[A]) = new Detector(broadphase, narrowphase)
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/Intersectable.scala b/src/main/scala/sims/collision/Intersectable.scala
new file mode 100644
index 0000000..7232bd3
--- /dev/null
+++ b/src/main/scala/sims/collision/Intersectable.scala
@@ -0,0 +1,22 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import sims.math._
+
+trait Intersectable[A <: Linear] {
+	
+	/**Computes the intersection between this linear element and `l`.
+	 * The intersection method does <b>not</b> correspond to the geometrical intersection.
+	 * Let A and B be two linear elements,
+	 * 
+	 * A and B intersect (i.e. an intersection point exists) \u21d4 card(A \u22c2 B) = 1
+	 * */
+	def intersection(l: A): Option[Vector2D] 
+
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/Linear.scala b/src/main/scala/sims/collision/Linear.scala
new file mode 100644
index 0000000..3c06515
--- /dev/null
+++ b/src/main/scala/sims/collision/Linear.scala
@@ -0,0 +1,67 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import scala.math._
+import sims.math._
+
+/**A base trait for all linear geometric elements specified by one point and a direction.
+ * @throws IllegalArgumentException if the directional vector is the null vector*/
+trait Linear {
+	
+	/**A point contained in this linear element.*/
+	val point: Vector2D
+	
+	/**Direction vector.*/
+	val direction: Vector2D
+	
+	/**Unit directional vector.*/
+  lazy val direction0 = direction.unit
+  
+  /**Right normal vector to <code>direction</code>.*/
+  lazy val rightNormal = direction.rightNormal
+  
+  /**Right normal unit vector to <code>direction</code>.*/
+  lazy val rightNormal0 = rightNormal.unit
+  
+  /**Left normal vector to <code>direction</code>.*/
+  lazy val leftNormal = direction.leftNormal
+  
+  /**Left normal unit vector to <code>direction</code>.*/
+  lazy val leftNormal0 = leftNormal.unit
+  
+  ///**Computes the closest point on this linear element to a given point.*/
+  //def closest(point: Vector2D): Vector2D
+  
+  ///**Computes the shortest distance form this linear element to a given point.*/
+  //def distance(point: Vector2D) = (closest(point) - point).length
+	
+	require(direction != 0, "A linear element's direction cannot be the null vector.")
+}
+
+object Linear {
+	
+	/** Clips a segment passing through points `p1` and `p2` to a half plain
+	  * given by a point `p` and a normal (pointing into the plain) `normal`. */
+	def clip(p1: Vector2D, p2: Vector2D, p: Vector2D, normal: Vector2D): List[Vector2D] = {
+  	val normal0 = normal.unit
+  	val direction = p2 - p1
+  	
+  	val d1 = (p1-p) dot normal0
+  	val d2 = (p2-p) dot normal0
+  		
+  	if (d1 < 0 && d2 < 0) return Nil
+  	if (d1 >= 0 && d2 >= 0) return List(p1, p2)
+  	
+  	val intersection = p1 + direction * abs(d1) / (abs(d1) + abs(d2))
+  	val inside = if (d1 > 0) p1 else p2
+  		
+  	List(inside, intersection)
+  }
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/Polygon.scala b/src/main/scala/sims/collision/Polygon.scala
new file mode 100644
index 0000000..c97b3d3
--- /dev/null
+++ b/src/main/scala/sims/collision/Polygon.scala
@@ -0,0 +1,32 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import sims.math._
+
+/**Common properties of all polygons.*/
+trait Polygon extends Collidable {
+	
+	/**Returns positions of all vertices of this Polygon. Vertices are ordered counter-clockwise.
+   * @return position vectors of the vertices*/
+  def vertices: Seq[Vector2D]
+  
+  /**Returns all sides of this polygon. The sides are ordered counter-clockwise, the first vertex of the side
+   * giving the side index.*/
+  def sides: Seq[Segment] = for (i <- 0 until vertices.length) yield (new Segment(vertices(i), vertices((i + 1) % vertices.length)))
+  
+  /**Returns this polygon's axis aligned bounding box.
+   * @see collision.AABB*/
+  override def aabb = {
+    val xs = vertices.view map (_.x)
+    val ys = vertices.view map (_.y)
+    new AABB(Vector2D(xs.min, ys.min),
+             Vector2D(xs.max, ys.max))
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/Projection.scala b/src/main/scala/sims/collision/Projection.scala
new file mode 100644
index 0000000..7617263
--- /dev/null
+++ b/src/main/scala/sims/collision/Projection.scala
@@ -0,0 +1,38 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import sims.math._
+
+/**Projection on an axis.
+ * <p>
+ * Projections are commonly used in SiMS for collision detection.
+ * @param axis directional vector of the axis of the projection
+ * @param lower lower value of the projection
+ * @param upper upper value of the projection*/
+case class Projection(axis: Vector2D,
+                      lower: Double,
+                      upper: Double) {
+  require(axis != Vector2D.Null, "A projection's axis cannot be given by a null vector!")
+  require(lower <= upper, "Invalid bounds. Lower must be less than or equal to upper.")
+  
+  /**Checks this projection for overlap with another projection.
+   * @throws IllegalArgumentExcepion if both projections have different axes*/
+  def overlaps(other: Projection): Boolean = {
+    require(axis == other.axis, "Cannot compare two projections on different axes!")
+      !((other.lower - this.upper) > 0 || (this.lower - other.upper) > 0)
+  }
+  
+  /**Returns the overlap between this projection and another projection.
+  * @throws IllegalArgumentExcepion if both projections have different axes*/
+  def overlap(other: Projection): Double = {
+    require(axis == other.axis, "Cannot compare two projections on different axes!")
+         math.min(upper, other.upper) - math.max(lower, other.lower)
+        
+  }
+}
diff --git a/src/main/scala/sims/collision/Ray.scala b/src/main/scala/sims/collision/Ray.scala
new file mode 100644
index 0000000..80a63fb
--- /dev/null
+++ b/src/main/scala/sims/collision/Ray.scala
@@ -0,0 +1,62 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import scala.math._
+import sims.math._
+
+/**A ray.
+ * @param point starting point of this ray
+ * @param direction this ray's directional vector
+ * @throws IllegalArgumentException if the directional vector is the null vector*/
+case class Ray(point: Vector2D, direction: Vector2D)
+	extends Linear
+	with Intersectable[Segment] {
+  
+	/*def closest(point: Vector2D) = {
+		var t = ((point - this.point) dot (direction)) / (direction dot direction)
+  	if (t < 0) t = 0
+  	this.point + direction * t
+	}*/
+	
+	def intersection(segment: Segment): Option[Vector2D] = {
+		
+		val n = segment.leftNormal
+  	
+  	// Handle case when two segments parallel
+  	if ((n dot direction) == 0) None
+  	else {
+  		val t = (n dot (segment.point1 - point)) / (n dot direction)
+  		val i = point + direction * t
+  		if (0 <= t && (i - segment.point1).length <= segment.length) Some(i)
+  		else None
+  	}
+		/*
+		// Returns 2 times the signed triangle area. The result is positive if
+  	// abc is ccw, negative if abc is cw, zero if abc is degenerate.
+  	def signed2DTriArea(a: Vector2D, b: Vector2D, c: Vector2D) = {
+  		(a.x - c.x) * (b.y - c.y) - (a.y - c.y) * (b.x - c.x);
+  	}
+  	
+  	if (signed2DTriArea(point, point + direction, segment.point1) * signed2DTriArea(point, point + direction, segment.point2) < 0) {
+  		val ab = segment.point2 - segment.point1
+  		val ac = segment.point2 - point
+  		val t = (ac.x * ab.y - ac.y * ab.x) / (direction.y * ab.x - direction.x - ab.y)
+  		if (t >= 0) Some(point + direction * t) else None
+  	} else None*/
+	}
+	
+  /**Checks if this ray contains the point <code>p</code>.*/
+  def contains(p: Vector2D) = {
+    val v = p - point
+    p == point ||
+    v ~ direction &&
+    signum(direction.x) == signum(v.x) &&
+    signum(direction.y) == signum(v.y)
+  }
+}
diff --git a/src/main/scala/sims/collision/Segment.scala b/src/main/scala/sims/collision/Segment.scala
new file mode 100644
index 0000000..45bd7b9
--- /dev/null
+++ b/src/main/scala/sims/collision/Segment.scala
@@ -0,0 +1,117 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision
+
+import scala.math._
+import sims.math._
+
+/** A segment passing through two points.
+  *  
+  * @param point1 position vector of the first point
+  * @param point2 position vector of the second point
+  * @throws IllegalArgumentException if both vertices are equal */
+case class Segment(point1: Vector2D, point2: Vector2D)
+	extends Linear
+	with Intersectable[Segment] {
+	
+  require(point1 != point2, "A segment must have two distinct vertices.")
+  
+  val point = point1
+  
+  /**Vector from <code>vertex1</code> to <code>vertex2</code>.*/
+  val direction = point2 - point
+  
+  /**Length of this segment.*/
+  val length = direction.length
+  
+
+  def closest(point: Vector2D) = {
+  	var t = ((point - point1) dot (direction)) / (direction dot direction)
+  	if (t < 0) t = 0
+  	if (t > 1) t = 1
+  	point1 + direction * t
+    }
+  
+  def distance(p: Vector2D) = {
+  	// For more concise code, the following substitutions are made:
+  	// * point1 -> a
+  	// * point2 -> b
+  	// * p      -> c
+  	
+  	val ab = direction
+  	val ac = p - point1
+  	val bc = p - point2
+  	
+  	val e = ac dot ab
+  	val distanceSquare = 
+  		// Handle cases where c projects outside ab
+  		if (e <= 0) ac dot ac
+  		else if (e >= (ab dot ab)) bc dot bc
+  		// Handle cases where c projects onto ab
+  		else (ac dot ac) - e * e / (ab dot ab)
+  	
+  	math.sqrt(distanceSquare)
+  }
+ 
+  def clipped(reference: Segment): List[Vector2D] = {
+		val clipped = Linear.clip(this.point1, this.point2, reference.point1, reference.direction)
+		if (clipped.length == 0) Nil
+		else Linear.clip(clipped(0), clipped(1), reference.point2, -reference.direction)
+	}
+	
+  
+  def intersection(segment: Segment): Option[Vector2D] = {
+  	val n = segment.leftNormal
+  	
+  	// Handle case when two segments parallel
+  	if ((n dot direction) == 0) None
+  	else {
+  		val t = (n dot (segment.point1 - point1)) / (n dot direction)
+  		val i = point + direction * t
+  		if (0 <= t && t <= 1 && (i - segment.point1).length <= segment.length) Some(i)
+  		else None
+  	}
+	 /*
+  	// Returns 2 times the signed triangle area. The result is positive if
+  	// abc is ccw, negative if abc is cw, zero if abc is degenerate.
+  	def signed2DTriArea(a: Vector2D, b: Vector2D, c: Vector2D) = {
+  		(a.x - c.x) * (b.y - c.y) - (a.y - c.y) * (b.x - c.x);
+  	}
+  	
+  	val a = point1; val b = point2; val c = segment.point; val d = segment.point2
+	
+    // Sign of areas correspond to which side of ab points c and d are
+    val a1 = signed2DTriArea(a, b, d); // Compute winding of abd (+ or -)
+    val a2 = signed2DTriArea(a, b, c); // To intersect, must have sign opposite of a1
+
+    // If c and d are on different sides of ab, areas have different signs
+    if (a1 * a2 < 0.0f) {
+        // Compute signs for a and b with respect to segment cd
+        val a3 = signed2DTriArea(c, d, a); // Compute winding of cda (+ or -)
+        // Since area is constant a1-a2 = a3-a4, or a4=a3+a2-a1
+        // float a4 = Signed2DTriArea(c, d, b); // Must have opposite sign of a3
+        val a4 = a3 + a2 - a1;
+        // Points a and b on different sides of cd if areas have different signs
+        if (a3 * a4 < 0.0f) {
+            // Segments intersect. Find intersection point along L(t)=a+t*(b-a).
+            // Given height h1 of a over cd and height h2 of b over cd,
+            // t = h1 / (h1 - h2) = (b*h1/2) / (b*h1/2 - b*h2/2) = a3 / (a3 - a4),
+            // where b (the base of the triangles cda and cdb, i.e., the length
+            // of cd) cancels out.
+            val t = a3 / (a3 - a4);
+            val p = a + (b - a) * t;
+            return Some(p);
+        }
+    }
+    // Segments not intersecting (or collinear)
+    return None;
+    */
+  }
+  
+  
+}
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
diff --git a/src/main/scala/sims/collision/narrowphase/NarrowPhaseDetector.scala b/src/main/scala/sims/collision/narrowphase/NarrowPhaseDetector.scala
new file mode 100644
index 0000000..783d366
--- /dev/null
+++ b/src/main/scala/sims/collision/narrowphase/NarrowPhaseDetector.scala
@@ -0,0 +1,16 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision.narrowphase
+
+import sims.collision._
+
+abstract class NarrowPhaseDetector[A <: Collidable: ClassManifest] {
+	
+	def collision(pair: (A, A)): Option[Collision[A]]
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/narrowphase/SAT.scala b/src/main/scala/sims/collision/narrowphase/SAT.scala
new file mode 100644
index 0000000..2e276b8
--- /dev/null
+++ b/src/main/scala/sims/collision/narrowphase/SAT.scala
@@ -0,0 +1,236 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.collision.narrowphase
+
+import sims.collision._
+import sims.math._
+
+class SAT[A <: Collidable: ClassManifest] extends NarrowPhaseDetector[A] {
+	
+	def collision(pair: (A, A)): Option[Collision[A]] = {
+		var c = getCollision(pair)
+		c orElse getCollision(pair.swap)
+	}
+	
+	private def getCollision(pair: (A,A)): Option[Collision[A]] = pair match {
+		
+		case (c1: Circle, c2: Circle) =>
+			collisionCircleCircle(c1, c2)(pair)
+			
+		case (p1: ConvexPolygon, p2: ConvexPolygon) =>
+			sat(pair, p1.sides.map(_.rightNormal0) ++ p2.sides.map(_.rightNormal0))
+			
+		case _ => None
+		
+	}
+	
+	private def collisionCircleCircle(c1: Circle, c2: Circle)(pair: (A, A)) = {
+		val d = (c2.position - c1.position)
+		val l = d.length
+		if (l <= c1.radius + c2.radius) {
+			val p = c1.position + d.unit * (l - c2.radius)
+			Some(new Collision[A] {
+				val item1 = pair._1
+				val item2 = pair._2
+				val normal = d.unit
+				val points = List(p)
+				val overlap = (c1.radius + c2.radius - l)
+			})
+		} else None
+	}
+	
+	private def collisionPolyPoly(p1: ConvexPolygon, p2: ConvexPolygon)(pair: (A, A)): Option[Collision[A]] = {
+		var minOverlap = Double.PositiveInfinity
+		var reference: ConvexPolygon = null
+		var incident: ConvexPolygon = null
+		var referenceSide: Segment = null
+		var incidentVerticeNumber = 0
+		
+		for (i <- 0 until p1.sides.length) {
+			var overlaps = false
+			
+			for (j <- 0 until p2.vertices.length) {
+				val s = p1.sides(i)
+				val v = p2.vertices(j)
+				
+				val overlap = (s.rightNormal0 dot s.point1) - (s.rightNormal0 dot v)
+				if (overlap > 0) overlaps = true
+				if (overlap > 0 && overlap < minOverlap.abs) {
+					minOverlap = overlap
+					
+					reference = p1
+					referenceSide = s
+					incident = p2
+					incidentVerticeNumber = j
+				}
+			}
+			if (!overlaps) return None
+		}
+		
+		for (i <- 0 until p2.sides.length) {	
+			var overlaps = false
+			
+			for (j <- 0 until p1.vertices.length) {
+				val s = p2.sides(i)
+				val v = p1.vertices(j)
+				
+				val overlap = (s.rightNormal0 dot s.point1) - (s.rightNormal0 dot v)
+				if (overlap > 0) overlaps = true
+				if (overlap > 0 && overlap < minOverlap.abs) {
+					minOverlap = overlap
+					
+					reference = p2
+					referenceSide = s
+					incident = p1
+					incidentVerticeNumber = j
+				}
+			}
+			if (!overlaps) return None
+		}
+		
+		val i = incidentVerticeNumber
+		val side1 = incident.sides(i)
+		val side2 = incident.sides(mod(i-1, incident.sides.length))
+		
+		val incidentSide = if ((side1.direction dot referenceSide.rightNormal0).abs <
+													(side2.direction dot referenceSide.rightNormal0).abs) side1
+											 else side2
+											 
+		val clipped: Segment = null //incidentSide clippedToSegment referenceSide
+		
+		Some(new Collision[A] {
+			val item1 = reference.asInstanceOf[A]
+			val item2 = incident.asInstanceOf[A]
+			val normal = referenceSide.rightNormal0
+			val points = List(clipped.point1, clipped.point2)
+			val overlap = minOverlap
+			
+		})
+		
+	}
+	
+	
+	
+	
+	def farthestFeature(collidable: Collidable, direction: Vector2D): Either[Vector2D, Segment] = collidable match {
+		
+		case c: Circle => Left(c.position + direction.unit * c.radius)
+		
+		case p: ConvexPolygon => {
+			var max = p.vertices(0) dot direction.unit
+			
+			//maximum vertice index
+			var i = 0
+  	
+			for (j <- 0 until p.vertices.length) {
+				val d = p.vertices(j) dot direction.unit
+				if (d > max) {
+					max = d
+					i = j
+				}
+			}
+			
+			/* 1) vertex is considered to be the first point of a segment
+			 * 2) polygons vertices are ordered counter-clockwise
+			 * 
+			 * implies:
+			 * previous segment is the (i-1)th segment
+			 * next segment is the ith segment */
+			val prev = if (i == 0) p.sides.last else p.sides(i - 1)
+			val next = p.sides(i)
+			
+			// check which segment is less parallel to direction
+			val side =
+				if ((prev.direction.unit dot direction).abs <= (next.direction.unit dot direction).abs) prev
+				else next
+			
+				Right(side)
+		}
+		
+		case _ => throw new IllegalArgumentException("Collidable is of unknown type.")
+		
+	}
+	
+
+	
+	//normal is considered pointing from _1 to _2
+	//_1 reference, _2 incident
+	def getCollisionPoints(pair: (A, A), normal: Vector2D, overlap: Double): Array[Vector2D] = {
+		var points = new scala.collection.mutable.ArrayBuffer[Vector2D]
+		
+		val feature1 = farthestFeature(pair._1, normal)
+		
+		//is feature 1 a vertex?
+		if (feature1.isLeft) {
+			return Array(feature1.left.get)
+		}
+		
+		val feature2 = farthestFeature(pair._2, -normal)
+		
+		//is feature 2 a vertex?
+		if (feature2.isLeft) {
+			return Array(feature2.left.get)
+		}
+		
+		//neither feature is a vertex
+		val side1 = feature1.right.get
+		val side2 = feature2.right.get
+		
+		
+		val flipped = (side1.direction.unit dot normal).abs > (side2.direction.unit dot -normal).abs
+		val reference = if (!flipped) side1 else side2
+		val incident = if (!flipped) side2 else side1
+		
+		
+		//both features are sides, clip feature2 to feature1
+		val clipped: Option[Segment] = None //incident clipped reference
+		
+		clipped match {
+			case None => Array()
+			case Some(Segment(point1, point2)) => Array(point1, point2) filter ((v: Vector2D) => ((v - reference.point1) dot reference.rightNormal0) <= 0)
+		}
+	}
+	
+	def sat(pair: (A, A), axes: Seq[Vector2D]): Option[Collision[A]] = {
+		var min = Double.PositiveInfinity
+		var n = axes(0)
+		
+		for (axis <- axes) {
+			val overlap = pair._1.project(axis) overlap pair._2.project(axis)
+			if (overlap < 0) return None
+			if (overlap < min) {
+				min = overlap
+				n = axis
+			}
+		}
+			
+		val pts = getCollisionPoints(pair, n, min)
+
+		if (pts.length == 0) return None
+		
+		Some(new Collision[A] {
+			val item1 = pair._1
+			val item2 = pair._2
+			val normal = n
+			val points = pts.toSeq
+			val overlap = min
+		})
+		
+		
+			
+	}
+	
+	
+	
+}
+
+object SAT {
+	
+	def apply[A <: Collidable: ClassManifest] = new SAT[A]
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/narrowphase/gjk/CS.scala b/src/main/scala/sims/collision/narrowphase/gjk/CS.scala
new file mode 100644
index 0000000..0d97b37
--- /dev/null
+++ b/src/main/scala/sims/collision/narrowphase/gjk/CS.scala
@@ -0,0 +1,83 @@
+package sims.collision.narrowphase
+package gjk
+
+import scala.math._
+import sims.collision._
+import sims.math._
+
+object CS {
+	
+	def farthestFeature(collidable: Collidable, direction: Vector2D): Either[Vector2D, Segment] = collidable match {		
+		case c: Circle => Left(c.support(direction))
+		
+		case p: ConvexPolygon => {
+			var max = p.vertices(0) dot direction.unit
+			
+			//maximum vertice index
+			var i = 0
+  	
+			for (j <- 0 until p.vertices.length) {
+				val d = p.vertices(j) dot direction.unit
+				if (d > max) {
+					max = d
+					i = j
+				}
+			}
+			
+			/* 1) vertex is considered to be the first point of a segment
+			 * 2) polygons vertices are ordered counter-clockwise
+			 * 
+			 * implies:
+			 * previous segment is the (i-1)th segment
+			 * next segment is the ith segment */
+			val prev = if (i == 0) p.sides.last else p.sides(i - 1)
+			val next = p.sides(i)
+			
+			// check which segment is less parallel to direction
+			val side =
+				if ((prev.direction0 dot direction).abs <= (next.direction0 dot direction).abs) prev
+				else next
+			
+				Right(side)
+		}
+		
+		case _ => throw new IllegalArgumentException("Collidable is of unknown type.")
+		
+	}
+	
+	def getCollisionPoints(pair: (Collidable, Collidable), normal: Vector2D): Manifold = {
+		var points = new scala.collection.mutable.ArrayBuffer[Vector2D]
+		
+		val feature1 = farthestFeature(pair._1, normal)
+		
+		//is feature 1 a vertex?
+		if (feature1.isLeft) {
+			return new Manifold(List(feature1.left.get), normal)
+		}
+		
+		val feature2 = farthestFeature(pair._2, -normal)
+		
+		//is feature 2 a vertex?
+		if (feature2.isLeft) {
+			return new Manifold(List(feature2.left.get), -normal)
+		}
+		
+		//neither feature is a vertex
+		val side1 = feature1.right.get
+		val side2 = feature2.right.get
+		
+		
+		val flipped = (side1.direction0 dot normal).abs > (side2.direction0 dot normal).abs
+		val reference = if (!flipped) side1 else side2
+		val incident = if (!flipped) side2 else side1
+		val n = if (!flipped) normal else -normal
+		
+		//both features are sides, clip feature2 to feature1
+		val clipped = incident clipped reference
+		
+		val c = clipped filter ((v: Vector2D) => ((v - reference.point1) dot n) > 0)
+		
+		new Manifold(c, n)
+	}
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/narrowphase/gjk/EPA.scala b/src/main/scala/sims/collision/narrowphase/gjk/EPA.scala
new file mode 100644
index 0000000..9f53690
--- /dev/null
+++ b/src/main/scala/sims/collision/narrowphase/gjk/EPA.scala
@@ -0,0 +1,98 @@
+package sims.collision.narrowphase
+package gjk
+
+import scala.collection.mutable.ListBuffer
+import sims.collision._
+import sims.math._
+
+/** The implementation was adapted from dyn4j by William Bittle (see http://www.dyn4j.org). */
+object EPA {
+	
+	val MaxIterations = 20
+	
+	val DistanceEpsilon = 0.0001
+	
+	private class Edge(val normal: Vector2D, val distance: Double, val index: Int)
+	
+	private def winding(simplex: ListBuffer[Vector2D]) = {
+		for (i <- 0 until simplex.size) {
+			val j = if (i + 1 == simplex.size) 0 else i + 1
+			//val winding = math.signum(simplex(j) - simplex(i))
+		}
+		
+	}
+		
+	
+	def penetration(simplex: ListBuffer[Vector2D], minkowskiSum: MinkowskiSum): Penetration = {
+		// this method is called from the GJK detect method and therefore we can assume
+		// that the simplex has 3 points
+		
+		// get the winding of the simplex points
+		// the winding may be different depending on the points added by GJK
+		// however EPA will preserve the winding so we only need to compute this once
+		val winding = math.signum((simplex(1) - simplex(0)) cross (simplex(2) - simplex(1))).toInt
+		// store the last point added to the simplex
+		var point = Vector2D.Null
+		// the current closest edge
+		var edge: Edge = null;
+		// start the loop
+		for (i <- 0 until MaxIterations) {
+			// get the closest edge to the origin
+			edge = this.findClosestEdge(simplex, winding);
+			// get a new support point in the direction of the edge normal
+			point = minkowskiSum.support(edge.normal);
+			
+			// see if the new point is significantly past the edge
+			val projection: Double = point dot edge.normal
+			if ((projection - edge.distance) < DistanceEpsilon) {
+				// then the new point we just made is not far enough
+				// in the direction of n so we can stop now and
+				// return n as the direction and the projection
+				// as the depth since this is the closest found
+				// edge and it cannot increase any more
+				return new Penetration(edge.normal, projection)
+			}
+			
+			// lastly add the point to the simplex
+			// this breaks the edge we just found to be closest into two edges
+			// from a -> b to a -> newPoint -> b
+			simplex.insert(edge.index, point);
+		}
+		// if we made it here then we know that we hit the maximum number of iterations
+		// this is really a catch all termination case
+		// set the normal and depth equal to the last edge we created
+		new Penetration(edge.normal, point dot edge.normal)
+	}
+	
+	@inline private def findClosestEdge(simplex: ListBuffer[Vector2D], winding: Int): Edge = {
+		// get the current size of the simplex
+		val size = simplex.size;
+		// create an edge
+		var edge = new Edge(Vector2D.Null, Double.PositiveInfinity, 0)
+		// find the edge on the simplex closest to the origin
+		for (i <- 0 until size) {
+			// compute j
+			val j = if (i + 1 == size) 0 else i + 1
+			// get the points that make up the current edge
+			val a = simplex(i);
+			val b = simplex(j);
+			// create the edge
+			val direction = simplex(j) - simplex(i);
+			// depending on the winding get the edge normal
+			// it would findClosestEdge(List<Vector2> simplex, int winding) {
+			// get the current size of the simplex
+			val normal = if (winding > 0) direction.rightNormal.unit
+									 else direction.leftNormal.unit
+		
+			// project the first point onto the normal (it doesnt matter which
+			// you project since the normal is perpendicular to the edge)
+			val d: Double = math.abs(simplex(i) dot normal);
+			// record the closest edge
+			if (d < edge.distance) 
+				edge = new Edge(normal, d, j)
+		}
+		// return the closest edge
+		edge
+	}
+
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/narrowphase/gjk/GJK.scala b/src/main/scala/sims/collision/narrowphase/gjk/GJK.scala
new file mode 100644
index 0000000..71ccee3
--- /dev/null
+++ b/src/main/scala/sims/collision/narrowphase/gjk/GJK.scala
@@ -0,0 +1,112 @@
+package sims.collision.narrowphase
+package gjk
+
+import sims.collision._
+import sims.math._
+import scala.collection.mutable.ListBuffer
+
+class GJK[A <: Collidable: ClassManifest] extends narrowphase.NarrowPhaseDetector[A] {
+	
+	
+	def penetration(pair: (A, A)): Option[Penetration] = {
+		val ms = new MinkowskiSum(pair)		
+		val s = ms.support(Vector2D.i)
+		val simplex = new ListBuffer[Vector2D]
+		simplex prepend s
+		var direction = -s
+		
+		while (true) {
+			
+			val a = ms.support(direction)
+			
+			if ((a dot direction) < 0) return None
+			
+			simplex prepend a
+			
+			val newDirection = checkSimplex(simplex, direction)
+			
+			if (newDirection == null) return Some(EPA.penetration(simplex, ms))
+			else direction = newDirection
+			
+		}
+		
+		throw new IllegalArgumentException("Something went wrong, should not reach here.")
+	}
+	
+	/** Checks whether the given simplex contains the origin. If it does, `null` is returned.
+	  * Otherwise a new search direction is returned and the simplex is updated. */
+	private def checkSimplex(simplex: ListBuffer[Vector2D], direction: Vector2D): Vector2D = {
+		if (simplex.length == 2) { //simplex == 2
+			val a = simplex(0)
+			val b = simplex(1)
+			val ab = b - a
+			val ao = -a
+			
+			if (ao directionOf ab) {
+				ab cross ao cross ab
+			} else {
+				simplex.remove(1)
+				ao
+			}
+		} // end simplex == 2
+		
+		else if (simplex.length == 3) { //simplex == 3
+			val a = simplex(0)
+			val b = simplex(1)
+			val c = simplex(2)
+			val ab = b - a
+			val ac = c - a
+			val ao = -a
+			val winding = ab cross ac
+			
+			if (ao directionOf (ab cross winding)) {
+				if (ao directionOf ab) {
+					simplex.remove(2)
+					ab cross ao cross ab
+				} else if (ao directionOf ac) {
+					simplex.remove(1)
+					ac cross ao cross ac
+				} else {
+					simplex.remove(2)
+					simplex.remove(1)
+					ao
+				}
+			} else {
+				if (ao directionOf (winding cross ac)) {
+					if (ao directionOf ac) {
+						simplex.remove(1)
+						ac cross ao cross ac
+					} else {
+						simplex.remove(2)
+						simplex.remove(1)
+						ao
+					}
+				} else {
+					null
+				}
+			}
+		} //end simplex == 3
+		
+		else throw new IllegalArgumentException("Invalid simplex size.")
+		
+	}
+	
+	def collision(pair: (A, A)): Option[Collision[A]] = {
+		val p = penetration(pair)
+		if (p.isEmpty) return None
+		val manif = CS.getCollisionPoints(pair, p.get.normal)
+		Some(new Collision[A] {
+			val item1 = pair._1 
+			val item2 = pair._2 
+			val normal = manif.normal
+			val overlap = p.get.overlap
+			val points = manif.points
+		})
+		
+	}
+
+}
+
+object GJK {
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/narrowphase/gjk/GJK2.scala b/src/main/scala/sims/collision/narrowphase/gjk/GJK2.scala
new file mode 100644
index 0000000..81764d4
--- /dev/null
+++ b/src/main/scala/sims/collision/narrowphase/gjk/GJK2.scala
@@ -0,0 +1,168 @@
+package sims.collision.narrowphase.gjk
+
+/**
+ * Created by IntelliJ IDEA.
+ * User: jakob
+ * Date: 3/27/11
+ * Time: 7:47 PM
+ * To change this template use File | Settings | File Templates.
+ */
+
+import scala.collection.mutable.ListBuffer
+import sims.math._
+import sims.collision._
+import sims.collision.narrowphase.NarrowPhaseDetector
+
+case class Separation(distance: Double, normal: Vector2D, point1: Vector2D, point2: Vector2D)
+class GJK2[A <: Collidable: ClassManifest] extends NarrowPhaseDetector[A] {
+
+  val margin = 0.01
+
+  case class MinkowskiPoint(convex1: Collidable, convex2: Collidable, direction: Vector2D) {
+    val point1 = convex1.support(direction) - direction.unit * margin
+    val point2 = convex2.support(-direction) + direction.unit * margin
+    val point = point1 - point2
+  }
+  
+  def support(c1: A, c2: A, direction: Vector2D) = MinkowskiPoint(c1, c2, direction)
+  implicit def minkowsi2Vector(mp: MinkowskiPoint) = mp.point
+
+  def separation(c1: A, c2: A): Option[(Separation)] = {
+
+    //initial search direction
+    val direction0 = Vector2D.i
+
+    //simplex points
+    var a = support(c1, c2, direction0)
+    var b = support(c1, c2, -direction0)
+
+    var counter = 0
+    while (counter < 100) {
+
+      //closest point on the current simplex closest to origin
+      val point = segmentClosestPoint(a, b,Vector2D.Null)
+
+      if (point.isNull) return None
+
+      //new search direction
+      val direction = -point.unit
+
+      //new Minkowski Sum point
+      val c = support(c1, c2, direction)
+
+      if (containsOrigin(a, b, c)) return None
+
+      val dc = (direction dot c)
+      val da = (direction dot a)
+
+      if (dc - da < 0.0001) {
+        val (point1, point2) = findClosestPoints(a, b)
+        return Some(Separation(dc, direction, point1, point2))
+      }
+
+      if (a.lengthSquare < b.lengthSquare) b = c
+      else a = c
+      //counter += 1
+    }
+    return None
+  }
+
+
+  def findClosestPoints(a: MinkowskiPoint, b: MinkowskiPoint): (Vector2D, Vector2D) =  {
+		var p1 = Vector2D.Null
+		var p2 = Vector2D.Null
+
+		// find lambda1 and lambda2
+		val l: Vector2D = b - a
+
+		// check if a and b are the same point
+		if (l.isNull) {
+			// then the closest points are a or b support points
+			p1 = a.point1
+			p2 = a.point2
+		} else {
+			// otherwise compute lambda1 and lambda2
+			val ll = l dot l;
+			val l2 = -l.dot(a) / ll;
+			val l1 = 1 - l2;
+
+			// check if either lambda1 or lambda2 is less than zero
+			if (l1 < 0) {
+				// if lambda1 is less than zero then that means that
+				// the support points of the Minkowski point B are
+				// the closest points
+				p1 = b.point1
+				p2 = b.point2
+			} else if (l2 < 0) {
+				// if lambda2 is less than zero then that means that
+				// the support points of the Minkowski point A are
+				// the closest points
+				p1 = a.point1
+				p2 = a.point2
+			} else {
+				// compute the closest points using lambda1 and lambda2
+				// this is the expanded version of
+				// p1 = a.p1.multiply(l1).add(b.p1.multiply(l2));
+				// p2 = a.p2.multiply(l1).add(b.p2.multiply(l2));
+        p1 = a.point1 * l1 + b.point1 * l2
+        p2 = a.point2 * l1 + b.point2 * l2
+			}
+		}
+
+    (p1, p2)
+  }
+
+  def segmentClosestPoint(a: Vector2D, b: Vector2D, point: Vector2D): Vector2D = {
+    if (a == b) return a
+    val direction = b - a
+    var t = ((point - a) dot (direction)) / (direction dot direction)
+    if (t < 0) t = 0
+  	if (t > 1) t = 1
+  	a + direction * t
+  }
+
+  def containsOrigin(a: Vector2D, b: Vector2D, c: Vector2D): Boolean =  {
+		val sa = a.cross(b);
+		val sb = b.cross(c);
+		val sc = c.cross(a);
+		// this is sufficient (we do not need to test sb * sc)
+		sa * sb  > 0 && sa * sc > 0
+	}
+
+  def collision(pair: (A, A)): Option[Collision[A]] = {
+    pair match {
+      case (c1: Circle, c2: Circle) => collisionCircleCircle(c1, c2)(pair)
+      case _ => gjkCollision(pair)
+    }
+  }
+
+  private def gjkCollision(pair: (A, A)): Option[Collision[A]] = {
+    val so = separation(pair._1, pair._2)
+    if (so.isEmpty) return None //deep contact is not implemented yet
+    val s = so.get
+    Some(new Collision[A] {
+      val item1 = pair._1
+      val item2 = pair._2
+      val overlap = -(s.distance - 2 * margin)
+      val points = List(s.point1)
+      val normal = s.normal.unit
+    })
+
+  }
+
+  private def collisionCircleCircle(c1: Circle, c2: Circle)(pair: (A, A)) = {
+		val d = (c2.position - c1.position)
+		val l = d.length
+		if (l <= c1.radius + c2.radius) {
+			val p = c1.position + d.unit * (l - c2.radius)
+			Some(new Collision[A] {
+				val item1 = pair._1
+				val item2 = pair._2
+				val normal = d.unit
+				val points = List(p)
+				val overlap = (c1.radius + c2.radius - l)
+			})
+		} else None
+	}
+
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/narrowphase/gjk/Manifold.scala b/src/main/scala/sims/collision/narrowphase/gjk/Manifold.scala
new file mode 100644
index 0000000..fb9a433
--- /dev/null
+++ b/src/main/scala/sims/collision/narrowphase/gjk/Manifold.scala
@@ -0,0 +1,5 @@
+package sims.collision.narrowphase.gjk
+
+import sims.math._
+
+class Manifold(val points: Seq[Vector2D], val normal: Vector2D)
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/narrowphase/gjk/MinkowskiSum.scala b/src/main/scala/sims/collision/narrowphase/gjk/MinkowskiSum.scala
new file mode 100644
index 0000000..f59905b
--- /dev/null
+++ b/src/main/scala/sims/collision/narrowphase/gjk/MinkowskiSum.scala
@@ -0,0 +1,12 @@
+package sims.collision.narrowphase
+package gjk
+
+import sims.collision._
+import sims.math._
+
+class MinkowskiSum(pair: (Collidable, Collidable)) {
+	
+	def support(direction: Vector2D) = 
+		pair._1.support(direction) - pair._2.support(-direction)
+		
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/narrowphase/gjk/Penetration.scala b/src/main/scala/sims/collision/narrowphase/gjk/Penetration.scala
new file mode 100644
index 0000000..0ecfcb3
--- /dev/null
+++ b/src/main/scala/sims/collision/narrowphase/gjk/Penetration.scala
@@ -0,0 +1,9 @@
+package sims.collision.narrowphase.gjk
+
+import sims.collision._
+import sims.math._
+
+class Penetration(
+		val normal: Vector2D,
+		val overlap: Double
+	)
\ No newline at end of file
diff --git a/src/main/scala/sims/collision/package.scala b/src/main/scala/sims/collision/package.scala
new file mode 100644
index 0000000..aa19a5d
--- /dev/null
+++ b/src/main/scala/sims/collision/package.scala
@@ -0,0 +1,18 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims
+
+import sims.collision.broadphase._
+import sims.collision.narrowphase._
+
+package object collision {
+	
+	implicit def broadphaseToConstructor[A <: Collidable: ClassManifest](b: BroadPhaseDetector[A]) =
+		new DetectorConstructor(b) 
+
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dsl/BodyPoint.scala b/src/main/scala/sims/dsl/BodyPoint.scala
new file mode 100644
index 0000000..5572b48
--- /dev/null
+++ b/src/main/scala/sims/dsl/BodyPoint.scala
@@ -0,0 +1,20 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dsl
+
+import sims.dynamics._
+import sims.math._
+
+class BodyPoint(val body: Body, val point: Vector2D) {
+	def this(body: Body) = this(body, body.position)
+	
+	def distance(bp: BodyPoint) = new DistanceJoint(body, point, bp.body, bp.point)
+	
+	def revolute(bp: BodyPoint) = new RevoluteJoint(body, bp.body, point)
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dsl/RichBody.scala b/src/main/scala/sims/dsl/RichBody.scala
new file mode 100644
index 0000000..0c7b234
--- /dev/null
+++ b/src/main/scala/sims/dsl/RichBody.scala
@@ -0,0 +1,33 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dsl
+
+import sims.dynamics._
+import sims.math._
+
+class RichBody(val body: Body) {
+	
+	def at(point: Vector2D) = new BodyPoint(body, point)
+	def atLocal(point: Vector2D) = new BodyPoint(body, body.position + point)
+	
+	def :@(point: Vector2D) = at(point)
+	def :@@(point: Vector2D) = atLocal(point)
+	def @:(point: Vector2D) = at(point)
+	def @@:(point: Vector2D) = atLocal(point)
+	
+	def @@(point: Vector2D) = at(point)
+}
+
+
+object Test {
+	val b = new Body(null)
+	
+	
+	val q = b :@@ (1.0, 2.0) revolute b
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dsl/dsl.scala b/src/main/scala/sims/dsl/dsl.scala
new file mode 100644
index 0000000..de5fa8a
--- /dev/null
+++ b/src/main/scala/sims/dsl/dsl.scala
@@ -0,0 +1,17 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims
+
+/** @todo implement DSL properly */
+package object dsl {
+	
+	implicit def body2Rich(b: sims.dynamics.Body) = new RichBody(b)
+	
+	implicit def body2BodyPoint(b: sims.dynamics.Body) = new BodyPoint(b)
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/Body.scala b/src/main/scala/sims/dynamics/Body.scala
new file mode 100644
index 0000000..5262468
--- /dev/null
+++ b/src/main/scala/sims/dynamics/Body.scala
@@ -0,0 +1,113 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics
+
+import sims.math._
+
+class Body(shapes0: Shape*) {
+	
+	val shapes: List[Shape] = shapes0.toList
+	
+	var force: Vector2D = Vector2D.Null
+	
+	var torque: Double = 0.0
+	
+	var linearVelocity: Vector2D = Vector2D.Null
+	
+	var angularVelocity: Double = 0.0 
+	
+	private var _position: Vector2D = 
+		(Vector2D.Null /: shapes)((v: Vector2D, s: Shape) => v + s.position * s.mass) / shapes.map(_.mass).sum
+	
+	def position = _position
+	
+	def position_=(pos: Vector2D) = {
+		val delta = pos - _position
+		_position = pos
+		for (s <- shapes) s.position += delta
+	}
+	
+	private var _rotation: Double = 0.0
+	
+	def rotation = _rotation
+	
+	def rotation_=(r: Double) = {
+		val delta = _rotation - r
+		_rotation = r
+		for (s <- shapes) {
+			s.rotation += delta
+			s.position = position + (s.local.get rotate r)
+		} 
+	}
+	
+	var fixed = false
+	
+	/**Returns the mass of this body. If the body is free, its mass is the sum of the masses of its shapes.
+ 	* If the body is fixed, its mass is infinite (`Double.PositiveInfinity`).
+ 	* @return this body's mass*/
+ 	lazy val mass: Double = if (!fixed) shapes.map(_.mass).sum else Double.PositiveInfinity
+ 	
+ 	/**Returns the moment of inertia for rotations about the COM of this body.
+ 	* It is calculated using the moments of inertia of this body's shapes and the parallel axis theorem.
+ 	* If the body is fixed, its moment of inertia is infinite (`Double.PositiveInfinity`).
+ 	* @return moment of inertia for rotations about the center of mass of this body*/
+ 	lazy val inertia: Double = if (!fixed) shapes.map((s: Shape) => s.inertia + s.mass * (s.local.get dot s.local.get)).sum else Double.PositiveInfinity
+ 	
+ 	/**Applies a force to the center of mass of this body.
+   * @param force applied force*/
+  def applyForce(force: Vector2D) = if (!fixed) this.force += force
+  
+  /**Applies a force to a point on this body. The point is considered to be contained within this body.
+   * @param force applied force
+   * @param point position vector of the point (in world coordinates)*/
+  def applyForce(force: Vector2D, point: Vector2D) = if (!fixed) {this.force += force; torque += (point - position) cross force}
+	
+	/**Applies a torque to the center of mass.*/
+	def applyTorque(torque: Double) = if (!fixed) this.torque += torque
+	
+	/**Applies an impulse to the center of mass of this body.
+   * @param impulse applied impulse*/  
+  def applyImpulse(impulse: Vector2D) = if (!fixed) linearVelocity += impulse / mass
+  
+  /**Applies an impulse to a point on this body. The point is considered to be contained within this body.
+   * @param impulse applied impulse
+   * @param point position vector of the point (in world coordinates)*/
+  def applyImpulse(impulse: Vector2D, point: Vector2D) = if (!fixed) {linearVelocity += impulse / mass; angularVelocity += ((point - position) cross impulse) / inertia}
+  
+  /**Applies an angular impulse to the center of mass.*/
+  def applyAngularImpulse(impulse: Double) = if (!fixed) angularVelocity += impulse / inertia
+  
+  /**Linear velocity of the given point on this body (in world coordinates).*/
+  def velocityOfPoint(point: Vector2D) = linearVelocity + (angularVelocity cross (point - position))
+  
+  /**Linear momentum.*/
+  def linearMomentum = linearVelocity * mass  
+  
+ 	for (s0 <- shapes0) {
+ 		s0.local = Some(s0.position - _position)
+ 		s0.body = this
+ 	}
+  
+  def contains(point: Vector2D) = shapes.exists(_.contains(point))
+  
+  def info = {
+  	"Body@" + hashCode + "(" + this.getClass() + ")\n" +  
+  			"\tPosition: " + position + "\n" + 
+  			"\tRotation: " + rotation + "\n" + 
+  			"\tLinear velocity: " + linearVelocity + "\n" + 
+  			"\tAngular velocity: " + angularVelocity + "\n" +
+  			"\tForce: " + force + "\n" +
+  			"\tTorque: " + torque + "\n" +
+  			"\tMass: " + mass + "\n" +
+  			"\tInertia: " + inertia + "\n" +
+  			"\tFixed: " + fixed + "\n" +
+  			"\tShape count" + shapes.length
+  			
+  }
+ 	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/Circle.scala b/src/main/scala/sims/dynamics/Circle.scala
new file mode 100644
index 0000000..9e00815
--- /dev/null
+++ b/src/main/scala/sims/dynamics/Circle.scala
@@ -0,0 +1,24 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics
+
+import scala.math.Pi
+import sims.collision.{AABB, Projection}
+import sims.math._
+
+/** A circle.
+  * @define shape circle
+  * 
+  * @param radius radius of this circle */
+case class Circle(radius: Double) extends sims.collision.Circle with Shape {
+	
+	val area: Double = Pi * radius * radius
+	
+	lazy val inertia: Double = mass * radius * radius / 2
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/Collision.scala b/src/main/scala/sims/dynamics/Collision.scala
new file mode 100644
index 0000000..e47c346
--- /dev/null
+++ b/src/main/scala/sims/dynamics/Collision.scala
@@ -0,0 +1,58 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics
+
+import sims.dynamics._
+import sims.dynamics.constraints._
+
+import sims.collision.{Collision => CCollision}
+
+/** A class representing a physical collision,
+  * implementing constraints to handle collision response. */
+class Collision(collision: CCollision[Shape]) extends Constraining {
+	
+	
+	
+	private def getNonPenetrationConstraints() = for (point <- collision.points) yield 
+		new Constraint {
+			val body1 = collision.item1.body
+			val body2 = collision.item2.body
+			def v = body2.velocityOfPoint(point) - body1.velocityOfPoint(point) 
+			val e = {
+				if ((v dot collision.normal.unit) > 0) 0.0
+				else if ((v dot collision.normal.unit) > -1) 0.0
+				else math.min(collision.item1.restitution, collision.item2.restitution)
+			}
+			def jacobian = new Jacobian(-collision.normal, -((point - body1.position) cross collision.normal),
+																	collision.normal, ((point - body2.position) cross collision.normal))
+			
+			override def bias = (v dot collision.normal.unit) * e
+			def value = -collision.overlap
+			override def inequality = true
+			override val limit = Some((0.0, Double.PositiveInfinity))
+			
+			val slop = 0.005
+			override def error =
+				if (collision.overlap > slop)
+					-(collision.overlap - slop)
+				else 0.0
+	}
+	
+	val constraints = getNonPenetrationConstraints() 
+	
+}
+
+object Collision {
+	
+	/**Converts a collision to a physical collision
+	 * (sims.collision.Collision to a sims.dynamics.Collision)*/
+	implicit def collision2Physical(c: sims.collision.Collision[Shape]) = new Collision(c)
+	
+	implicit def collision2Constructor(c: sims.collision.Collision[Shape]) = new { def toPhysical = new Collision(c) }
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/ContactPoint.scala b/src/main/scala/sims/dynamics/ContactPoint.scala
new file mode 100644
index 0000000..31eb1b0
--- /dev/null
+++ b/src/main/scala/sims/dynamics/ContactPoint.scala
@@ -0,0 +1,64 @@
+package sims.dynamics
+
+import sims.collision.{Collision => CCollision}
+import sims.dynamics.constraints._
+import sims.math._
+
+class RestingPoint(collision: CCollision[Shape], point: Vector2D) extends Constraint {
+	import collision._
+	val body1 = item1.body
+	val body2 = item2.body
+	
+	def relativeVelocity = body2.velocityOfPoint(point) - body1.velocityOfPoint(point)
+	
+	override def inequality = true
+	
+	override val limit = Some((0.0, Double.PositiveInfinity))
+	
+	override def value = -overlap
+	
+	override def jacobian = 
+		new Jacobian(-normal.unit, -((point - body1.position) cross normal.unit),
+								 normal.unit,  ((point - body2.position) cross normal.unit))
+			
+	val slop = 0.005
+	override def error =
+		if (collision.overlap > slop)
+			-(collision.overlap - slop)
+		else 0.0
+}
+
+class ImpactPoint(collision: CCollision[Shape], point: Vector2D) extends Constraint {
+	import collision._
+	val body1 = item1.body
+	val body2 = item2.body
+	
+	def relativeVelocity = body2.velocityOfPoint(point) - body1.velocityOfPoint(point)
+	
+	override def inequality = true
+	
+	override val limit = Some((0.0, Double.PositiveInfinity))
+	
+	override def value = -overlap
+	
+	override def jacobian = 
+		new Jacobian(-normal.unit, -((point - body1.position) cross normal.unit),
+								 normal.unit,  ((point - body2.position) cross normal.unit))
+	
+	val restitution = math.min(collision.item1.restitution, collision.item2.restitution)
+	override def bias = (relativeVelocity dot collision.normal.unit) * restitution
+			
+	override def error = 0
+}
+
+object ContactResolver {
+	
+	def relativeVelocity(collision: CCollision[Shape], point: Vector2D) = 
+		collision.item2.body.velocityOfPoint(point) - collision.item2.body.velocityOfPoint(point)
+	
+	def resolve(collision: CCollision[Shape]): Seq[Constraint] = for (p <- collision.points) yield {
+		val v = (relativeVelocity(collision, p) dot collision.normal.unit)
+		if (v < -1) new RestingPoint(collision, p)
+		else new ImpactPoint(collision, p)
+	}
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/DistanceJoint.scala b/src/main/scala/sims/dynamics/DistanceJoint.scala
new file mode 100644
index 0000000..2e24e30
--- /dev/null
+++ b/src/main/scala/sims/dynamics/DistanceJoint.scala
@@ -0,0 +1,54 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics
+
+import sims.dynamics.constraints._
+import sims.math._
+
+/** A distance joint, used to keep two bodies at a fixed distance.
+ 	* The distance is given by the initial distance of the anchor points.
+ 	* 
+ 	* This joint removes one degree of freedom between two bodies,
+ 	* hence it contains one [[sims.dynamics.constraints.Constraint]]. 
+ 	* 
+ 	* @param body1 first body connected by this joint
+ 	* @param anchor1 anchor point on first body (in world coordinates)
+ 	* @param body2 second body connected by this joint 
+ 	* @param anchor2 anchor point on second body (in world coordinates) */
+class DistanceJoint(val body1: Body, val anchor1: Vector2D, val body2: Body, val anchor2: Vector2D) extends Joint {
+	def this(body1: Body, body2: Body) = this(body1, Vector2D.Null, body2, Vector2D.Null)
+	
+	private val self = this
+	
+	val local1 = anchor1 - body1.position
+	val local2 = anchor2 - body2.position
+	private val l = (anchor2 - anchor1).length //(body2.position + local2 - body1.position - local1).length
+	private val rotation01 = body1.rotation
+	private val rotation02 = body2.rotation
+	
+	def r1 = (local1 rotate (body1.rotation - rotation01))
+	def r2 = (local2 rotate (body2.rotation - rotation02))
+	
+	def x1 = body1.position + r1
+	def x2 = body2.position + r2
+	
+	private def v1 = body1 velocityOfPoint x1
+	private def v2 = body2 velocityOfPoint x2
+	
+	private def x = x2 - x1
+	private def v = v2 - v1
+	
+	val constraints = List(new Constraint{
+		val body1 = self.body1
+		val body2 = self.body2
+		def value = x.length - l
+		def jacobian = new Jacobian(-x.unit, -(r1 cross x.unit), x.unit, (r2 cross x.unit))
+	})
+	
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/Joint.scala b/src/main/scala/sims/dynamics/Joint.scala
new file mode 100644
index 0000000..a3eb847
--- /dev/null
+++ b/src/main/scala/sims/dynamics/Joint.scala
@@ -0,0 +1,21 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics
+
+import sims.dynamics.constraints.Constraining
+
+/** A base trait for all joints between two bodies. */
+trait Joint extends Constraining {
+	
+	/** First body connected by this joint. */
+	def body1: Body
+	
+	/** Second body connected by this joint. */
+	def body2: Body
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/Rectangle.scala b/src/main/scala/sims/dynamics/Rectangle.scala
new file mode 100644
index 0000000..90b8d9d
--- /dev/null
+++ b/src/main/scala/sims/dynamics/Rectangle.scala
@@ -0,0 +1,38 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics
+
+import sims.math._
+import sims.collision.ConvexPolygon
+
+/**A rectangle is a polygon.
+  * @define shape rectangle
+  * 
+  * @param halfWidth this rectangle's half width
+  * @param halfHeight this rectangle's half height
+  * @param density density of this rectangle */
+case class Rectangle(halfWidth: Double, halfHeight : Double) extends ConvexPolygon with Shape {
+  
+  val area = halfWidth * halfHeight * 4
+  
+  val inertia = 1.0 / 12.0  * mass * ((2 * halfWidth) * (2 * halfWidth) + (2 * halfHeight) * (2 * halfHeight))
+  
+  /**Returns the vectors from the center to the vertices of this rectangle.
+   * The first vertex is the upper-right vertex at a rotation of 0.
+   * Vertices are ordered counter-clockwise.*/
+  def halfDiags: Array[Vector2D] = Array(Vector2D(halfWidth, halfHeight),
+                                         Vector2D(-halfWidth, halfHeight),
+                                         Vector2D(-halfWidth, -halfHeight),
+                                         Vector2D(halfWidth, -halfHeight)) map (_ rotate rotation)
+  
+  /**Returns the position vectors of this rectangle's vertices.
+   * The first vertex is the upper-right vertex at a rotation of 0.
+   * Vertices are ordered counter-clockwise.*/
+  def vertices = for (h <- halfDiags) yield position + h
+  
+} 
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/RegularPolygon.scala b/src/main/scala/sims/dynamics/RegularPolygon.scala
new file mode 100644
index 0000000..7f62ea8
--- /dev/null
+++ b/src/main/scala/sims/dynamics/RegularPolygon.scala
@@ -0,0 +1,39 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics
+
+import scala.math._
+import sims.collision.ConvexPolygon
+import sims.math._
+
+/**A regular polygon with <code>n</code> sides whose excircle has a radius <code>radius</code>.
+  * @define shape regular polygon
+  * 
+  * @param n number of sides.
+  * @param radius radius of the excircle
+  * @param density density of this regular polygon
+  * @throws IllegalArgumentException if <code>n</code> is smaller than 3 */
+case class RegularPolygon(n: Int, radius: Double) extends ConvexPolygon with Shape {
+  require(n >= 3, "A polygon must have at least 3 sides.")
+  
+  /**Height of one of the constituting triangles.*/
+  private val h: Double = radius * cos(Pi / n)
+  /**Half width of one of the constituting triangles.*/
+  private val b: Double = radius * sin(Pi / n)
+  
+  def halfDiags = (for (i <- (0 until n).toArray) yield (Vector2D(0, radius) rotate (2 * Pi * i / n))) map (_ rotate rotation)
+  
+  def vertices = for (h <- halfDiags) yield position + h
+  
+  val area = n * h * b / 2
+  
+  /**Moment of inertia of one of the constituting triangles about the center of this polygon.*/
+  private val Ic: Double = density * b * (3 * b + 16) * pow(h, 4) / 54
+  
+  val inertia = n * Ic
+}
diff --git a/src/main/scala/sims/dynamics/RevoluteJoint.scala b/src/main/scala/sims/dynamics/RevoluteJoint.scala
new file mode 100644
index 0000000..5d7a85f
--- /dev/null
+++ b/src/main/scala/sims/dynamics/RevoluteJoint.scala
@@ -0,0 +1,55 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics
+
+import sims.dynamics.constraints._
+import sims.math._
+
+/** A revolute joint, used to keep two bodies fixed relatively at one point.
+ 	* 
+ 	* This joint removes two degrees of freedom between two bodies,
+ 	* hence it contains two [[sims.dynamics.constraints.Constraint]]s. 
+ 	* 
+ 	* @param body1 first body connected by this joint
+ 	* @param body2 second body connected by this joint 
+ 	* @param anchor anchor point where bodies are fixed relatively */
+class RevoluteJoint(val body1: Body, val body2: Body, val anchor: Vector2D) extends Joint {
+	def this(body1: Body, body2: Body) = this(body1, body2, body1.position)
+	
+	private val self = this
+	
+	private val local1 = anchor - body1.position
+	private val local2 = anchor - body2.position
+	
+	private val rotation01 = body1.rotation
+	private val rotation02 = body2.rotation
+	
+	def r1 = (local1 rotate (body1.rotation - rotation01))
+	def r2 = (local2 rotate (body2.rotation - rotation02))
+	
+	def x1 = body1.position + r1
+	def x2 = body2.position + r2
+	
+	def x = x2 - x1
+	
+	val constraints = List(
+		new Constraint {
+			val body1 = self.body1
+			val body2 = self.body2
+			def value = x.x
+			def jacobian = new Jacobian(-Vector2D(1, 0), r1.y, Vector2D(1, 0), -r2.y)
+		},
+		new Constraint {
+			val body1 = self.body1
+			val body2 = self.body2
+			def value = x.y
+			def jacobian = new Jacobian(-Vector2D(0, 1), -r1.x, Vector2D(0, 1), r2.x)
+		}
+	)
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/Shape.scala b/src/main/scala/sims/dynamics/Shape.scala
new file mode 100644
index 0000000..d1aa1ae
--- /dev/null
+++ b/src/main/scala/sims/dynamics/Shape.scala
@@ -0,0 +1,67 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics
+
+import sims.collision._
+import sims.math._
+
+
+/** 
+	* @define shape shape */
+trait Shape extends AnyRef with Collidable with CachedCollidable {
+	
+	override def equals(that: Any) = super.equals(that)
+	override def hashCode = super.hashCode
+	
+	/** (temporary solution) Local position of $shape to body.
+	  * @deprecated find solution avoiding NullPointers
+	  * @todo find solution avoiding NullPointers */
+	var local: Option[Vector2D] = None
+	
+	var body: sims.dynamics.Body = null
+	
+	override def fixed = body.fixed
+	
+	var restitution = 1.0
+	
+	/**Position of this $shape's center of mass in world coordinates.*/
+	private var _position: Vector2D = Vector2D.Null
+	def position = _position
+	def position_=(pos: Vector2D) = {
+		_position = pos
+		move()
+	}
+	
+  /**Rotation of this shape about its center of mass.*/
+	var rotation: Double = 0.0
+	
+	/** Mass per area. */
+  def density: Double = 1.0
+  
+  /** Area of this $shape. */
+  def area: Double
+  
+  /** Mass of this $shape. The mass is given by volume times density. */
+  def mass: Double = area * density
+ 
+  /**Moment of inertia for a rotation about this $shape's center of mass.*/
+  def inertia: Double
+  
+  /**Returns this $shape's axis aligned bounding box.*/
+  def aabb: sims.collision.AABB
+  
+  /**Returns the projection of this $shape onto the line given by the directional vector <code>axis</code>.
+   * @param axis directional vector of the line
+   * @return projection of this shape*/
+  def project(axis: Vector2D): Projection
+  
+  /**Checks if the point <code>point</code> is contained in this $shape.*/
+  def contains(point: Vector2D): Boolean
+ 
+  def asBody = new sims.dynamics.Body(this)
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/World.scala b/src/main/scala/sims/dynamics/World.scala
new file mode 100644
index 0000000..e02f38a
--- /dev/null
+++ b/src/main/scala/sims/dynamics/World.scala
@@ -0,0 +1,101 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics
+
+import scala.collection.mutable.ArrayBuffer
+import sims.collision._
+import sims.collision.broadphase.SAP
+import sims.collision.narrowphase.SAT
+import sims.math.Vector2D
+
+class World {
+	
+	val detector = SAP[sims.dynamics.Shape] narrowedBy
+                  new sims.collision.narrowphase.gjk.GJK2[sims.dynamics.Shape]
+								 //SAT[sims.dynamics.Shape]
+	
+	var collisionDetection = true
+	
+	private val _bodies = new ArrayBuffer[Body]
+	def bodies: Seq[Body] = _bodies
+	
+	def +=(b: Body): Unit = {
+		_bodies += b
+		detector ++= b.shapes
+	}
+	def -=(b: Body): Unit = {
+		_bodies -= b
+		detector --= b.shapes
+	}
+	
+	def shapes: Seq[sims.dynamics.Shape] = for (b <- bodies; s <- b.shapes) yield s
+	
+	private val _joints = new ArrayBuffer[Joint]
+	def joints: Seq[Joint] = _joints
+	
+	def +=(j: Joint): Unit = _joints += j
+	def -=(j: Joint): Unit = _joints -= j
+	
+	def clear() = {
+		detector.clear()
+		for (b <- bodies) _bodies -= b
+		for (j <- joints) _joints -= j
+	}
+	
+	
+	var h = 1.0 / 60
+	
+	var iterations = 10
+	
+	var errorReduction = 0.2
+	
+	var gravity = sims.math.Vector2D(0, -9.8)
+	
+	def preStep() = {}
+	
+	def step() = {
+		
+		preStep()
+		
+		for (b <- _bodies) {
+			b applyForce gravity * b.mass
+			b.linearVelocity += (b.force / b.mass) * h
+			b.angularVelocity += (b.torque / b.inertia) * h
+		}
+		
+		for (j <- joints) {
+			j.preSolve()
+		}
+		for (i <- 0 until iterations)
+			for (j <- joints) j.correctVelocity(h, errorReduction)
+		
+		if (collisionDetection) {
+			import Collision._
+			val physicalCollisions: Seq[Collision] = detector.collisions.map(_.toPhysical)
+			for (c <- physicalCollisions) c.preSolve()
+			for (i <- 0 until iterations)
+				for (c <- physicalCollisions) c.correctVelocity(h, errorReduction)
+			
+		}
+		
+		
+		for (b <- _bodies) {
+			b.position += b.linearVelocity * h
+			b.rotation += b.angularVelocity * h
+			b.force = Vector2D.Null
+		}
+		
+		postStep()
+	}
+	
+	def postStep(): Unit = {
+		detector.invalidate()
+	}
+	
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/constraints/Constraining.scala b/src/main/scala/sims/dynamics/constraints/Constraining.scala
new file mode 100644
index 0000000..f1834a2
--- /dev/null
+++ b/src/main/scala/sims/dynamics/constraints/Constraining.scala
@@ -0,0 +1,28 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics.constraints
+
+import sims.dynamics._
+
+/**A base trait implemented by objects representing constraints (such as joints or collisions).
+ * @see sims.dynamics.constraints.Constraint
+ */
+trait Constraining {
+	
+	/**All constraints represented by this constraining object.*/
+	def constraints: Seq[Constraint]
+	
+	/**Invoke `preSolve()` on each constraint.
+	 * @see sims.dynamics.constraints.Constraint*/
+	def preSolve() = for (c <- constraints) c.preSolve() 
+		
+	/**Solves all constraints of this constraining object.
+	 * @see sims.dynamics.constraints.Constraint*/
+	def correctVelocity(h: Double, erp: Double): Unit = for (c <- constraints) c.correctVelocity(h, erp)
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/constraints/Constraint.scala b/src/main/scala/sims/dynamics/constraints/Constraint.scala
new file mode 100644
index 0000000..367c84d
--- /dev/null
+++ b/src/main/scala/sims/dynamics/constraints/Constraint.scala
@@ -0,0 +1,126 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics.constraints
+
+import sims.dynamics._
+import sims.math._
+
+/** A base trait for implementing and solving constraints.
+  * Constraints are solved using the 'sequential impulse' method presented by Erin Catto
+  * at Game Developpers Conference 2008 (see http://www.box2d.org/documentation.html).
+  * 
+  * One instance of this trait represents one constraint for one degree of freedom between
+  * two bodies.
+  * @see sims.dynamics.constraints.Constrained */
+trait Constraint {
+	
+	/** First constrained body. */
+	def body1: Body
+	
+	/** Second constrained body. */
+	def body2: Body
+	
+	/** Value of the position constraint function ('C' in the presentation). */
+	def value: Double
+	
+	/** Jacobian for the velocity constraint ('J' in 'Ċ=Jv+b' in the presentation). */
+	def jacobian: Jacobian
+	
+	/** Velocity bias ('b' in 'Ċ=Jv+b' in the presentation).*/
+	def bias: Double = 0
+	
+	/** Lower and upper limits of the total corrective impulse allowed
+	  * when solving this constraint.
+	  * The first element represents the lower limit,
+	  * the second the upper limit.
+	  * None represents no limits. */
+	def limit: Option[(Double, Double)] = None
+	
+	/** Defines whether or not this constraint should be treated as an inequality constraint.
+	  * An inequality constraint will only be solved if the constraint function is less than
+	  * zero (i.e. `value < 0`). */
+	def inequality = false
+	
+	/** Position error used for Baumgarte sabilization. 
+	  * Corresponds to `value` by default. */
+	def error = value
+	
+	/** Accumulated impulse. */
+	private var accumulated = 0.0
+	
+	/** Constraint mass cache, constant for all iterations. */
+	private var m = 0.0
+	
+	/** Jacobian cache, constant for all iterations. */
+	private var J: Jacobian = null
+	
+	/** Velocity bias cache, constant for all iterations. */
+	private var b = 0.0
+	
+	/** Method that should be called before solving this constraint.
+	  * Computes constraint masses and other invariable data for iterating over constraints. */
+	def preSolve() = {
+		//compute jacobian and bias for next solving
+		J = jacobian
+		b = bias
+		
+		//invMass=(J*invert(M)*transpose(J))
+		val invMass: Double = (
+				(J.v1.x * J.v1.x + J.v1.y * J.v1.y) / body1.mass + J.w1 * J.w1 / body1.inertia +
+				(J.v2.x * J.v2.x + J.v2.y * J.v2.y) / body2.mass + J.w2 * J.w2 / body2.inertia
+			)
+		
+		//if invMass == 0, both bodies have infinite mass (i.e. are fixed)
+		m = if (invMass == 0.0) 0.0 else 1.0 / invMass
+		
+		//apply accumulated impulse from prevoius step
+		body1.applyImpulse(J.v1 * accumulated)
+		body1.applyAngularImpulse(J.w1 * accumulated)
+		body2.applyImpulse(J.v2 * accumulated)
+		body2.applyAngularImpulse(J.w2 * accumulated)
+		
+		//initialize accumulated impulse
+		//accumulated = 0.0
+	}
+	
+	/** Solves this constraint by applying corrective impulses to its constrained bodies.
+	  * @todo implement error correction properly for collisiions (add slop tolerance, etc)
+	  * @param h time interval over which the correction should be applied
+	  * @param erp error reduction parameter. */
+	def correctVelocity(h: Double, erp: Double): Unit = {
+		
+		//if m == 0, both bodies are fixed and there is no point in applying corrective impulse
+		if (m == 0) return ()
+		
+		//C > 0 => ignore
+		if (inequality && value > 0.0) return ()
+		
+		//lambda
+		var lambda = -m * (
+				J.v1.x * body1.linearVelocity.x + J.v1.y * body1.linearVelocity.y + J.w1 * body1.angularVelocity +
+				J.v2.x * body2.linearVelocity.x + J.v2.y * body2.linearVelocity.y + J.w2 * body2.angularVelocity +
+				b + (erp / h * error)
+			)
+			
+		
+		//clamp accumulated impulse
+		if (limit != None) {
+			val temp = accumulated
+			accumulated = math.max(limit.get._1, math.min(accumulated + lambda, limit.get._2))
+			lambda = accumulated - temp
+		}
+		
+		
+		//apply accumulated impulse
+		body1.applyImpulse(J.v1 * lambda)
+		body1.applyAngularImpulse(J.w1 * lambda)
+		body2.applyImpulse(J.v2 * lambda)
+		body2.applyAngularImpulse(J.w2 * lambda)	
+	}
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/dynamics/constraints/Jacobian.scala b/src/main/scala/sims/dynamics/constraints/Jacobian.scala
new file mode 100644
index 0000000..3b16bde
--- /dev/null
+++ b/src/main/scala/sims/dynamics/constraints/Jacobian.scala
@@ -0,0 +1,26 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.dynamics.constraints
+
+import sims.math.Vector2D
+
+/** A Jacobian matrix used for solving constraints.
+  * 
+  * In SiMS 2 constraints are considered to remove one degree of freedom for two bodies.
+  * Let '''v''',,1,,, w,,1,,, '''v''',,2,,, w,,2,, be the linear and angular velocities of both bodies
+  * respectively.
+  * Let v = ['''v''',,1,,, w,,1,,, '''v''',,2,,, w,,2,,].
+  * The velocity constraint function is then given by 'Ċ='''Jv'''+b' and 
+  * the Jacobian ('''J''') is given by '''J''' = ['''Jv''',,1,,, Jw,,1,,, '''Jv''',,2,,, Jw,,2,,] = [Jv,,1,x,,, Jv,,1,y,,, Jw,,1,,, Jv,,2,x,,, Jv,,2,y,,, Jw,,2,,]
+  *  
+  * @param v1 corresponds to '''Jv''',,1,, in the description above
+  * @param w1 corresponds to Jw,,1,, in the description above
+  * @param v2 corresponds to '''Jv''',,2,, in the description above
+  * @param w2 corresponds to Jw,,2,, in the description above
+  * @see sims.dynamics.constraints.Constraint */
+class Jacobian(val v1: Vector2D, val w1: Double, val v2: Vector2D, val w2: Double)
\ No newline at end of file
diff --git a/src/main/scala/sims/math/PseudoVector3D.scala b/src/main/scala/sims/math/PseudoVector3D.scala
new file mode 100644
index 0000000..ea51c10
--- /dev/null
+++ b/src/main/scala/sims/math/PseudoVector3D.scala
@@ -0,0 +1,16 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.math
+
+/**An x3-axis aligned vector. Since SIMS is in 2D, 3D vectors are only used as a convenience to simulate operations that only exist
+ * in three dimensions such as the cross product.*/
+case class PseudoVector3D(x3: Double) {
+	
+	def cross(v: Vector2D): Vector2D = v.leftNormal * x3
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/math/math.scala b/src/main/scala/sims/math/math.scala
new file mode 100644
index 0000000..36f7a8b
--- /dev/null
+++ b/src/main/scala/sims/math/math.scala
@@ -0,0 +1,23 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims
+
+package object math {
+	
+	implicit def double2PseudoVector3D(x3: Double) = new PseudoVector3D(x3)
+	
+	implicit def tuple2Vector[A: Numeric, B: Numeric](t: (A, B)): Vector2D = {
+		val x = implicitly[Numeric[A]].toDouble(t._1)
+		val y = implicitly[Numeric[B]].toDouble(t._2)
+		new Vector2D(x, y)
+	}
+	
+	def mod(x: Int, y: Int): Int = {val r = x % y; if (r < 0) r + y else r}
+	
+	
+}
\ No newline at end of file
diff --git a/src/main/scala/sims/math/vector.scala b/src/main/scala/sims/math/vector.scala
new file mode 100644
index 0000000..8e7c716
--- /dev/null
+++ b/src/main/scala/sims/math/vector.scala
@@ -0,0 +1,127 @@
+/*    _____ _ __  ________    ___                                      *\
+**   / ___/(_)  |/  / ___/   |__ \  Simple Mechanics Simulator 2       **
+**   \__ \/ / /|_/ /\__ \    __/ /  copyright (c) 2011 Jakob Odersky   **
+**  ___/ / / /  / /___/ /   / __/                                      **
+** /____/_/_/  /_//____/   /____/                                      **
+\*                                                                     */
+
+package sims.math
+
+import scala.math._
+
+/** A 2D vector.
+  * @param x 1st component
+  * @param y 2nd component */
+case class Vector2D(x: Double, y: Double) {
+	
+	/** Components of this vector. */
+	lazy val components = List(x, y)
+    
+  /** Vector addition. */
+  def +(v: Vector2D): Vector2D = new Vector2D(x + v.x, y + v.y)
+  
+  /** Scalar multiplication. */
+  def *(n: Double): Vector2D = new Vector2D(x * n, y * n)
+  
+	/** Inverse of this vector. */
+  lazy val unary_- = this * (-1)
+  
+  /** Add inverse of another vector. */
+  def -(v: Vector2D) = this + -v
+  
+  /** Multiply by inverse of scalar. */
+  def /(n: Double) = this * (1 / n)
+  
+  /** Dot product. */
+  def dot(v: Vector2D): Double = x * v.x + y * v.y
+  
+  /** Cross product. Length only because in 2D. The direction would be given by the x3-axis. */
+  def cross(v: Vector2D): Double = x * v.y - y * v.x
+  
+  /** Cross product with an imaginary vector parallel to the x3-axis.
+    * Its magnitude is given by |p| and its direction sign(p). */
+  def cross(p: PseudoVector3D): Vector2D = rightNormal * p.x3
+  
+  /** Magnitude of this vector. */
+  lazy val length: Double = math.sqrt(lengthSquare)
+
+  lazy val lengthSquare: Double = x * x + y * y
+  
+  /** Unit vector. */
+  lazy val unit: UnitVector2D =
+  	if (!(x == 0.0 && y == 0.0)) new UnitVector2D(x / length, y / length) 
+    else throw new UnsupportedOperationException("Null vector does not have a unit vector.")
+  
+  /** Returns the projection of this vector onto the vector `v`. */
+  def project(v: Vector2D): Vector2D = {
+    if (v != Vector2D.Null)
+      v * ((this dot v) / (v.lengthSquare))
+    else
+      Vector2D.Null
+   }
+	
+	/** Returns the projection of this vector onto the unit vector `v`. */
+  def project(v: UnitVector2D): Vector2D = {
+    if (v != Vector2D.Null)
+      v * (this dot v)
+    else
+      Vector2D.Null
+   }
+  
+  /** Returns this vector rotated by `angle` radians. */
+  def rotate(angle: Double): Vector2D = {
+    Vector2D(cos(angle) * x - sin(angle) * y,
+             cos(angle) * y + sin(angle) * x)
+  }
+  
+  /** Left normal vector. (-y, x) */
+  lazy val leftNormal: Vector2D = Vector2D(-y, x)
+  
+  /** Right normal vector. (y, -x) */
+  lazy val rightNormal: Vector2D = Vector2D(y, -x)
+  
+  /** Checks if this vector is the null vector. */
+  def isNull: Boolean = this == Vector2D.Null
+  
+  /** Colinearity check. */
+  def ~(v: Vector2D): Boolean = x * v.y - v.x * y == 0
+  
+  
+  def leftOf(v: Vector2D): Boolean = (this dot v.leftNormal) > 0
+  def rightOf(v: Vector2D): Boolean = (this dot v.rightNormal) > 0
+  def directionOf(v: Vector2D): Boolean = (this dot v) > 0
+  
+}
+	
+object Vector2D {
+	
+	/**Null vector.*/
+  val Null = Vector2D(0,0)
+  
+  /**Horizontal unit vector. (1,0)*/
+  val i = new UnitVector2D(1,0)
+  
+  /**Vertical unit vector. (0,1)*/
+  val j = new UnitVector2D(0,1)
+	
+}
+
+/** A two dimensional vector considered having a magnitude of one.
+  * Unit vectors are used internally for increasing performance
+  * (i.e. the length of a unit vector is always one, the unit vector of
+  * a unit vector is always itself). */
+class UnitVector2D(x: Double, y: Double) extends Vector2D(x, y) {
+	
+	def isValid = x * x + y * y == 1
+	
+	override lazy val unit = this
+	
+	override lazy val length = 1.0
+
+  override lazy val lengthSquare = 1.0
+	
+  override lazy val leftNormal = new UnitVector2D(-y, x)
+  
+  override lazy val rightNormal = new UnitVector2D(y, -x)
+	
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/ClippingTest.scala b/src/test/scala/sims/test/ClippingTest.scala
new file mode 100644
index 0000000..317abfe
--- /dev/null
+++ b/src/test/scala/sims/test/ClippingTest.scala
@@ -0,0 +1,58 @@
+package sims.test
+
+import org.scalatest.FunSuite
+import org.scalatest.matchers.ShouldMatchers
+
+import sims.math._
+import sims.collision._
+
+class ClippingTest extends FunSuite with ShouldMatchers {
+	
+	/*
+	test("clipping segment to segment, outside clipping zone") {
+		val A = new Segment((1, 1), (3, 0))
+		val B = new Segment((0,0), (0, 1))
+		val C = new Segment((4,-1), (10, 5))
+		
+		(B clipped A) should equal (None)
+		(C clipped A) should equal (None)
+	}
+	
+	test("clipping segment to segment, inside clipping zone") {
+		val segments = List(
+				new Segment((2, 3), (4.5, 8)),
+				new Segment((3, -4), (2, -5)),
+				new Segment((0, 0), (10, 0)),
+				new Segment((1.1, -1234), (9.999, 0.001)),
+				new Segment((5, 5), (5, -5))
+			)
+			
+		val clippers = List(
+				new Segment((0, 0), (10, 0)),
+				new Segment((10, 0), (0, 0)),
+				new Segment((120, -0.01), (-130, -0.01)),
+				new Segment((-2.3, 10000), (14, 10000))
+			)
+			
+		def translate(s: Segment, v: Vector2D) = {
+			new Segment(s.point1 + v, s.point2 + v)
+		}
+		
+		def rotate(s: Segment, r: Double) = {
+			new Segment(s.point1 rotate r, s.point2 rotate r)
+		}
+		
+		for (s <- segments; c <- clippers) {(s clipped c).get should equal (s)}
+		
+		for (i <- 0 until 1000) for(s <- segments; c <- clippers) {
+			val r = new scala.util.Random
+			def next = r.nextDouble * 1000 * (if (r.nextBoolean) -1 else 1)
+			val x = (next, next)
+			val y = next
+			(translate(rotate(s, y), x) clipped translate(rotate(c, y), x)).get should equal (translate(rotate(s, y), x))
+		}
+		
+	}
+	*/
+
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/LinearOverlapTest.scala b/src/test/scala/sims/test/LinearOverlapTest.scala
new file mode 100644
index 0000000..aa99f6f
--- /dev/null
+++ b/src/test/scala/sims/test/LinearOverlapTest.scala
@@ -0,0 +1,92 @@
+package sims.test
+
+import org.scalatest.FlatSpec
+import org.scalatest.matchers.ShouldMatchers
+
+import sims.math._
+import sims.collision._
+
+class LinearOverlapTest extends FlatSpec with ShouldMatchers {
+	
+	"A segment" should "throw IllegalArgumentException if both of its " +
+	"vertices degenerate into a single one" in {
+		evaluating { val s1 = Segment(Vector2D(0,0), Vector2D(0,0)) } should produce [IllegalArgumentException]
+	}
+	
+	it should "not intersect with itself" in {
+		val s1 = Segment(Vector2D(2, 2), Vector2D(3, 5))
+		s1 intersection s1 should equal (None)
+	}
+	
+	"Two segments" should "have an intersection point if they intersect" in {
+		val s1 = Segment(Vector2D(0, 0), Vector2D(3, 1))
+		val s2 = Segment(Vector2D(0, 1), Vector2D(3, -2))
+		s1 intersection s2 should not equal (None)
+	}
+	
+	it should "have an intersection point if they share a vertice" in {
+		val s1 = Segment(Vector2D(1, 2), Vector2D(3, 1))
+		val s2 = Segment(Vector2D(3, 1), Vector2D(3, -2))
+		s1 intersection s2 should not equal (None)
+	}
+	
+	it should "have an intersection point if one contains one of the other's vertices" in {
+		val s1 = Segment(Vector2D(2, 4), Vector2D(3, 100))
+		val s2 = Segment(Vector2D(1, 3), Vector2D(3, 5))
+		s1 intersection s2 should not equal (None)
+	}
+	
+	it should "not have an intersection point if they are parallel" in {
+		val s1 = Segment(Vector2D(0, 0), Vector2D(3, 1))
+		val s2 = Segment(Vector2D(0, 1), Vector2D(3, 2))
+		s1 intersection s2 should equal (None)
+	}
+		
+	it should "not have an intersection point if they are parallel and lie on each other" in {
+		val s1 = Segment(Vector2D(2, 2), Vector2D(6, 6))
+		val s2 = Segment(Vector2D(3, 3), Vector2D(4, 4))
+		s1 intersection s2 should equal (None)
+	}
+	
+	
+	"A ray and a segment" should "have an intersection point if they intersect" in {
+		val r1 = Ray(Vector2D(3, 5), Vector2D(3, -1))
+		val s1 = Segment(Vector2D(6.32, math.sqrt(4.0)), Vector2D(10, 15.5))
+		r1 intersection s1 should not equal (None)
+	}
+	
+	it should "have an intersection point if they share a vertice" in {
+		val r1 = Ray(Vector2D(3, 4), Vector2D(2, 1))
+		val s1 = Segment(Vector2D(0, 10), Vector2D(3, 4))
+		r1 intersection s1 should not equal (None)
+	}
+	
+	it should "have an intersection point if the ray contains one of the segment's vertices" in {
+		val r1 = Ray(Vector2D(0, 0), Vector2D(1, 2))
+		val s1 = Segment(Vector2D(2, 4), Vector2D(5, 4))
+		r1 intersection s1 should not equal (None)
+	}
+	
+	it should "have an intersection point if the segment contains the ray's vertice" in {
+		val r1 = Ray(Vector2D(0, math.Pi), Vector2D(1, 2))
+		val s1 = Segment(Vector2D(0, 0), Vector2D(0, 4))
+		r1 intersection s1 should not equal (None)
+		
+		val r2 = Ray(Vector2D(2, 3), Vector2D(-2, -1))
+		val s2 = Segment(Vector2D(0, 4), Vector2D(4, 2))
+		r2 intersection s2 should not equal (None)
+	}
+	
+	it should "not have an intersection point if they are parallel" in {
+		val r1 = Ray(Vector2D(2, 3), Vector2D(3, 4))
+		val s1 = Segment(Vector2D(1, 4), Vector2D(4, 8))
+		r1 intersection s1 should equal (None)
+	}
+	
+	it should "not have an intersection point if they lie on each other" in {
+		val r1 = Ray(Vector2D(0, 1), Vector2D(2, 3))
+		val s1 = Segment(Vector2D(-1, 0), Vector2D(4, 4))
+		r1 intersection s1 should equal (None)
+	}
+	
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gjk/EPA.scala b/src/test/scala/sims/test/gjk/EPA.scala
new file mode 100644
index 0000000..e64a73e
--- /dev/null
+++ b/src/test/scala/sims/test/gjk/EPA.scala
@@ -0,0 +1,98 @@
+package sims.test.gjk
+
+import scala.collection.mutable.ListBuffer
+import sims.math._
+
+/** The implementation was adapted from dyn4j by William Bittle (see http://www.dyn4j.org). */
+object EPA {
+	
+	val MaxIterations = 100
+	
+	val DistanceEpsilon = 0.0001
+	
+	protected class Edge(val normal: Vector2D, val distance: Double, val index: Int)
+	
+	private def winding(simplex: ListBuffer[Vector2D]) = {
+		for (i <- 0 until simplex.size) {
+			val j = if (i + 1 == simplex.size) 0 else i + 1
+			//val winding = math.signum(simplex(j) - simplex(i))
+		}
+		
+	}
+		
+	
+	def getPenetration(simplex: ListBuffer[Vector2D], minkowskiSum: MinkowskiSum): Penetration = {
+		// this method is called from the GJK detect method and therefore we can assume
+		// that the simplex has 3 points
+		
+		// get the winding of the simplex points
+		// the winding may be different depending on the points added by GJK
+		// however EPA will preserve the winding so we only need to compute this once
+		val winding = math.signum((simplex(1) - simplex(0)) cross (simplex(2) - simplex(1))).toInt
+		// store the last point added to the simplex
+		var point = Vector2D.Null
+		// the current closest edge
+		var edge: Edge = null;
+		// start the loop
+		for (i <- 0 until MaxIterations) {
+			// get the closest edge to the origin
+			edge = this.findClosestEdge(simplex, winding);
+			// get a new support point in the direction of the edge normal
+			point = minkowskiSum.support(edge.normal);
+			
+			// see if the new point is significantly past the edge
+			val projection: Double = point dot edge.normal
+			if ((projection - edge.distance) < DistanceEpsilon) {
+				// then the new point we just made is not far enough
+				// in the direction of n so we can stop now and
+				// return n as the direction and the projection
+				// as the depth since this is the closest found
+				// edge and it cannot increase any more
+				return new Penetration(edge.normal, projection)
+			}
+			
+			// lastly add the point to the simplex
+			// this breaks the edge we just found to be closest into two edges
+			// from a -> b to a -> newPoint -> b
+			simplex.insert(edge.index, point);
+		}
+		// if we made it here then we know that we hit the maximum number of iterations
+		// this is really a catch all termination case
+		// set the normal and depth equal to the last edge we created
+		return new Penetration(edge.normal, point dot edge.normal)
+	}
+	
+	private def findClosestEdge(simplex: ListBuffer[Vector2D], winding: Int): Edge = {
+		// get the current size of the simplex
+		val size = simplex.size;
+		// create an edge
+		var edge = new Edge(Vector2D.Null, Double.PositiveInfinity, 0)
+		// find the edge on the simplex closest to the origin
+		for (i <- 0 until size) {
+			// compute j
+			val j = if (i + 1 == size) 0 else i + 1
+			// get the points that make up the current edge
+			val a = simplex(i);
+			val b = simplex(j);
+			// create the edge
+			val direction = simplex(j) - simplex(i);
+			// depending on the winding get the edge normal
+			// it would findClosestEdge(List<Vector2> simplex, int winding) {
+			// get the current size of the simplex
+			val normal = if (winding > 0) direction.rightNormal.unit
+									 else direction.leftNormal.unit
+		
+			// project the first point onto the normal (it doesnt matter which
+			// you project since the normal is perpendicular to the edge)
+			val d: Double = math.abs(simplex(i) dot normal);
+			// record the closest edge
+			if (d < edge.distance) 
+				edge = new Edge(normal, d, j)
+		}
+		// return the closest edge
+		return edge;
+	}
+
+}
+
+class Penetration(val normal: Vector2D, val overlap: Double)
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gjk/FeatureManifold.scala b/src/test/scala/sims/test/gjk/FeatureManifold.scala
new file mode 100644
index 0000000..527d8ba
--- /dev/null
+++ b/src/test/scala/sims/test/gjk/FeatureManifold.scala
@@ -0,0 +1,89 @@
+package sims.test.gjk
+
+import sims.collision._
+import sims.math._
+
+object FeatureManifold {
+	
+	def farthestFeature(collidable: Collidable, direction: Vector2D): Either[Vector2D, Segment] = collidable match {
+		
+		case c: Circle => Left(c.position + direction.unit * c.radius)
+		
+		case p: ConvexPolygon => {
+			var max = p.vertices(0) dot direction.unit
+			
+			//maximum vertice index
+			var i = 0
+  	
+			for (j <- 0 until p.vertices.length) {
+				val d = p.vertices(j) dot direction.unit
+				if (d > max) {
+					max = d
+					i = j
+				}
+			}
+			
+			/* 1) vertex is considered to be the first point of a segment
+			 * 2) polygons vertices are ordered counter-clockwise
+			 * 
+			 * implies:
+			 * previous segment is the (i-1)th segment
+			 * next segment is the ith segment */
+			val prev = if (i == 0) p.sides.last else p.sides(i - 1)
+			val next = p.sides(i)
+			
+			// check which segment is less parallel to direction
+			val side =
+				if ((prev.direction0 dot direction).abs <= (next.direction0 dot direction).abs) prev
+				else next
+			
+				Right(side)
+		}
+		
+		case _ => throw new IllegalArgumentException("Collidable is of unknown type.")
+		
+	}
+	
+	def getCollisionPoints(pair: (Collidable, Collidable), normal: Vector2D): Option[Manifold] = {
+		var points = new scala.collection.mutable.ArrayBuffer[Vector2D]
+		
+		val feature1 = farthestFeature(pair._1, normal)
+		
+		//is feature 1 a vertex?
+		if (feature1.isLeft) {
+			return Some(Manifold(Array(feature1.left.get), normal))
+		}
+		
+		val feature2 = farthestFeature(pair._2, -normal)
+		
+		//is feature 2 a vertex?
+		if (feature2.isLeft) {
+			return Some(Manifold(Array(feature2.left.get), -normal))
+		}
+		
+		//neither feature is a vertex
+		val side1 = feature1.right.get
+		val side2 = feature2.right.get
+		
+		
+		val flipped = (side1.direction0 dot normal).abs > (side2.direction0 dot normal).abs
+		val reference = if (!flipped) side1 else side2
+		val incident = if (!flipped) side2 else side1
+		
+		
+		//both features are sides, clip feature2 to feature1
+		val clipped = incident clipped reference
+		
+		/*val n = if (!flipped) normal else -normal
+		clipped match {
+			case None => None
+			case Some(Segment(point1, point2)) => Some( 
+				Manifold(Array(point1, point2) filter ((v: Vector2D) => ((v - reference.point1) dot n) <= 0), n)
+				)
+		}*/
+		error("")
+	}
+
+}
+
+case class Manifold(pts: Array[Vector2D], normal: Vector2D)
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gjk/GJK.scala b/src/test/scala/sims/test/gjk/GJK.scala
new file mode 100644
index 0000000..a8b1732
--- /dev/null
+++ b/src/test/scala/sims/test/gjk/GJK.scala
@@ -0,0 +1,115 @@
+package sims.test.gjk
+
+import scala.collection.mutable.ListBuffer
+import sims.collision._
+import sims.collision.narrowphase.NarrowPhaseDetector
+import sims.math._
+
+/** A narrowphase detector using the Gilbert-Johnson-Keerthi algorithm. */
+class GJK[A <: Collidable: ClassManifest] extends NarrowPhaseDetector[A] {
+	
+	/** Checks whether the given simplex contains the origin. If it does, `None` is returned.
+	  * Otherwise a new search direction is returned and the simplex is updated. */
+	protected def checkSimplex(simplex: ListBuffer[Vector2D], direction: Vector2D): Option[Vector2D] = {
+		if (simplex.length == 2) { //simplex == 2
+			val a = simplex(0)
+			val b = simplex(1)
+			val ab = b - a
+			val ao = -a
+			
+			if (ao directionOf ab) {
+				Some(ab cross ao cross ab)
+			} else {
+				simplex.remove(1)
+				Some(ao)
+			}
+		} // end simplex == 2
+		
+		else if (simplex.length == 3) { //simplex == 3
+			val a = simplex(0)
+			val b = simplex(1)
+			val c = simplex(2)
+			val ab = b - a
+			val ac = c - a
+			val ao = -a
+			val winding = ab cross ac
+			
+			if (ao directionOf (ab cross winding)) {
+				if (ao directionOf ab) {
+					simplex.remove(2)
+					Some(ab cross ao cross ab)
+				} else if (ao directionOf ac) {
+					simplex.remove(1)
+					Some(ac cross ao cross ac)
+				} else {
+					simplex.remove(2)
+					simplex.remove(1)
+					Some(ao)
+				}
+			} else {
+				if (ao directionOf (winding cross ac)) {
+					if (ao directionOf ac) {
+						simplex.remove(1)
+						Some(ac cross ao cross ac)
+					} else {
+						simplex.remove(2)
+						simplex.remove(1)
+						Some(ao)
+					}
+				} else {
+					None
+				}
+			}
+		} //end simplex == 3
+		
+		else throw new IllegalArgumentException("Invalid simplex size.")
+		
+	}
+	
+	
+	def getPenetration(pair: (A, A)): Option[Penetration] = {
+		implicit val pr = pair
+		val ms = new MinkowskiSum(pair)
+		import ms._
+		val s = support(Vector2D.i)
+		val simplex = new ListBuffer[Vector2D]
+		simplex prepend s
+		var direction = -s
+		
+		var counter = 0
+		while (counter < 100) {
+			
+			val a = support(direction)
+			
+			if ((a dot direction) < 0) return None
+			
+			simplex prepend a
+			
+			val newDirection = checkSimplex(simplex, direction)
+			
+			if (newDirection.isEmpty) return Some(EPA.getPenetration(simplex, ms))
+			else direction = newDirection.get
+			
+			counter += 1
+		}
+		throw new IllegalArgumentException("Something went wrong, should not reach here.")
+	}
+	
+	override def collision(pair: (A, A)): Option[Collision[A]] = {
+		val p = getPenetration(pair)
+		if (p == None) return None
+		val man = FeatureManifold.getCollisionPoints(pair, p.get.normal)
+		if (man == None) return None
+		
+		Some(new Collision[A] {
+			val item1 = pair._1 
+			val item2 = pair._2
+			val normal = man.get.normal
+			val overlap = p.get.overlap 
+			val points = man.get.pts.toList
+		})
+	}
+	
+}
+
+
diff --git a/src/test/scala/sims/test/gjk/GJK2.scala b/src/test/scala/sims/test/gjk/GJK2.scala
new file mode 100644
index 0000000..7fdaec3
--- /dev/null
+++ b/src/test/scala/sims/test/gjk/GJK2.scala
@@ -0,0 +1,168 @@
+package sims.test.gjk
+
+/**
+ * Created by IntelliJ IDEA.
+ * User: jakob
+ * Date: 3/27/11
+ * Time: 7:47 PM
+ * To change this template use File | Settings | File Templates.
+ */
+
+import scala.collection.mutable.ListBuffer
+import sims.math._
+import sims.collision._
+import sims.collision.narrowphase.NarrowPhaseDetector
+
+case class Separation(distance: Double, normal: Vector2D, point1: Vector2D, point2: Vector2D)
+class GJK2[A <: Collidable: ClassManifest] extends NarrowPhaseDetector[A] {
+
+  val margin = 0.1
+
+  case class MinkowskiPoint(convex1: Collidable, convex2: Collidable, direction: Vector2D) {
+    val point1 = convex1.support(direction) - direction.unit * margin
+    val point2 = convex2.support(-direction) + direction.unit * margin
+    val point = point1 - point2
+  }
+  
+  def support(c1: A, c2: A, direction: Vector2D) = MinkowskiPoint(c1, c2, direction)
+  implicit def minkowsi2Vector(mp: MinkowskiPoint) = mp.point
+
+  def separation(c1: A, c2: A): Option[(Separation)] = {
+
+    //initial search direction
+    val direction0 = Vector2D.i
+
+    //simplex points
+    var a = support(c1, c2, direction0)
+    var b = support(c1, c2, -direction0)
+
+    var counter = 0
+    while (counter < 100) {
+
+      //closest point on the current simplex closest to origin
+      val point = segmentClosestPoint(a, b,Vector2D.Null)
+
+      if (point.isNull) return None
+
+      //new search direction
+      val direction = -point.unit
+
+      //new Minkowski Sum point
+      val c = support(c1, c2, direction)
+
+      if (containsOrigin(a, b, c)) return None
+
+      val dc = (direction dot c)
+      val da = (direction dot a)
+
+      if (dc - da < 0.0001) {
+        val (point1, point2) = findClosestPoints(a, b)
+        return Some(Separation(dc, direction, point1, point2))
+      }
+
+      if (a.lengthSquare < b.lengthSquare) b = c
+      else a = c
+      //counter += 1
+    }
+    return None
+  }
+
+
+  def findClosestPoints(a: MinkowskiPoint, b: MinkowskiPoint): (Vector2D, Vector2D) =  {
+		var p1 = Vector2D.Null
+		var p2 = Vector2D.Null
+
+		// find lambda1 and lambda2
+		val l: Vector2D = b - a
+
+		// check if a and b are the same point
+		if (l.isNull) {
+			// then the closest points are a or b support points
+			p1 = a.point1
+			p2 = a.point2
+		} else {
+			// otherwise compute lambda1 and lambda2
+			val ll = l dot l;
+			val l2 = -l.dot(a) / ll;
+			val l1 = 1 - l2;
+
+			// check if either lambda1 or lambda2 is less than zero
+			if (l1 < 0) {
+				// if lambda1 is less than zero then that means that
+				// the support points of the Minkowski point B are
+				// the closest points
+				p1 = b.point1
+				p2 = b.point2
+			} else if (l2 < 0) {
+				// if lambda2 is less than zero then that means that
+				// the support points of the Minkowski point A are
+				// the closest points
+				p1 = a.point1
+				p2 = a.point2
+			} else {
+				// compute the closest points using lambda1 and lambda2
+				// this is the expanded version of
+				// p1 = a.p1.multiply(l1).add(b.p1.multiply(l2));
+				// p2 = a.p2.multiply(l1).add(b.p2.multiply(l2));
+        p1 = a.point1 * l1 + b.point1 * l2
+        p2 = a.point2 * l1 + b.point2 * l2
+			}
+		}
+
+    (p1, p2)
+  }
+
+  def segmentClosestPoint(a: Vector2D, b: Vector2D, point: Vector2D): Vector2D = {
+    if (a == b) return a
+    val direction = b - a
+    var t = ((point - a) dot (direction)) / (direction dot direction)
+    if (t < 0) t = 0
+  	if (t > 1) t = 1
+  	a + direction * t
+  }
+
+  def containsOrigin(a: Vector2D, b: Vector2D, c: Vector2D): Boolean =  {
+		val sa = a.cross(b);
+		val sb = b.cross(c);
+		val sc = c.cross(a);
+		// this is sufficient (we do not need to test sb * sc)
+		sa * sb  > 0 && sa * sc > 0
+	}
+
+  def collision(pair: (A, A)): Option[Collision[A]] = {
+    pair match {
+      case (c1: Circle, c2: Circle) => collisionCircleCircle(c1, c2)(pair)
+      case _ => gjkCollision(pair)
+    }
+  }
+
+  private def gjkCollision(pair: (A, A)): Option[Collision[A]] = {
+    val so = separation(pair._1, pair._2)
+    if (so.isEmpty) return None //deep contact is not implemented yet
+    val s = so.get
+    Some(new Collision[A] {
+      val item1 = pair._1
+      val item2 = pair._2
+      val overlap = -(s.distance - 2 * margin)
+      val points = List(s.point1)
+      val normal = s.normal.unit
+    })
+
+  }
+
+  private def collisionCircleCircle(c1: Circle, c2: Circle)(pair: (A, A)) = {
+		val d = (c2.position - c1.position)
+		val l = d.length
+		if (l <= c1.radius + c2.radius) {
+			val p = c1.position + d.unit * (l - c2.radius)
+			Some(new Collision[A] {
+				val item1 = pair._1
+				val item2 = pair._2
+				val normal = d.unit
+				val points = List(p)
+				val overlap = (c1.radius + c2.radius - l)
+			})
+		} else None
+	}
+
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gjk/GJKTest.scala b/src/test/scala/sims/test/gjk/GJKTest.scala
new file mode 100644
index 0000000..d2ba22f
--- /dev/null
+++ b/src/test/scala/sims/test/gjk/GJKTest.scala
@@ -0,0 +1,115 @@
+package sims.test.gjk
+
+import processing.core.PApplet
+import processing.core.PConstants._
+
+class GJKTest extends PApplet {
+	implicit val top = this
+		
+	import sims.dynamics._
+	import sims.math._
+	import sims.test.gui._
+	import sims.test.gui.RichShape._
+	
+	var s1: GraphicalShape = _ 
+	var s2: GraphicalShape = _
+	
+	override def setup() = {
+		size(600, 600, P2D)
+		background(255,255,255)
+		frameRate(60)
+		
+		
+		s1 = (new Rectangle(1, 3) {position = Vector2D(5,5)}).toGraphical
+		s2 = (new Rectangle(1, 2) {position = Vector2D(8,8); rotation = 0.2}).toGraphical
+		
+	}
+	
+	val PPM = 39.37f * 96
+	var viewScale: Float = 1.0f / 80
+	
+	val GJK = new GJK2[Shape]
+	
+	var invert = false
+	def pair = if (!invert) (s1, s2) else (s2, s1)
+	
+	override def draw() = {
+		smooth()
+		background(255,255,255)
+		translate(0, height)
+		scale(viewScale * PPM, -viewScale * PPM)
+		
+		if (keyCode == 32) invert = true
+		else invert = false
+		
+		val collision = GJK.collision(pair._1.shape, pair._2.shape)
+		/*if (collision != None) {
+			pushMatrix()
+			rectMode(CORNER)
+			stroke(255, 0, 50)
+			strokeWeight(10)
+			fill(0, 0, 0, 0)
+			rect(0, 0, 600, 600)
+			strokeWeight(1)
+			popMatrix()
+		}*/
+    //val separation = GJK.collision(pair._1.shape, pair._2.shape)
+    //if (!separation.isEmpty)
+      //List(separation.get.point1, separation.get.point2) foreach (p => ellipse(p.x.toFloat, p.y.toFloat, 0.1f, 0.1f))
+
+
+	
+		label()
+		s2.shape.position = Vector2D(mouseX / viewScale / PPM, -(mouseY - height) / viewScale / PPM)
+		
+		s1.render()
+		s2.render()
+		
+		
+		
+		collision match {
+			case Some(c) => {
+				stroke(0, 255, 0)
+				for (p <- c.points) {
+					ellipse(p.x.toFloat, p.y.toFloat, 0.1f, 0.1f)
+					val s = p
+					val e = p + c.normal
+					line(s.x.toFloat, s.y.toFloat, e.x.toFloat, e.y.toFloat)
+					println(c.overlap)
+				}
+			}
+			case _ => ()
+    }
+		
+		/*stroke(255, 0, 255)
+		val f = FeatureManifold.farthestFeature(pair._1.shape, Vector2D.j + Vector2D.i)
+		f match {
+			case Left(p) => ellipse(p.x.toFloat, p.y.toFloat, 0.1f, 0.1f)
+			case Right(s) => line(s.point1.x.toFloat, s.point1.y.toFloat, s.point2.x.toFloat, s.point2.y.toFloat)
+		}*/
+		
+	}
+	
+	private val fontSize = 16
+	private val f = createFont("Monospaced.plain", fontSize)
+	private def label() = {
+		val size = 16
+		fill(0, 0, 0)
+		textMode(SCREEN)
+		textFont(f)
+		
+		val p1 = pair._1.shape
+		val p2 = pair._2.shape
+		text("1", (p1.position.x * PPM * viewScale).toFloat, (height - p1.position.y * PPM * viewScale).toFloat)
+		text("2", (p2.position.x * PPM * viewScale).toFloat, (height - p2.position.y * PPM * viewScale).toFloat)
+	}
+
+}
+
+object GJKTest {
+	
+	def main(args: Array[String]): Unit = {
+		PApplet.main(args ++ Array("sims.test.gjk.GJKTest"))
+	}
+	
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gjk/MinkowskiSum.scala b/src/test/scala/sims/test/gjk/MinkowskiSum.scala
new file mode 100644
index 0000000..7c574ce
--- /dev/null
+++ b/src/test/scala/sims/test/gjk/MinkowskiSum.scala
@@ -0,0 +1,11 @@
+package sims.test.gjk
+
+import sims.collision._
+import sims.math._
+
+class MinkowskiSum(pair: (Collidable, Collidable)) {
+	
+	def support(direction: Vector2D) = 
+		pair._1.support(direction) - pair._2.support(-direction)
+		
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/DebugWorld.scala b/src/test/scala/sims/test/gui/DebugWorld.scala
new file mode 100644
index 0000000..937bd77
--- /dev/null
+++ b/src/test/scala/sims/test/gui/DebugWorld.scala
@@ -0,0 +1,29 @@
+package sims.test.gui
+
+class DebugWorld extends sims.dynamics.World with Publisher {
+	
+	override def +=(b: sims.dynamics.Body) = {
+		super.+=(b)
+		publish(BodyAdded(this, b))
+	}
+	
+	override def -=(b: sims.dynamics.Body) = {
+		super.-=(b)
+		publish(BodyRemoved(this, b))
+	}
+	
+	override def +=(j: sims.dynamics.Joint) = {
+		super.+=(j)
+		publish(JointAdded(this, j))
+	}
+	
+	override def -=(j: sims.dynamics.Joint) = {
+		super.-=(j)
+		publish(JointRemoved(this, j))
+	}
+	
+	override def step() = {
+		super.step()
+		publish(Stepped(this))
+	}
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/KeyManager.scala b/src/test/scala/sims/test/gui/KeyManager.scala
new file mode 100644
index 0000000..f7cc199
--- /dev/null
+++ b/src/test/scala/sims/test/gui/KeyManager.scala
@@ -0,0 +1,53 @@
+package sims.test.gui
+
+import processing.core.PApplet
+
+class KeyManager(implicit top: Main) {
+
+	def keyPressed(keyCode: Int) = keyCode match {
+		// ENTER
+		case 10 => top.SceneManager.currentScene.world.step()
+		
+		// SPACE
+		case 32 => top.paused = !top.paused
+		
+		// PAGE UP
+		case 33 => top.viewScale += top.viewScale * 0.02f
+		
+		// PAGE DOWN
+		case 34 => top.viewScale -= top.viewScale * 0.02f
+		
+		// 0
+		case 36 => {top.offsetX = 0; top.offsetY = 0}
+		
+		// LEFT
+		case 37 => top.offsetX += 50
+		
+		// UP
+		case 38 => top.offsetY -= 50
+		
+		// RIGHT
+		case 39 => top.offsetX -= 50
+		
+		// DOWN
+		case 40 => top.offsetY += 50
+		
+		// , (<)
+		case 44 => top.SceneManager.previousScene()
+		
+		// . (>)
+		case 46 => top.SceneManager.nextScene()
+		
+		// b
+		case 66 => top.SceneManager.currentScene.world.errorReduction += 0.1
+		
+		//v
+		case 86 => top.SceneManager.currentScene.world.errorReduction -= 0.1
+		
+		case 45 => top.SceneManager.currentScene.world.iterations -= 1
+		case 61 => top.SceneManager.currentScene.world.iterations += 1
+			
+		case x: Any => println("unknown key: " + x)
+	}
+	
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/Main.scala b/src/test/scala/sims/test/gui/Main.scala
new file mode 100644
index 0000000..745d793
--- /dev/null
+++ b/src/test/scala/sims/test/gui/Main.scala
@@ -0,0 +1,143 @@
+package sims.test.gui
+
+import processing.core.PApplet
+import processing.core.PConstants._
+import scala.collection.mutable.ArrayBuffer
+
+import sims.math._
+import sims.test.gui.RichShape._
+import sims.test.gui.scenes._
+import sims.dynamics.Shape
+
+class Main extends PApplet {
+	implicit val top = this
+	
+	val SceneManager = new SceneManager
+	import SceneManager._
+	
+	val KeyManager = new KeyManager
+	import KeyManager._
+	
+	var (offsetX, offsetY) = (200.0f, 100.0f)
+	val PPM = 39.37f * 96
+	var viewScale: Float = 1.0f / 80
+	
+	private val fontSize = 16
+	private val f = createFont("Monospaced.plain", fontSize)
+	private def displayText(lines: String*) = {
+		val size = 16
+		val indent = 10
+		
+		fill(0, 0, 0)
+		textMode(SCREEN)
+		textFont(f)
+		
+		for (i <- 0 until lines.length) text(lines(i), indent, (i + 1) * size)
+	}
+	
+	
+	override def setup() = {
+		size(screenWidth * 2 / 3, screenHeight * 2 / 3, P2D)
+		background(255,255,255)
+		frameRate(60)
+		//frame.setResizable(true)
+		currentScene = scenes(0)
+	}
+	
+	var paused = true
+	override def draw() = {
+		smooth()
+		background(255,255,255)
+		
+		translate(offsetX, height - offsetY)
+		scale(viewScale * PPM, -viewScale * PPM)
+		
+		val t0 = System.nanoTime()
+		if (!paused) currentScene.world.step()
+		val collisions = if (currentScene.world.collisionDetection) SceneManager.currentScene.world.detector.collisions() else Seq()
+		val dStep = System.nanoTime() - t0
+		
+		for (g <- graphicals) g.render()
+		fill(255, 0, 0)
+		stroke(20, 0, 0)
+		for (c <- collisions; p <- c.points) {
+			ellipse(p.x.toFloat, p.y.toFloat, 0.1f, 0.1f)
+			stroke(0, 255, 0)
+			val s = p
+			val e = p + c.normal
+			line(s.x.toFloat, s.y.toFloat, e.x.toFloat, e.y.toFloat)
+		}
+				
+				//_.points.foreach((v) => ellipse(v.x.toFloat, v.y.toFloat, 0.1f, 0.1f)))
+		
+		val dRender = System.nanoTime() - t0 - dStep
+		
+		displayText(
+								"status     : " + (if (paused) "paused" else "running"),
+								"------------",
+								"fps    [Hz]: " + frameRate,
+								"------------",
+								"step   [ms]: " + (dStep / 1E6f),
+								"       [%] : " + (dStep.toFloat / (dStep + dRender) * 100),
+								"render [ms]: " + (dRender / 1E6f),
+								"       [%] : " + (dRender.toFloat / (dStep + dRender) * 100),
+								"------------",
+								"memory [MB]: " + java.lang.Runtime.getRuntime.totalMemory / 1E6,
+								"load       : " + java.lang.management.ManagementFactory.getOperatingSystemMXBean.getSystemLoadAverage(),
+								"------------",
+								"bodies     : " + currentScene.world.bodies.length,
+								"shapes     : " + currentScene.world.shapes.length,
+								"joints     : " + currentScene.world.joints.length,
+								"constraints: " + currentScene.world.joints.map(_.constraints.length).sum,
+								"collisions : " + collisions.length,
+								"it     [1] : " + currentScene.world.iterations,
+								"dt     [ms]: " + currentScene.world.h.toFloat,
+								"erp    [ms]: " + currentScene.world.errorReduction.toFloat,
+								"------------",
+								"(" + scaledMouseX + ", " + scaledMouseY + ")"
+				)
+	}
+	
+	def drawGrid() = {
+		
+	}
+	
+	override def keyPressed() = KeyManager.keyPressed(keyCode)
+	
+	def scaledMouseX = (mouseX - offsetX) / viewScale / PPM 
+	def scaledMouseY = (height - mouseY - offsetY) / viewScale / PPM
+	
+	var mouseJoint: Option[MouseJoint] = None
+	override def mousePressed(): Unit = {
+		import Vector2D._
+		val body = currentScene.world.bodies.find(_.contains((scaledMouseX, scaledMouseY)))
+		if (body.isEmpty) return ()
+		val mj = new MouseJoint(body.get, (scaledMouseX, scaledMouseY))
+		currentScene.world += mj
+		mouseJoint = Some(mj)
+	}
+	
+	override def mouseReleased(): Unit = {
+		if (mouseJoint.isEmpty) return ()
+		currentScene.world -= mouseJoint.get
+		mouseJoint = None
+	}
+	
+	override def mouseDragged(): Unit = {
+		import Vector2D._
+		
+		if (mouseJoint.isEmpty) return ()
+		
+		mouseJoint.get.anchor = (scaledMouseX, scaledMouseY)
+		
+	}
+
+	
+
+}
+
+object Main {
+  def main(args : Array[String]) : Unit = {
+  	PApplet.main(args ++ Array("sims.test.gui.Main"))
+  }
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/MouseJoint.scala b/src/test/scala/sims/test/gui/MouseJoint.scala
new file mode 100644
index 0000000..0d74dd5
--- /dev/null
+++ b/src/test/scala/sims/test/gui/MouseJoint.scala
@@ -0,0 +1,38 @@
+package sims.test.gui
+
+import sims.dynamics._
+import sims.dynamics.constraints._
+import sims.math._
+
+class MouseJoint(val body1: Body, var anchor: Vector2D) extends Joint {
+	val body2 = new Body() {fixed = true}
+	
+	private val self = this
+	
+	private val local1 = anchor - body1.position
+	
+	private val rotation01 = body1.rotation
+	
+	def r1 = (local1 rotate (body1.rotation - rotation01))
+	
+	def x1 = body1.position + r1
+	def x2 = anchor
+	
+	def x = x2 - x1
+	
+	val constraints = List(
+		new Constraint {
+			val body1 = self.body1
+			val body2 = self.body2
+			def value = x.x
+			def jacobian = new Jacobian(-Vector2D(1, 0), r1.y, Vector2D.i, 0)
+		},
+		new Constraint {
+			val body1 = self.body1
+			val body2 = self.body2
+			def value = x.y
+			def jacobian = new Jacobian(-Vector2D(0, 1), -r1.x, Vector2D.j, 0)
+		}
+	)
+	
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/RichJoint.scala b/src/test/scala/sims/test/gui/RichJoint.scala
new file mode 100644
index 0000000..2c3d5fd
--- /dev/null
+++ b/src/test/scala/sims/test/gui/RichJoint.scala
@@ -0,0 +1,51 @@
+package sims.test.gui
+
+import processing.core.PApplet
+import processing.core.PConstants._
+import sims.dynamics._
+
+class RichJoint(joint: Joint) {
+private implicit def double2Float(x: Double): Float = x.toFloat
+	
+	def toGraphical(implicit parent: PApplet) = new GraphicalJoint(joint) {
+		
+		val top = parent
+		
+		val render = joint match {
+			
+			case j: DistanceJoint => () => {
+				top.pushMatrix()
+				top.stroke(0, 0, 0)
+				top.fill(0, 0, 0)
+				top.line(j.x1.x, j.x1.y, j.x2.x, j.x2.y)
+				top.stroke(100, 100, 100)
+				top.line(j.body1.position.x, j.body1.position.y, j.x1.x, j.x1.y)
+				top.line(j.body2.position.x, j.body2.position.y, j.x2.x, j.x2.y)
+				top.popMatrix()
+			}
+			
+			case j: RevoluteJoint => () => {
+				top.pushMatrix()
+				top.stroke(0, 0, 0)
+				top.fill(0, 0, 0)
+				top.line(j.x1.x, j.x1.y, j.x2.x, j.x2.y)
+				top.stroke(100, 100, 100)
+				top.line(j.body1.position.x, j.body1.position.y, j.x1.x, j.x1.y)
+				top.line(j.body2.position.x, j.body2.position.y, j.x2.x, j.x2.y)
+				top.popMatrix()
+			}
+			
+			case j: MouseJoint => () => ()
+			
+			case _ => throw new IllegalArgumentException("Cannot create graphical joint: unknown joint.")
+		}
+	
+	}
+	
+}
+
+object RichJoint {
+	
+	implicit def jointToRichShape(j: Joint) = new RichJoint(j)
+	
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/RichShape.scala b/src/test/scala/sims/test/gui/RichShape.scala
new file mode 100644
index 0000000..6817ebe
--- /dev/null
+++ b/src/test/scala/sims/test/gui/RichShape.scala
@@ -0,0 +1,49 @@
+package sims.test.gui
+
+import processing.core.PApplet
+import processing.core.PConstants._
+import sims.dynamics._
+
+class RichShape(shape: Shape) {
+	private implicit def double2Float(x: Double): Float = x.toFloat
+	
+	def toGraphical(implicit parent: PApplet) = new GraphicalShape(shape) {
+		
+		val top = parent
+		
+		val render = shape match {
+			
+			case c: Circle => () => {
+				top.pushMatrix()
+				top.stroke(0, 0, 0)
+				top.fill(0, 0, 255, 200)
+				top.translate(c.position.x, c.position.y)
+				top.rotate(-c.rotation)
+				top.ellipseMode(CENTER)
+				top.ellipse(0, 0, c.radius * 2, c.radius * 2)
+				top.line(0,0, c.radius, 0)
+				top.popMatrix()
+			}
+			
+			case r: Rectangle => () => {
+				top.pushMatrix()
+				top.translate(r.position.x, r.position.y)
+				top.rotate(-r.rotation)
+				top.fill(255, 0, 0, 200)
+				top.rectMode(CENTER)
+				top.rect(0, 0, r.halfWidth * 2, r.halfHeight * 2)
+				top.popMatrix()
+			}
+			
+			case _ => throw new IllegalArgumentException("Cannot create graphical shape: unknown shape.")
+		}
+	
+	}
+	
+}
+
+object RichShape {
+	
+	implicit def shapeToRichShape(s: Shape) = new RichShape(s)
+	
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/Scene.scala b/src/test/scala/sims/test/gui/Scene.scala
new file mode 100644
index 0000000..6e1664e
--- /dev/null
+++ b/src/test/scala/sims/test/gui/Scene.scala
@@ -0,0 +1,13 @@
+package sims.test.gui
+
+trait Scene extends Reactor {
+	def name: String = this.getClass().getName()
+	def description: String = ""
+	
+	val world = new DebugWorld
+	
+	def init(): Unit
+	
+	def exit(): Unit = {}
+	
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/SceneManager.scala b/src/test/scala/sims/test/gui/SceneManager.scala
new file mode 100644
index 0000000..39da308
--- /dev/null
+++ b/src/test/scala/sims/test/gui/SceneManager.scala
@@ -0,0 +1,95 @@
+package sims.test.gui
+
+import processing.core.PApplet
+import scala.collection.mutable.ArrayBuffer
+import sims.math._
+import sims.test.gui.scenes._
+import sims.test.gui.RichShape._
+import sims.test.gui.RichJoint._
+
+class SceneManager(implicit top: PApplet) {
+	
+	/* Contains objects that will be rendered on `draw()`. */
+	private var _graphicals = new ArrayBuffer[Graphical[_]]
+	def graphicals: Seq[Graphical[_]] = _graphicals
+	
+	/* Current scene. */
+	private var _currentScene: Scene = EmptyScene
+	
+	/* Get current scene. */
+	def currentScene = _currentScene
+	
+	/* Set current scene. */
+	def currentScene_=(newScene: Scene) = {
+		
+		// remove reactions
+		currentScene.deafTo(currentScene.world)
+		currentScene.reactions.clear()
+		
+		// empty world
+		currentScene.world.clear()
+		
+		// clear graphical objects
+		_graphicals.clear()
+		
+		// custom exit behavior
+		currentScene.exit()
+		
+		// add new reactions to create / remove graphical objects
+		newScene.listenTo(newScene.world)
+		newScene.reactions += {
+			case BodyAdded(newScene.world, body) => for (s <- body.shapes) _graphicals += s.toGraphical
+			case BodyRemoved(newScene.world, body) => for (s <- body.shapes) {
+				val index = _graphicals.findIndexOf((g: Graphical[_]) => g match {
+					case gs: GraphicalShape => gs.physical == s
+					case _ => false
+				})
+				_graphicals.remove(index)
+			}
+			
+			case JointAdded(newScene.world, joint) => _graphicals += joint.toGraphical
+			case JointRemoved(newScene.world, joint) => {
+				val index = _graphicals.findIndexOf((g: Graphical[_]) => g match {
+					case gj: GraphicalJoint => gj.physical == joint
+					case _ => false
+				})
+				_graphicals.remove(index)
+			}
+			
+		}
+		
+		// custom initialization
+		newScene.init()
+		
+		// set current scene
+		_currentScene = newScene
+		
+		println("set scene to '" + currentScene.name + "'")
+	}
+	
+	private var currentSceneIndex = 0
+	val scenes = List(
+			BasicScene,
+			CollisionScene,
+			LongCollisionScene,
+			CloudScene,
+			PyramidScene,
+			ShiftedStackScene,
+			JointScene
+		)
+		
+	def nextScene() = {
+		currentSceneIndex += 1
+		currentScene = scenes(mod(currentSceneIndex, scenes.length))
+	}
+	
+	def previousScene() = {
+		currentSceneIndex -= 1
+		currentScene = scenes(mod(currentSceneIndex, scenes.length))
+	}
+	
+	def restartScene() = {
+		currentScene = currentScene
+	}
+	
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/events.scala b/src/test/scala/sims/test/gui/events.scala
new file mode 100644
index 0000000..22015ce
--- /dev/null
+++ b/src/test/scala/sims/test/gui/events.scala
@@ -0,0 +1,49 @@
+package sims.test.gui
+
+import scala.collection.mutable.ListBuffer
+import sims.dynamics._
+
+trait Event
+case class BodyAdded(world: World, body: Body) extends Event
+case class BodyRemoved(world: World, body: Body) extends Event
+case class Stepped(wordl: World) extends Event
+case class JointAdded(world: World, joint: Joint) extends Event
+case class JointRemoved(world: World, joint: Joint) extends Event
+
+object Reactions {
+	class Impl extends Reactions {
+		private val parts = new ListBuffer[Reaction]
+		def isDefinedAt(e: Event) = parts exists (_ isDefinedAt e)
+		def +=(r: Reaction) = parts += r
+		def -=(r: Reaction) = parts -= r
+		def clear() = parts.clear()
+		def apply(e: Event) {
+			for (p <- parts; if p isDefinedAt e) p(e)
+		}
+	}
+	type Reaction = PartialFunction[Event, Unit]
+}
+
+abstract class Reactions extends Reactions.Reaction {
+	def +=(r: Reactions.Reaction): Unit
+	def -=(r: Reactions.Reaction): Unit
+	def clear(): Unit
+}
+
+trait Reactor {
+	val reactions: Reactions = new Reactions.Impl 
+	
+	def listenTo(ps: Publisher*) = for (p <- ps) p.subscribe(reactions)
+	def deafTo(ps: Publisher*) = for (p <- ps) p.unsubscribe(reactions)
+}
+
+trait Publisher {
+	import Reactions._
+	
+	private val listeners = new ListBuffer[Reaction]
+	
+	def subscribe(listener: Reaction) = listeners += listener
+	def unsubscribe(listener: Reaction) = listeners -= listener
+	
+	def publish(e: Event) { for (l <- listeners) l(e) }
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/graphicals.scala b/src/test/scala/sims/test/gui/graphicals.scala
new file mode 100644
index 0000000..39b2ea0
--- /dev/null
+++ b/src/test/scala/sims/test/gui/graphicals.scala
@@ -0,0 +1,13 @@
+package sims.test.gui
+
+import processing.core.PApplet
+import sims.dynamics.Shape
+import sims.dynamics.Joint
+
+abstract class Graphical[+A](val physical: A) {
+	val top: PApplet
+	val render: () => Unit
+}
+
+abstract class GraphicalShape(val shape: Shape) extends Graphical[Shape](shape)
+abstract class GraphicalJoint(val joint: Joint) extends Graphical[Joint](joint)
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/scenes/BasicScene.scala b/src/test/scala/sims/test/gui/scenes/BasicScene.scala
new file mode 100644
index 0000000..9598ab1
--- /dev/null
+++ b/src/test/scala/sims/test/gui/scenes/BasicScene.scala
@@ -0,0 +1,18 @@
+package sims.test.gui
+package scenes
+
+import sims.math._
+import sims.dynamics._
+import sims.dynamics._
+
+object BasicScene extends Scene {
+	
+	def init() = {
+		world.gravity = Vector2D.Null
+		val s = new Circle(1)
+		world += new Body(s) {linearVelocity = Vector2D(0.1, 0.01); angularVelocity = 1}
+		world += new Body(new Rectangle(2,1)) {linearVelocity = Vector2D(0.1, 0.01); angularVelocity = 1}
+	}
+	
+
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/scenes/CloudScene.scala b/src/test/scala/sims/test/gui/scenes/CloudScene.scala
new file mode 100644
index 0000000..660e309
--- /dev/null
+++ b/src/test/scala/sims/test/gui/scenes/CloudScene.scala
@@ -0,0 +1,47 @@
+package sims.test.gui
+package scenes
+
+import sims.math._
+import sims.dynamics._
+
+object CloudScene extends Scene {
+	override def description = "A cloud of circles."
+		
+	val MaxItems = 1000
+	val MaxItemSize = 0.2
+	val Width = 10
+	val Height = 10
+	
+	val random = new scala.util.Random(1234567890)
+	def randomCircles(): Seq[Body] = for (i <- 0 until MaxItems) yield {
+		val rX = random.nextDouble * Width
+		val rY = random.nextDouble * Height
+		val c = new Circle(random.nextDouble * MaxItemSize) {
+			position = Vector2D(rX, rY)
+		}
+		new Body(c) {
+			linearVelocity = Vector2D(random.nextDouble * (if (random.nextBoolean) 1 else -1), random.nextDouble * (if (random.nextBoolean) 1 else -1))
+			angularVelocity = random.nextDouble * (if (random.nextBoolean) 1 else -1) * 10
+		}
+	}
+	
+	def frame() = {
+		val points = List(Vector2D(-1, -1), Vector2D(11, -1), Vector2D(11, 11), Vector2D(-1, 11))
+		for (i <- 0 until points.length) yield {
+			val sp = points(i)
+			val ep = points((i + 1) % points.length)
+			val center = (sp + ep) / 2
+			val r = new Rectangle((center - ep).length, 0.2) {
+				position = center
+				rotation = math.Pi / 2 * i
+			}
+			new Body(r) {fixed = true}
+		}
+	}	
+	
+	override def init() = {
+		world.gravity = Vector2D.Null
+		for (r <- randomCircles()) world += r
+		//for (r <- frame()) world += r
+	}
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/scenes/CollisionScene.scala b/src/test/scala/sims/test/gui/scenes/CollisionScene.scala
new file mode 100644
index 0000000..cf2ea61
--- /dev/null
+++ b/src/test/scala/sims/test/gui/scenes/CollisionScene.scala
@@ -0,0 +1,41 @@
+package sims.test.gui
+package scenes
+
+import sims.dynamics._
+import sims.math._
+import sims.dynamics._
+
+object CollisionScene extends Scene {
+	override def description = "A basic collision detection test."
+	
+	var c1 = (new Circle(1) {restitution = 1.0}).asBody
+	var c2 = (new Circle(1) {position = Vector2D(3, 0)}).asBody
+	var r1 = (new Rectangle(0.5, 0.5) {position = Vector2D(6,0)}).asBody
+	
+	def init() = {
+		c1 = (new Circle(1) {restitution = 1.0}).asBody
+		c2 = (new Circle(1) {position = Vector2D(3, 0); restitution = 1.0}).asBody
+		r1 = (new Rectangle(0.5, 0.5) {position = Vector2D(6,0)}).asBody
+		
+		c1.linearVelocity = Vector2D(1, 0)
+	
+		world.gravity = Vector2D(0, 0)
+		world += c1
+		world += c2
+		world += r1
+		
+		reactions += {
+			case Stepped(`world`) =>
+				println(
+					"p: " + (c1.linearMomentum.length + c2.linearMomentum.length + r1.linearMomentum.length).toFloat + 
+					"\tE: " + (E(c1) + E(c2) + E(r1)).toFloat
+					)
+		}
+	}
+	
+	
+	def E(b: Body) = {
+		(b.linearVelocity dot b.linearVelocity) * b.mass / 2
+	}
+		
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/scenes/EmptyScene.scala b/src/test/scala/sims/test/gui/scenes/EmptyScene.scala
new file mode 100644
index 0000000..a88679a
--- /dev/null
+++ b/src/test/scala/sims/test/gui/scenes/EmptyScene.scala
@@ -0,0 +1,6 @@
+package sims.test.gui
+package scenes
+
+object EmptyScene extends Scene {
+	def init() = ()
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/scenes/JointScene.scala b/src/test/scala/sims/test/gui/scenes/JointScene.scala
new file mode 100644
index 0000000..4a0726f
--- /dev/null
+++ b/src/test/scala/sims/test/gui/scenes/JointScene.scala
@@ -0,0 +1,107 @@
+package sims.test.gui
+package scenes
+
+import sims.dynamics._
+import sims.math._
+import sims.dynamics._
+
+object JointScene extends Scene {
+	
+	override def init() = {
+		val b1 = new Body(new Circle(0.1)) {fixed = true}
+		val b2 = new Body(new Rectangle(0.1, 0.5)) {position = Vector2D(2,2)}
+		val j = new DistanceJoint(b1, b1.position, b2, b2.position + Vector2D(0, -0.5))
+		world += b1
+		world += b2
+		world += j
+		
+		val chainBodies = for (i <- 0 until 10) yield new Body(new Rectangle(0.5, 0.1)){fixed = i == 0 || i == 9; position = Vector2D(i, 5)}
+		val chainHinges = for (i <- 0 until chainBodies.length - 1) yield
+			new RevoluteJoint(chainBodies(i), chainBodies(i + 1), Vector2D(i + 0.5, 5))
+		for (b <- chainBodies) world += b
+		for (j <- chainHinges) world += j
+		
+		import sims.dsl._
+		val c = new Body(new Circle(0.1)) {position = Vector2D(4, 0)}
+		world += c
+		world += c distance chainBodies(4)
+		
+		val r = new Body(new Rectangle(2, 0.1)) {position = Vector2D(4, 0)}
+		world += r
+		world += c revolute r
+		
+		val c2 = new Body(new Circle(0.2)) {position = Vector2D(2, 2)}
+		world += c2
+		world += r :@@ (-2, 0) distance c2
+		
+		val r2 = new Body(new Rectangle(0.1, 0.2)) {position = Vector2D(6, 2)}
+		world += r2
+		world += r :@@ (2, 0) distance (0, -0.2) @@: r2
+		
+		val r3 = new Body(new Rectangle(0.3, 0.1)) {position = Vector2D(6.3, 2.2)}
+		world += r3
+		world += r2 :@ (6, 2.2) revolute r3
+		
+		// chaos pendulum
+		{
+			val c1 = new Body(new Circle(0.1)) {fixed = true; position = Vector2D(12, 2)}
+			world += c1
+			val r1 = new Body(new Rectangle(1, 0.1)) {position = Vector2D(13, 2)}
+			world += r1
+			val r2 = new Body(new Rectangle(1, 0.1)) {position = Vector2D(15, 2)}
+			world += r2
+			
+			world += c1 revolute r1
+			world += r1 :@@ (1, 0) revolute r2
+			
+		}
+		
+		// net
+		{
+			val w = 10
+			val h = 10
+			val d = 0.2
+			
+			val nodes = 
+				for (i <- (0 until w).toArray) yield
+					for (j <- (0 until h).toArray) yield
+						new Body(new Circle(0.05)) {fixed = i == 0 && j == h - 1 ; position = Vector2D(i * d, j * d) + Vector2D(-3, 2)}
+			
+			for (n <- nodes.flatten) world += n
+			
+			val joints = {
+				var r: List[DistanceJoint] = Nil
+				for(i <- 0 to nodes.length - 1; j <- 0 to nodes(i).length - 1) {
+					if (i > 0)
+						r = (nodes(i-1)(j) distance nodes(i)(j)) :: r
+					if (j > 0)
+						r = (nodes(i)(j-1) distance nodes(i)(j)) :: r
+				}
+				r
+			}
+			
+			for (j <- joints) world += j
+			
+		}
+		
+		
+		world.collisionDetection = false
+		world.iterations = 10
+		world.errorReduction = 1
+		
+		
+		/*
+		val r1 = new Body(new Rectangle(0.5, 0.1)) {fixed = true; position = Vector2D(5, 5)}
+		val r2 = new Body(new Rectangle(0.5, 0.1)) {position = Vector2D(6, 5)}
+		val r3 = new Body(new Rectangle(0.5, 0.1)) {position = Vector2D(7, 5)}
+		val j12 = new RevoluteJoint(r1, r2, Vector2D(5.5, 5))
+		val j23 = new RevoluteJoint(r2, r3, Vector2D(6.5, 5))
+		world += r1
+		world += r2
+		world += r3
+		world += j12
+		world += j23
+		*/
+	}
+
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/scenes/LongCollisionScene.scala b/src/test/scala/sims/test/gui/scenes/LongCollisionScene.scala
new file mode 100644
index 0000000..d153f1b
--- /dev/null
+++ b/src/test/scala/sims/test/gui/scenes/LongCollisionScene.scala
@@ -0,0 +1,54 @@
+package sims.test.gui
+package scenes
+
+import sims.dynamics._
+import sims.math._
+import sims.dynamics._
+
+object LongCollisionScene extends Scene {
+	override def description = "A test to verify conservation in a collision."
+	
+	def makeBodies() = (for (i <- 0 until 10) yield {
+		new Circle(0.2) {
+			position = Vector2D(0 + 2.01 * radius * i, 0)
+			restitution = 0.8
+		}
+	}.asBody) ++ (for (i <- 0 until 10) yield {
+		new Circle(0.2) {
+			position = Vector2D(6 + 2.01 * radius * i, 0)
+			restitution = 0.8
+		}
+	}.asBody)
+		
+	override def init() = {
+		val bodies = makeBodies()
+		bodies(0).fixed = true
+		bodies(19).fixed = true
+		for (b <- bodies) world += b
+		world.gravity = Vector2D.Null
+		
+		val bullet = new Body(new Circle(0.2) {
+			position = Vector2D(5, 0)
+			restitution = 0.8}) {
+			linearVelocity = Vector2D(3, 0)
+		}
+		world += bullet
+		
+		world.iterations = 10
+		
+		registerListeners()
+	}
+	
+	def registerListeners() = {
+		reactions += {
+			case Stepped(`world`) => println(
+					"P: " + world.bodies.map(P(_)).sum.toFloat + 
+					"\tE: " + world.bodies.map(E(_)).sum.toFloat
+				)
+		}
+	}
+	
+	def P(b: Body) = if (b.fixed) 0 else b.linearMomentum.length
+	def E(b: Body) = if (b.fixed) 0 else (b.linearVelocity dot b.linearVelocity) * b.mass * 0.5
+
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/scenes/PyramidScene.scala b/src/test/scala/sims/test/gui/scenes/PyramidScene.scala
new file mode 100644
index 0000000..19438de
--- /dev/null
+++ b/src/test/scala/sims/test/gui/scenes/PyramidScene.scala
@@ -0,0 +1,40 @@
+package sims.test.gui
+package scenes
+
+import sims.math._
+import sims.dynamics._
+import sims.dynamics._
+
+object PyramidScene extends Scene {
+	override def description = "A pyramid made out of circles."
+	
+	val Base = 40
+	var Radius = 0.2
+	
+	val s = math.sqrt(3)
+	
+	def pyramid: Seq[Body] = (for (i <- 0 until Base) yield {
+		for (j <- 0 until Base - i) yield new Body(
+			new Circle(Radius) {
+				position = Vector2D(2 * j * Radius + i * Radius, s * i * Radius)
+				restitution = 0.5
+			}
+		) {fixed = (i == 0)}
+	}).flatten
+		
+	override def init() = {
+		//world.gravity = Vector2D.Null
+		for (b <- pyramid) world += b
+	
+		val b = new Circle(0.3) {
+			override val density = 10.0
+			position = Vector2D(4,15)
+		}
+		val bd = new Body(b){
+			linearVelocity = Vector2D(0, -2.5)
+		}
+	
+		world += bd
+	}
+
+}
\ No newline at end of file
diff --git a/src/test/scala/sims/test/gui/scenes/ShiftedStackScene.scala b/src/test/scala/sims/test/gui/scenes/ShiftedStackScene.scala
new file mode 100644
index 0000000..c8a7af6
--- /dev/null
+++ b/src/test/scala/sims/test/gui/scenes/ShiftedStackScene.scala
@@ -0,0 +1,30 @@
+package sims.test.gui
+package scenes
+
+import sims.math._
+import sims.dynamics._
+
+object ShiftedStackScene extends Scene {
+	override def description = "A stack of shifted rectangles."
+	/*override val world = new DebugWorld{
+		import sims.collision._
+		import sims.collision.narrowphase._
+		import sims.collision.broadphase._
+		override val detector = SAP[Shape] narrowedBy new sims.test.gjk.GJK[Shape]
+	}*/
+	
+	val width = 1.0
+	val height = 0.2
+	
+	def stack() = for (i <- 0 until 2) yield
+		new Body(new Rectangle(width / 2, height / 2) {
+			position = Vector2D(0.25 * (i % 2) , i * height)
+			restitution = 0.0
+		}) {fixed = i == 0}
+	
+	override def init() = {
+		for (s <- stack()) world += s
+		world.iterations = 100
+	}
+
+}
\ No newline at end of file