diff options
author | Jakob Odersky <jodersky@gmail.com> | 2009-11-05 21:02:40 +0000 |
---|---|---|
committer | Jakob Odersky <jodersky@gmail.com> | 2009-11-05 21:02:40 +0000 |
commit | 9d20024aa35cd7f923ebfc1ed9a2ffbf2731da70 (patch) | |
tree | 4e97fadc391b310ee1cc7156fda590dff414b2c3 /src/sims/dynamics/joints/test/UnitCircleJoint.scala | |
parent | 034bc5930ea6f01745f64a6070711d899d2c27ae (diff) | |
download | sims-9d20024aa35cd7f923ebfc1ed9a2ffbf2731da70.tar.gz sims-9d20024aa35cd7f923ebfc1ed9a2ffbf2731da70.tar.bz2 sims-9d20024aa35cd7f923ebfc1ed9a2ffbf2731da70.zip |
Initial import.
Diffstat (limited to 'src/sims/dynamics/joints/test/UnitCircleJoint.scala')
-rw-r--r-- | src/sims/dynamics/joints/test/UnitCircleJoint.scala | 45 |
1 files changed, 45 insertions, 0 deletions
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) = {} +} |