2 files changed, 130 insertions, 0 deletions
diff --git a/src/main/scala/sims/dynamics/joints/test/PrismaticJoint.scala b/src/main/scala/sims/dynamics/joints/test/PrismaticJoint.scala
new file mode 100644
index 0000000..040647d
--- /dev/null
+++ b/src/main/scala/sims/dynamics/joints/test/PrismaticJoint.scala
@@ -0,0 +1,84 @@
+package sims.dynamics.joints.test
+
+import sims.dynamics._
+import sims.dynamics.joints._
+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
+  
+  def x = connection2 - connection1
+  
+  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
+    val n0 = x.leftNormal
+    val v = this.v
+    val r1 = (connection1 - node1.pos)
+    val r2 = (connection2 - node2.pos)
+    val cr1 = r1 cross n0
+    val cr2 = r2 cross n0
+    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
+    val n0 = x.leftNormal
+    val v = this.v
+    val r1 = (connection1 - node1.pos)
+    val r2 = (connection2 - node2.pos)
+    val cr1 = r1 cross n0
+    val cr2 = r2 cross n0
+    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/main/scala/sims/dynamics/joints/test/UnitCircleJoint.scala b/src/main/scala/sims/dynamics/joints/test/UnitCircleJoint.scala
new file mode 100644
index 0000000..09e72d9
--- /dev/null
+++ b/src/main/scala/sims/dynamics/joints/test/UnitCircleJoint.scala
@@ -0,0 +1,46 @@
+/*
+ * 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.dynamics.joints._
+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) = {}
+}