6 files changed, 277 insertions, 0 deletions
diff --git a/src/sims/dynamics/joints/DistanceJoint.scala b/src/sims/dynamics/joints/DistanceJoint.scala
new file mode 100644
index 0000000..2d5633f
--- /dev/null
+++ b/src/sims/dynamics/joints/DistanceJoint.scala
@@ -0,0 +1,76 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.dynamics.joints
+
+import sims.geometry._
+
+/** DistanceJoints halten die Bindungspunkte auf ihren Bindungskoerpern bei einem konstanten Abstand.
+ * @param node1 erster Koerper der Verbindung
+ * @param anchor1 Bindungspunkt auf Koerper eins
+ * @param node2 zweiter Koerper der Verbindung
+ * @param anchor2 Bindungspunkt auf Koerper zwei*/
+case class DistanceJoint(node1: Body, anchor1: Vector2D, node2: Body, anchor2: Vector2D) extends  Joint{
+  def this(node1: Body, node2: Body) = this(node1, node1.pos, node2, node2.pos)
+  
+  /**Abstand der beiden Bindungspunkte bei initialisierung (der gewollte Abstand).*/
+  val distance = (anchor2 - anchor1).length
+  
+  private val a1 = anchor1 - node1.pos
+  private val a2 = anchor2 - node2.pos
+  private val initRotation1 = node1.rotation
+  private val initRotation2 = node2.rotation
+  
+  /**Ergibt den Bindungspunkt auf Koerper eins.*/
+  def connection1 = (a1 rotate (node1.rotation - initRotation1)) + node1.pos
+  
+  /**Ergibt den Bindungspunkt auf Koerper zwei.*/
+  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)
+  
+  /* x = connection2 - connection1
+     * C = ||x|| - L
+     * u = x / ||x||
+     * v = v2 + w2 cross r2 - v1 - w1 cross r1
+     * Cdot = u dot v
+     * J = [-u	-(r1 cross u)	u	(r2 cross u)]
+     * 1/m = J * M^-1 * JT
+     * = 1/m1 * u * u + 1/m2 * u * u + 1/I1 * (r1 cross u)^2 + 1/I2 * (r2 cross u)^2*/
+  override 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 = -m * Cdot	//=-JV/JM^-1JT
+    val impulse = x.unit * lambda	//P=J lambda
+    node1.applyImpulse(-impulse, connection1)
+    node2.applyImpulse(impulse, connection2) 
+  }
+  
+  override def correctPosition(h: Double) = {
+    val C = x.length - distance
+    val cr1 = (connection1 - node1.pos) cross x.unit
+    val cr2 = (connection2 - node2.pos) cross x.unit
+    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 = -x.unit * m * C
+    node1.pos -= impulse / node1.mass
+    node2.pos += impulse / node2.mass
+    node1.rotation -= ((connection1 - node1.pos) cross impulse) / node1.I
+    node2.rotation += ((connection2 - node2.pos) cross impulse) / node2.I
+  }
+  
+}
+\ No newline at end of file
diff --git a/src/sims/dynamics/joints/ForceJoint.scala b/src/sims/dynamics/joints/ForceJoint.scala
new file mode 100644
index 0000000..fa17eac
--- /dev/null
+++ b/src/sims/dynamics/joints/ForceJoint.scala
@@ -0,0 +1,14 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.dynamics.joints
+
+/**Eine Verbindung die Kraft auf ihre Bindungskoerper ausueben kann.*/
+trait ForceJoint {
+  
+  /**Uebt eine Kraft auf die Bindungskoerper aus.*/
+  def applyForce(): Unit
+}
diff --git a/src/sims/dynamics/joints/Joint.scala b/src/sims/dynamics/joints/Joint.scala
new file mode 100644
index 0000000..9690af2
--- /dev/null
+++ b/src/sims/dynamics/joints/Joint.scala
@@ -0,0 +1,27 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.dynamics.joints
+
+import sims.geometry._
+import sims.dynamics._
+
+/**Joints sind Verbindungen die die Bewegung zwischen zwei Koerpern einschraenken.
+ * Ihre Implementierung wurde von Erin Catto's box2d inspiriert.*/
+abstract class Joint extends Constraint{
+  
+  /**Erster Koerper der Verbindung.*/
+  val node1: Body
+  
+  /**Zweiter Koerper der Verbindung.*/
+  val node2: Body
+  
+  /**Korrigiert die Geschwindigkeit der Koerper damit diese den Randbedingungen der Verbindung entsprechen.*/
+  def correctVelocity(h: Double): Unit
+  
+  /**Korrigiert die Position der Koerper damit diese den Randbedingungen der Verbindung entsprechen.*/
+  def correctPosition(h: Double): Unit
+}
+\ No newline at end of file
diff --git a/src/sims/dynamics/joints/RevoluteJoint.scala b/src/sims/dynamics/joints/RevoluteJoint.scala
new file mode 100644
index 0000000..7a7ae1c
--- /dev/null
+++ b/src/sims/dynamics/joints/RevoluteJoint.scala
@@ -0,0 +1,55 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.dynamics.joints
+
+import sims.geometry._
+import sims.math._
+import sims.dynamics._
+import Math._
+
+/**Ein Gelenk, dass zwei Koerper an einem Punkt verbindet. Inspiriert von JBox2D.*/
+case class RevoluteJoint(node1: Body, node2: Body, anchor: Vector2D) extends Joint{
+  private val a1 = anchor - node1.pos
+  private val a2 = anchor - 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
+  
+  def x = connection2 - connection1
+  def v = node2.velocityOfPoint(connection2) - node1.velocityOfPoint(connection1)
+  
+  /* x = connection2 - connection1
+   * C = x
+   * Cdot = v = v2 - v1 = v2 + (w2 cross r2) - v1 - (w1 cross r1)
+   * J = [-I -r1_skew I r2_skew ] ?????
+   */
+  def correctVelocity(h: Double) = {
+    val m1 = node1.mass
+    val m2 = node2.mass
+    val I1 = node1.I
+    val I2 = node2.I
+    val r1 = connection1 - node1.pos
+    val r2 = connection2 - node2.pos
+   
+    val K1 = new Matrix22(1/m1 + 1/m2,	0,
+                          0,			1/m1 + 1/m2)
+    val K2 = new Matrix22(1/I1 * r1.x * r1.x,	-1/I1 * r1.x * r1.y,
+                          -1/I1 * r1.x * r1.y,	1/I1 * r1.x * r1.x)
+    val K3 = new Matrix22(1/I2 * r2.x * r2.x,	-1/I2 * r2.x * r2.y,
+                          -1/I2 * r2.x * r2.y,	1/I2 * r2.x * r2.x)
+    val pivotMass = (K1 + K2 + K3).invert
+    val cdot = v
+    val p = pivotMass * cdot
+    node1.applyImpulse(p, connection1)
+    node2.applyImpulse(-p, connection2)
+  }
+  
+  def correctPosition(h: Double) = {
+    
+  }
+}
diff --git a/src/sims/dynamics/joints/SpringJoint.scala b/src/sims/dynamics/joints/SpringJoint.scala
new file mode 100644
index 0000000..f03b35d
--- /dev/null
+++ b/src/sims/dynamics/joints/SpringJoint.scala
@@ -0,0 +1,60 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.dynamics.joints
+
+import sims.geometry._
+
+/**Eine Hooksche Feder.
+ * @param node1 erster Koerper der Verbindung
+ * @param anchor1 Bindungspunkt auf Koerper eins
+ * @param node2 zweiter Koerper der Verbindung
+ * @param anchor2 Bindungspunkt auf Koerper zwei
+ * @param springConstant Federkonstante
+ * @param initialLength Initiallaenge
+ */
+case class SpringJoint(node1: Body, anchor1: Vector2D, node2: Body, anchor2: Vector2D, springConstant: Double, initialLength: Double) extends Joint with ForceJoint{
+  
+  def this(node1: Body, anchor1: Vector2D, node2: Body, anchor2: Vector2D, springConstant: Double) = {
+    this(node1: Body, anchor1, node2: Body, anchor2, springConstant: Double, (anchor2 - anchor1).length)
+  }
+  
+  def this(node1: Body, node2: Body, springConstant: Double, initialLength: Double) = {
+    this(node1: Body, node1.pos, node2: Body, node2.pos, springConstant: Double, initialLength: Double)
+  }
+  def this(node1: Body, node2: Body, springConstant: Double) = {
+    this(node1: Body, node1.pos, node2: Body, node2.pos, springConstant: Double, (node2.pos - node1.pos).length)
+  }
+  
+  private val a1 = anchor1 - node1.pos
+  private val a2 = anchor2 - node2.pos
+  private val initRotation1 = node1.rotation
+  private val initRotation2 = node2.rotation
+  
+  /**Ergibt den Bindungspunkt auf Koerper eins.*/
+  def connection1 = (a1 rotate (node1.rotation - initRotation1)) + node1.pos
+  
+  /**Ergibt den Bindungspunkt auf Koerper zwei.*/
+  def connection2 = (a2 rotate (node2.rotation - initRotation2)) + node2.pos
+  
+  /**Daempfung.*/
+  var damping = 0.0
+  
+  /**Relative Position der Bindungspunkte.*/
+  def x = connection2 - 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)
+  }
+  
+  def correctPosition(h: Double) = ()
+  def correctVelocity(h: Double) = ()
+}
+\ No newline at end of file
diff --git a/src/sims/dynamics/joints/test/UnitCircleJoint.scala b/src/sims/dynamics/joints/test/UnitCircleJoint.scala
new file mode 100644
index 0000000..64f613c
--- /dev/null
+++ b/src/sims/dynamics/joints/test/UnitCircleJoint.scala
@@ -0,0 +1,45 @@
+/*
+ * Simple Mechanics Simulator (SiMS)
+ * copyright (c) 2009 Jakob Odersky
+ * made available under the MIT License
+*/
+
+package sims.dynamics.joints.test
+
+import sims.dynamics._
+import sims.geometry._
+
+class UnitCircleJoint(body: Body, anchor: Vector2D) extends Joint{
+  
+  val node1 = body
+  val node2 = body
+  
+  private val a = anchor - body.pos
+  private val initRotation = body.rotation
+  def connection = (a rotate (body.rotation - initRotation)) + body.pos
+  def x = connection
+  def v = body.velocityOfPoint(connection)
+
+  /*
+   * C = ||x|| - 1
+   * Cdot = x/||x|| dot v = u dot v
+   * J = [u	(r cross u)]
+   */
+  def correctVelocity(h: Double) = {
+    val r = connection - body.pos
+    val u = x.unit
+    val cr = r cross u
+    val mc = 1.0/(1/body.mass + 1/body.I * cr * cr)
+    val lambda = -mc * (u dot v)
+    val Pc = u * lambda
+    
+    val vupdate = u * lambda / body.mass
+    val wupdate = (r cross u) * lambda / body.I
+    
+    println("dv = " + vupdate + "	dw = " + wupdate)
+    body.linearVelocity = body.linearVelocity +  u * lambda / body.mass
+    body.angularVelocity = body.angularVelocity + (r cross u) * lambda / body.I 
+  }
+  
+  def correctPosition(h: Double) = {}
+}