8 files changed, 136 insertions, 7 deletions
diff --git a/src/sims/collision/PolyCollision.scala b/src/sims/collision/PolyCollision.scala
index bbe5568..3eeb7ca 100644
--- a/src/sims/collision/PolyCollision.scala
+++ b/src/sims/collision/PolyCollision.scala
@@ -40,11 +40,15 @@ case class PolyCollision(p1: ConvexPolygon, p2: ConvexPolygon) extends Collision
   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) ++
+  lazy val points = (for (v <- incPoly.vertices; if refPoly.contains(v)) yield v)++
+    (for (v <- refPoly.vertices; if incPoly.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)
+  */
 }
diff --git a/src/sims/dynamics/World.scala b/src/sims/dynamics/World.scala
index d7ac8ae..7b165f5 100644
--- a/src/sims/dynamics/World.scala
+++ b/src/sims/dynamics/World.scala
@@ -67,7 +67,7 @@ class World {
     for (j <- p.joints) this += j
   }
   
-  def ++=(bs: Seq[Body]) = for(b <- bs) this += b
+  def ++=(bs: Seq[Body]): Unit = for(b <- bs) this += b
   
   /**Entfernt den gegebenen Koerper aus dieser Welt.*/
   def -=(body: Body): Unit = bodies -= body
diff --git a/src/sims/dynamics/joints/SpringJoint.scala b/src/sims/dynamics/joints/SpringJoint.scala
index f03b35d..67ea57f 100644
--- a/src/sims/dynamics/joints/SpringJoint.scala
+++ b/src/sims/dynamics/joints/SpringJoint.scala
@@ -46,15 +46,39 @@ case class SpringJoint(node1: Body, anchor1: Vector2D, node2: Body, anchor2: Vec
   /**Relative Position der Bindungspunkte.*/
   def x = connection2 - connection1
   
+  /**Relative Geschwindigkeit der Bindungspunkte.*/
+  def v = node2.velocityOfPoint(connection2) - node1.velocityOfPoint(connection1)
+  
   /**Ergibt die Federkraft nach dem Hookschen Gesetz.*/
   def force = (x.length - initialLength) * springConstant
   
   /**Uebt die Federkraft auf die Bindungspunkte aus.*/
   def applyForce() = {
-    node1.applyForce(x.unit * force - ((node1 velocityOfPoint connection1) * damping) project x, connection1)
-    node2.applyForce(-x.unit * force - ((node2 velocityOfPoint connection2) * damping) project x, connection2)
+    node1.applyForce(x.unit * force - (v * damping) project x, connection1)
+    node2.applyForce(-x.unit * force - (v * damping) project x, connection2)
+    //println("this should not happen")
   }
   
-  def correctPosition(h: Double) = ()
-  def correctVelocity(h: Double) = ()
+  def correctVelocity(h: Double) = {
+    /*
+    val x = this.x	//relativer Abstand
+    val v = this.v	//relative Geschwindigkeit
+    val r1 = (connection1 - node1.pos)	//Abstand Punkt-Schwerpunkt, Koerper 1
+    val r2 = (connection2 - node2.pos)	//Abstand Punkt-Schwerpunkt, Koerper 2
+    val cr1 = r1 cross x.unit	//Kreuzprodukt
+    val cr2 = r2 cross x.unit	//Kreuzprodukt
+    val Cdot = x.unit dot v	//Velocity-Constraint
+    val invMass = 1/node1.mass + 1/node1.I * cr1 * cr1 + 1/node2.mass + 1/node2.I * cr2 * cr2	//=J M^-1 JT
+    val m = if (invMass == 0.0) 0.0 else 1/invMass	//Test um Nulldivision zu vermeiden
+    val lambda = Math.min(Math.max(-this.force * h, (-m * Cdot)), this.force * h)
+    println (force * h, -m * Cdot)
+    val impulse = x.unit * lambda
+    node1.applyImpulse(-impulse, connection1)
+    node2.applyImpulse(impulse, connection2)
+    */
+  }
+
+  def correctPosition(h: Double) = {
+    
+  }
 }
 \ No newline at end of file
