9 files changed, 425 insertions, 0 deletions

diff --git a/src/sims/collision/AABB.scala b/src/sims/collision/AABB.scala
new file mode 100644
index 0000000..51b3e12
--- /dev/null
+++ b/src/sims/collision/AABB.scala
@@ -0,0 +1,29 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.collision
+
+import geometry._
+
+/**
+ * Axis Aligned Bounding Boxes, kurz AABBs, sind Rechtecke die eine bestimmte Form umhuellen.
+ * Da AABBs nach den X- und Y-Achsen orientiert sind, ermoeglichen sie eine schnelle
+ * und einfache Feststellung ob zwei AABBs sich ueberschneiden.
+ * @param minVertex Ortsvektor der minimalen Ecke des AABBs
+ * @param maxVertex Ortsvektor der maximalen Ecke des AABBs
+ */
+case class AABB(val minVertex: Vector2D,
+                val maxVertex: Vector2D)
+{ 
+  /**
+   * Ueberprueft ob dieses AABB sich mit dem AABB <code>box</code> ueberschneidet.
+   * @param box das mit diesem auf Ueberschneidung zu ueberpruefende AABB*/
+  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/sims/collision/CircleCollision.scala b/src/sims/collision/CircleCollision.scala
new file mode 100644
index 0000000..baf401a
--- /dev/null
+++ b/src/sims/collision/CircleCollision.scala
@@ -0,0 +1,22 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.collision
+
+import geometry._
+import dynamics._
+
+/**Kollision zwischen zwei Kreisen.*/
+case class CircleCollision(c1: Circle, c2: Circle) extends Collision {
+  val shape1 = c1
+  val shape2 = c2
+  val normal = (c2.pos - c1.pos).unit
+  val points = {
+    val distance = (c2.pos - c1.pos).length
+    val p = shape1.pos + normal * (distance - c2.radius)
+    List(p)
+  } 
+}
diff --git a/src/sims/collision/Collision.scala b/src/sims/collision/Collision.scala
new file mode 100644
index 0000000..ad23f4d
--- /dev/null
+++ b/src/sims/collision/Collision.scala
@@ -0,0 +1,109 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.collision
+
+import dynamics._
+import geometry._
+
+/**Kollisionen zwischen zwei Formen enthalten Methoden zur Berrechnen der Kollisionsreaktion.*/
+abstract class Collision extends Constraint {
+  
+  /**Erste Kollisionsform (Referenz).*/
+  val shape1: Shape
+  
+  /**Zweite Kollisionsform (eindringend).*/
+  val shape2: Shape
+  
+  /**Kollisionspunkte.*/
+  val points: Iterable[Vector2D]
+  
+  /**Normalenvektor zu der Kollisionsebene.*/
+  val normal: Vector2D
+  
+  /* C = delta
+   * Cdot = (vp2 - vp1) dot n
+   * = v2 + (w2 cross r2) - v2 - (w1 cross r1)
+   * = v2 + (w2 cross (p - x2)) - v2 - (w1 cross(p - x1))
+   * J = [-n	-((p-x1) cross n)	n	((p-x2) cross n)]*/
+  def correctVelocity(h: Double) = {
+    val coefficientOfRestitution = shape1.restitution * shape2.restitution
+    for (p <- points) {
+      val b1 = shape1.body
+      val b2 = shape2.body
+      val relativeNormalVelocity = (b2.velocityOfPoint(p) - b1.velocityOfPoint(p)) dot normal
+      val Cdot = relativeNormalVelocity + relativeNormalVelocity * coefficientOfRestitution 
+      if (Cdot <= 0) {
+        val r1 = p - b1.pos
+        val r2 = p - b2.pos
+        val cr1 = r1 cross normal
+        val cr2 = r2 cross normal
+        val invMass = 1/b1.mass * (normal dot normal) + 1/b1.I * cr1 * cr1 + 1/b2.mass * (normal dot normal) + 1/b2.I * cr2 * cr2
+        val m = if (invMass == 0.0) 0.0 else 1/invMass
+        val lambda = -m * Cdot
+        //wenn fixed, dann ist Masse unendlich => kein 'if (fixed != true)'
+        b1.linearVelocity += -normal * lambda / b1.mass
+        b1.angularVelocity += -(r1 cross normal) * lambda / b1.I  
+        b2.linearVelocity += normal * lambda / b2.mass
+        b2.angularVelocity += (r2 cross normal) * lambda / b2.I
+        correctFriction(p, (-normal * lambda).length / h, h)
+      }
+    }
+  }
+  
+  /* Cdot = vt = [v2 + (w2 cross r2) - v1 - (w2 cross r2)] dot t
+   * J = [-t	-(r2 cross t)	t	(r1 cross t)]
+   * 1/m = J * M * JT
+   * = 1/m1 * (t dot t) + 1/m2 * (t dot t) + 1/I1 * (r1 cross u)^2 + 1/I2 * (r2 cross u)^2*/
+  def correctFriction(point: Vector2D, normalForce: Double, h: Double) = {
+    val b1 = shape1.body
+    val b2 = shape2.body
+    val tangent = normal.leftNormal
+    val Cdot = (b2.velocityOfPoint(point) - b1.velocityOfPoint(point)) dot tangent 
+    val r1 = point - b1.pos
+    val r2 = point - b2.pos
+    val cr1 = r1 cross tangent
+    val cr2 = r2 cross tangent
+    val invMass = 1/b1.mass * (tangent dot tangent) + 1/b1.I * cr1 * cr1 + 1/b2.mass * (tangent dot tangent) + 1/b2.I * cr2 * cr2
+    val m = if (invMass == 0.0) 0.0 else 1/invMass
+    val mu = shape1.friction * shape2.friction
+    val lambda = -m * Cdot
+    val cf = shape1.friction * shape2.friction
+    val cl = Math.min(Math.max(-normalForce * cf * h, lambda), normalForce * cf * h)
+    val impulse = tangent * cl
+    b1.applyImpulse(-impulse, point)
+    b2.applyImpulse(impulse, point)
+  }
+  
+  def correctPosition(h: Double) = {
+    val b1 = shape1.body
+    val b2 = shape2.body
+    
+    for (p <- points) {
+      val overlap = shape1.project(normal) overlap shape2.project(normal)
+      val C = Collision.ToleratedOverlap - overlap
+      if (C <= 0.0) {
+        val r1 = p - b1.pos
+        val r2 = p - b2.pos
+        val cr1 = r1 cross normal
+        val cr2 = r2 cross normal
+        val invMass = 1/b1.mass + 1/b1.I * cr1 * cr1 + 1/b2.mass + 1/b2.I * cr2 * cr2
+        val m = if (invMass == 0.0) 0.0 else 1/invMass
+        val impulse = -normal.unit * m * C
+        b1.pos += -impulse / b1.mass
+        b1.rotation += -(r1 cross impulse) / b1.I
+        b2.pos += impulse / b2.mass
+        b2.rotation += (r2 cross impulse) / b2.I
+      }
+    }
+  }
+}
+
+object Collision {
+  
+  /**Erlaubte Ueberlappung.*/
+  val ToleratedOverlap: Double = 0.01
+}
diff --git a/src/sims/collision/Detector.scala b/src/sims/collision/Detector.scala
new file mode 100644
index 0000000..e847235
--- /dev/null
+++ b/src/sims/collision/Detector.scala
@@ -0,0 +1,23 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.collision
+
+
+import sims.geometry._
+import sims.dynamics._
+import scala.collection._
+import scala.collection.mutable._
+
+/**Eine Welt ermittelt ihre Kollisionen durch konkrete Implementierungen dieser Klasse.*/
+abstract class Detector {
+  
+  /**Die Welt dessen Formen auf Kollisionen ueberprueft werden sollen.*/
+  val world: World
+  
+  /**Ergibt alle Kollisionen zwischen Formen der Welt <code>world</code>.*/
+  def collisions: Seq[Collision]
+}
\ No newline at end of file
diff --git a/src/sims/collision/GridDetector.scala b/src/sims/collision/GridDetector.scala
new file mode 100644
index 0000000..2c027a3
--- /dev/null
+++ b/src/sims/collision/GridDetector.scala
@@ -0,0 +1,122 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.collision
+
+import sims.dynamics._
+import sims.geometry._
+import scala.collection._
+import scala.collection.mutable._
+
+/**Eine konkrete Implementierung von <code>Detector</code>. <code>GridDetector</code> ermittelt
+ * alle Kollisionen mit einem Gittersystem.*/
+class GridDetector(override val world: World) extends Detector {
+  
+  /**Array von Kollisionserkennungsmethoden fuer Formenpaare.*/
+  val detectionMethods = new ArrayBuffer[PartialFunction[(Shape, Shape), Boolean]]
+  detectionMethods += {
+    case (c1: Circle, c2: Circle) => {	//Kollision wenn Distanz <= Summe der Radien
+      val d = (c1.pos - c2.pos).length
+      val rSum = c1.radius + c2.radius
+      d - rSum <= 0
+      }
+    
+    case (p1: ConvexPolygon, p2: ConvexPolygon) => {	//SAT
+      val sides = p1.sides ++ p2.sides
+      val axes = sides map (_.n0)
+      axes.forall((a: Vector2D) => p1.project(a) overlaps p2.project(a))
+    }
+    
+    case (p: ConvexPolygon, c: Circle) => {	//Distanz von Zentrum zu Seiten oder Eckpunkten
+      val distances = for (s <- p.sides) yield (s distance c.pos)
+      distances.exists(_ - c.radius <= 0) || (p contains c.pos)
+      }
+    
+    case (c: Circle, p: ConvexPolygon) => {	//Distanz von Zentrum zu Seiten oder Eckpunkten
+      val distances = for (s <- p.sides) yield (s distance c.pos)
+      distances.exists(_ - c.radius <= 0) || (p contains c.pos)
+      }
+  }
+  
+   /**Array von Kollisionsmethoden fuer Formenpaare.*/
+  val collisionMethods = new ArrayBuffer[PartialFunction[(Shape, Shape), Collision]]
+  collisionMethods += {
+    case (c1: Circle, c2: Circle) => CircleCollision(c1, c2)
+    case (p1: ConvexPolygon, p2: ConvexPolygon) => PolyCollision(p1, p2)
+    case (p: ConvexPolygon, c: Circle) => PolyCircleCollision(p, c)
+    case (c: Circle, p: ConvexPolygon) => PolyCircleCollision(p, c)
+  }
+  
+  /**Gibt an, ob das Formenpaar <code>p</code> kollidiert.
+   * @param p Formenpaar.*/
+  def colliding(p: Pair) = {
+   if (detectionMethods.exists(_.isDefinedAt(p))) 
+     detectionMethods.find(_.isDefinedAt(p)).get.apply(p)
+    else throw new IllegalArgumentException("No collision method for colliding pair!")
+  }
+  
+  /**Gibt die Kollision des Formenpaares <code>p</code> zurueck.
+   * @param p Formenpaar.*/
+  def collision(p: Pair): Collision = {
+    if (collisionMethods.exists(_.isDefinedAt(p)))
+      collisionMethods.find(_.isDefinedAt(p)).get.apply(p)
+    else throw new IllegalArgumentException("No collision found in colliding pair!")
+  }
+  
+  /**Breite und Hoehe einer Gitterzelle.*/
+  var gridSide: Double = 2
+  
+  /**Ergibt potenzielle Kollisionspaare der Welt <code>world</code>.
+   * <p>
+   * Ein Kollisionspaar ist ein Paar aus zwei verschiedenen Formen, das folgenden Bedingungen unterliegt:
+   * <ul>
+   * <li>Die Formen muessen sich in der gleichen Gitterzelle befinden.</li>
+   * <li>Ihre AABBs muessen sich ueberlappen.</li>
+   * <li>Die Formen duerfen nicht von dem gleichen Koerper sein.</li>
+   * <li>Mindestens eine Form darf nicht Fixiert sein.</li>
+   * <li>Beide muessen {@link dynamics.Shape#collidable collidierbar} sein.</li>
+   * </ul>*/
+  def getPairs = {
+    val grid = new HashMap[(Int, Int), List[Shape]]
+    def gridCoordinates(v: Vector2D) = ((v.x / gridSide).toInt, (v.y / gridSide).toInt)
+    def addToGrid(s: Shape) = {
+      val aabb = s.AABB  
+      val minCell = gridCoordinates(aabb.minVertex)
+      val maxCell = gridCoordinates(aabb.maxVertex)
+      val coords = for(i <- (minCell._1 to maxCell._1); j <- (minCell._2 to maxCell._2)) yield (i, j)
+      for (c <- coords) {
+        if (grid.contains(c))
+          {if (grid(c).forall(_ ne s)) grid(c) = s :: grid(c)}
+        else
+            grid += (c -> List(s))
+      	}
+      }
+    for(s <- world.shapes) addToGrid(s)
+    var ps: List[Pair] = Nil
+    for(cell <- grid.values) {
+      ps = ps ::: (for (s1: Shape <- cell; s2: Shape <- cell;
+                    if (s1 ne s2);                       
+           			if (s1.body ne s2.body);
+           			if (s1.collidable && s2.collidable);
+                    if (s1.AABB overlaps s2.AABB);
+                    if (!s1.transientShapes.contains(s2) && !s2.transientShapes.contains(s1))) yield Pair(s1, s2)
+       				).removeDuplicates
+    }
+    ps.toSeq
+  }
+  
+  private var cache = (world.time, getPairs)
+  
+  /**Alle potentiellen Kollisionspaare der Welt.
+   * @see getPairs*/
+  def pairs = {if (world.time != cache._1) cache = (world.time, getPairs); cache._2}
+  
+  /**Ergibt alle kollidierenden Paare.*/
+  def collidingPairs: Seq[Pair] = for(p <- pairs; if (colliding(p))) yield p
+  
+  /**Ergibt alle Kollisionen.*/
+  def collisions: Seq[Collision] = for(p <- pairs; if (colliding(p))) yield collision(p)
+}
diff --git a/src/sims/collision/Overlap.scala b/src/sims/collision/Overlap.scala
new file mode 100644
index 0000000..97ecdd6
--- /dev/null
+++ b/src/sims/collision/Overlap.scala
@@ -0,0 +1,11 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.collision
+
+import sims.geometry._
+
+case class Overlap(poly: ConvexPolygon, sideNum: Int, overlap: Double)
diff --git a/src/sims/collision/Pair.scala b/src/sims/collision/Pair.scala
new file mode 100644
index 0000000..048748d
--- /dev/null
+++ b/src/sims/collision/Pair.scala
@@ -0,0 +1,21 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.collision
+
+import sims.dynamics._
+
+/**Formenpaar.*/
+case class Pair(s1: Shape, s2: Shape) extends Tuple2(s1, s2){
+  def this(t: Tuple2[Shape, Shape]) = this(t._1, t._2)
+  
+  override def equals(other: Any) = { //overriden to prevent removal during "GridDetector.getPairs" 
+    other match {
+      case Pair(a, b) => ((a eq this.s1) && (b eq this.s2)) || ((b eq this.s1) && (a eq this.s2))  
+      case _ => false
+    }
+  }
+}
diff --git a/src/sims/collision/PolyCircleCollision.scala b/src/sims/collision/PolyCircleCollision.scala
new file mode 100644
index 0000000..1bf982e
--- /dev/null
+++ b/src/sims/collision/PolyCircleCollision.scala
@@ -0,0 +1,38 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.collision
+
+import sims.dynamics._
+import sims.geometry._
+
+/**Kollision zwischen einem konvexen Polygon und einem Kreis.*/
+case class PolyCircleCollision(p: ConvexPolygon, c: Circle) extends Collision {
+  require(p.isInstanceOf[Shape])
+  val shape1 = p.asInstanceOf[Shape]
+  val shape2 = c
+  
+  val normal = {
+    //minimum overlap
+    var min = (p.sides(0) distance c.pos) - c.radius
+    var axis = p.sides(0).n0
+    for (s <- p.sides; val delta = (s distance c.pos) - c.radius) if (delta <= 0 && delta < min) {
+      min = delta
+      axis = s.n0
+    }
+    for (v <- p.vertices; val delta = (v - c.pos).length - c.radius) if (delta <= 0 && delta <= min){
+      min = delta
+      axis = (c.pos - v).unit
+    }
+    axis
+  }
+  
+  val points = List(
+    c.pos - normal * c.radius
+  )
+  
+
+}
diff --git a/src/sims/collision/PolyCollision.scala b/src/sims/collision/PolyCollision.scala
new file mode 100644
index 0000000..bbe5568
--- /dev/null
+++ b/src/sims/collision/PolyCollision.scala
@@ -0,0 +1,50 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.collision
+
+import sims.geometry._
+import sims.dynamics._
+import scala.collection.mutable.Map
+import scala.collection.mutable._
+
+/**Kollision zwischen zwei konvexen Polygonen.*/
+case class PolyCollision(p1: ConvexPolygon, p2: ConvexPolygon) extends Collision {
+  require(p1.isInstanceOf[Shape])
+  require(p2.isInstanceOf[Shape])
+  
+  def overlap(axis: Vector2D) = {
+   // println((p1.project(axis) overlap p2.project(axis)).toString + "	to	" +  (p2.project(axis) overlap p1.project(axis)))
+    p1.project(axis) overlap p2.project(axis)
+  }
+  
+  lazy val overlaps = (for (i <- 0 until p2.sides.length) yield Overlap(p2, i, overlap(p2.sides(i).n0))) ++ 
+    			 (for (i <- 0 until p1.sides.length) yield Overlap(p1, i, overlap(p1.sides(i).n0)))
+  
+  private var potMinOverlap = overlaps.find(_.overlap > 0.0)
+  require(potMinOverlap != None)
+  private var _minOverlap: Overlap = potMinOverlap.get
+  var minOverlap: Overlap = { 
+    for (o <- overlaps) if ((o.overlap < _minOverlap.overlap) && (o.overlap > 0.0)) _minOverlap = o
+    _minOverlap
+  }
+  
+  
+  private lazy val refPoly = minOverlap.poly
+  private lazy val incPoly = if (minOverlap.poly eq p1) p2 else p1
+  
+  lazy val shape1 = refPoly.asInstanceOf[Shape]
+  lazy val shape2 = incPoly.asInstanceOf[Shape]
+  
+  lazy val normal = refPoly.sides(minOverlap.sideNum).n0
+  lazy val points = (for (v <- incPoly.vertices; if refPoly.contains(v)) yield v) ++
+    (for (s <- incPoly.sides;
+    val clip = s.clipToSegment(refPoly.sides((refPoly.sides.length - (minOverlap.sideNum + 1)) % refPoly.sides.length));
+    if (clip != None)) yield clip.get) ++ 
+    (for (s <- incPoly.sides;
+    val clip = s.clipToSegment(refPoly.sides((refPoly.sides.length - (minOverlap.sideNum - 1)) % refPoly.sides.length));
+    if (clip != None)) yield clip.get)
+}