diff --git a/src/sims/dynamics/joints/test/PrismaticJoint.scala b/src/sims/dynamics/joints/test/PrismaticJoint.scala
new file mode 100644
index 0000000..f163261
--- /dev/null
+++ b/src/sims/dynamics/joints/test/PrismaticJoint.scala
@@ -0,0 +1,84 @@
+package sims.dynamics.joints.test
+
+import sims.geometry._
+
+case class PrismaticJoint(node1: Body, anchor1: Vector2D, node2: Body, anchor2: Vector2D) extends Joint{
+  def this(node1: Body, node2: Body) = this(node1, node1.pos, node2, node2.pos)
+  
+  val angle = node2.rotation - node1.rotation
+  
+  private val a1 = anchor1 - node1.pos
+  private val a2 = anchor2 - node2.pos
+  private val initRotation1 = node1.rotation
+  private val initRotation2 = node2.rotation
+  
+  def connection1 = (a1 rotate (node1.rotation - initRotation1)) + node1.pos
+  def connection2 = (a2 rotate (node2.rotation - initRotation2)) + node2.pos
+  
+  /**Relative Position der Bindungspunkte.*/
+  def x = connection2 - connection1
+  
+  /**Relative Geschwindigkeit der Bindungspunkte.*/
+  def v = node2.velocityOfPoint(connection2) - node1.velocityOfPoint(connection1)
+  
+  
+  def correctVelocity(h: Double) = {
+    correctLinear(h)
+    //correctAngular(h)
+  }
+  
+  def correctLinear(h: Double) = {
+    val x = this.x.unit	//relativer Abstand
+    val n0 = x.leftNormal
+    val v = this.v	//relative Geschwindigkeit
+    val r1 = (connection1 - node1.pos)	//Abstand Punkt-Schwerpunkt, Koerper 1
+    val r2 = (connection2 - node2.pos)	//Abstand Punkt-Schwerpunkt, Koerper 2
+    val cr1 = r1 cross n0	//Kreuzprodukt
+    val cr2 = r2 cross n0	//Kreuzprodukt
+    val Cdot = n0 dot v
+    val invMass = 1/node1.mass + 1/node1.I * cr1 * cr1 + 1/node2.mass + 1/node2.I * cr2 * cr2
+    val m = if (invMass == 0.0) 0.0 else 1/invMass
+    val impulse = -n0 * m * Cdot
+    node1.applyImpulse(-impulse, connection1)
+    node2.applyImpulse(impulse, connection2)
+  }
+  
+  //J=[-1,1]
+  
+  def correctAngular(h: Double) = {
+    val w = node2.angularVelocity - node1.angularVelocity
+    val Cdot = w
+    val invMass = node1.I + node2.I
+    val m = 1 / invMass
+    val lambda = m * Cdot
+    node1.angularVelocity += lambda / node1.I
+    node2.angularVelocity += -lambda / node2.I
+  }
+  
+  def correctPosition(h: Double) = {
+    /*
+    val x = this.x.unit	//relativer Abstand
+    val n0 = x.leftNormal
+    val v = this.v	//relative Geschwindigkeit
+    val r1 = (connection1 - node1.pos)	//Abstand Punkt-Schwerpunkt, Koerper 1
+    val r2 = (connection2 - node2.pos)	//Abstand Punkt-Schwerpunkt, Koerper 2
+    val cr1 = r1 cross n0	//Kreuzprodukt
+    val cr2 = r2 cross n0	//Kreuzprodukt
+    val C = n0 dot x
+    val invMass = 1/node1.mass + 1/node1.I * cr1 * cr1 + 1/node2.mass + 1/node2.I * cr2 * cr2
+    val m = if (invMass == 0.0) 0.0 else 1/invMass
+    val impulse = -n0 * m * C
+    node1.pos += -impulse
+    node1.rotation += -impulse cross r1
+    node2.pos += impulse
+    node2.rotation += impulse cross r2
+    
+    val relOmega = node2.angularVelocity - node2.angularVelocity
+    val invMassOmega = node1.I + node2.I
+    val mOmega = if (invMassOmega == 0.0) 0.0 else 1/invMassOmega
+    val Crot = node2.rotation - node2.rotation + angle 
+    node1.rotation += mOmega * Crot
+    node2.rotation += -mOmega * Crot
+      */
+  }
+}
diff --git a/src/sims/materials/Material.scala b/src/sims/materials/Material.scala
new file mode 100644
index 0000000..b05e082
--- /dev/null
+++ b/src/sims/materials/Material.scala
@@ -0,0 +1,7 @@
+package sims.materials
+
+trait Material {
+  val density: Double
+  val restitution: Double
+  val friction: Double
+}
diff --git a/src/sims/materials/Rubber.scala b/src/sims/materials/Rubber.scala
new file mode 100644
index 0000000..b408d2d
--- /dev/null
+++ b/src/sims/materials/Rubber.scala
@@ -0,0 +1,5 @@
+package sims.materials
+
+object Rubber {
+
+}
diff --git a/src/sims/materials/Steel.scala b/src/sims/materials/Steel.scala
new file mode 100644
index 0000000..71594b8
--- /dev/null
+++ b/src/sims/materials/Steel.scala
@@ -0,0 +1,5 @@
+package sims.materials
+
+object Steel {
+
+}
diff --git a/src/sims/prefabs/Ragdoll.scala b/src/sims/prefabs/Ragdoll.scala
index ad58653..00e4b8d 100644
--- a/src/sims/prefabs/Ragdoll.scala
+++ b/src/sims/prefabs/Ragdoll.scala
@@ -38,6 +38,6 @@ class Ragdoll(position: Vector2D) extends Prefab {
   
   val shoulder1 = RevoluteJoint(headTorso, upperArm1, position + Vector2D(0, -0.16))
   val shoulder2 = RevoluteJoint(headTorso, upperArm2, position + Vector2D(0, -0.16))
-  override val joints = List(shoulder1, shoulder2)
+  override val joints = Nil //List(shoulder1, shoulder2)
 
 }