diff options
Diffstat (limited to 'src/main/scala/sims/dynamics/joints')
7 files changed, 390 insertions, 0 deletions
diff --git a/src/main/scala/sims/dynamics/joints/DistanceJoint.scala b/src/main/scala/sims/dynamics/joints/DistanceJoint.scala new file mode 100644 index 0000000..1bf9b46 --- /dev/null +++ b/src/main/scala/sims/dynamics/joints/DistanceJoint.scala @@ -0,0 +1,77 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics.joints + +import sims.dynamics._ +import sims.geometry._ + +/** DistanceJoints keep their connection points at a constant distance. + * @param node1 first associated body + * @param anchor1 first connection point + * @param node2 second associated body + * @param anchor2 second connection point*/ +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) + + /**Distance between the two connection points at initialisation (the desired distance).*/ + 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 + + /**Returns the connection point on body one (in world coordinates).*/ + def connection1 = (a1 rotate (node1.rotation - initRotation1)) + node1.pos + + /**Returns the connection point on body two (in world coordinates).*/ + def connection2 = (a2 rotate (node2.rotation - initRotation2)) + node2.pos + + /**Relative position of the connection points.*/ + def x = connection2 - connection1 + + /**Relative velocity of the connection points.*/ + 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 //relative position + val v = this.v //relative velocity + val r1 = (connection1 - node1.pos) + val r2 = (connection2 - node2.pos) + val cr1 = r1 cross x.unit + val cr2 = r2 cross x.unit + 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 //avoid division by zero + 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/main/scala/sims/dynamics/joints/ForceJoint.scala b/src/main/scala/sims/dynamics/joints/ForceJoint.scala new file mode 100644 index 0000000..2074ee4 --- /dev/null +++ b/src/main/scala/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 + +/**A joint which can apply a force to its anchor bodies, thus adding or removing energy to the system.*/ +trait ForceJoint { + + /**Applies a force on the anchor bodies.*/ + def applyForce(): Unit +} diff --git a/src/main/scala/sims/dynamics/joints/Joint.scala b/src/main/scala/sims/dynamics/joints/Joint.scala new file mode 100644 index 0000000..652df97 --- /dev/null +++ b/src/main/scala/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 constrain the movement of two bodies. + * Their implementation was inspired by Erin Catto's box2d.*/ +abstract class Joint extends Constraint{ + + /**First body of the joint.*/ + val node1: Body + + /**Second body of the joint.*/ + val node2: Body + + /**Corrects the velocities of this joint's associated bodies.*/ + def correctVelocity(h: Double): Unit + + /**Corrects the positions of this joint's associated bodies.*/ + def correctPosition(h: Double): Unit +}
\ No newline at end of file diff --git a/src/main/scala/sims/dynamics/joints/RevoluteJoint.scala b/src/main/scala/sims/dynamics/joints/RevoluteJoint.scala new file mode 100644 index 0000000..bf6f05e --- /dev/null +++ b/src/main/scala/sims/dynamics/joints/RevoluteJoint.scala @@ -0,0 +1,57 @@ +/* + * 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._ + +/**A revolute joint that connects two bodies at a singe point. Inspired from JBox2D. + * <b>Warning:</b> there are still several bugs with revolute joints, if they are between two free + * bodies and not connected at their respective COMs.*/ +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/main/scala/sims/dynamics/joints/SpringJoint.scala b/src/main/scala/sims/dynamics/joints/SpringJoint.scala new file mode 100644 index 0000000..10d5afb --- /dev/null +++ b/src/main/scala/sims/dynamics/joints/SpringJoint.scala @@ -0,0 +1,85 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics.joints + +import sims.dynamics._ +import sims.geometry._ + +/**A spring obeying Hooke's law. + * @param node1 first associated body + * @param anchor1 first connection point + * @param node2 second associated body + * @param anchor2 second connection point + * @param springConstant spring constant + * @param initialLength initial length + */ +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 + + /**Returns the connection point on body one (in world coordinates).*/ + def connection1 = (a1 rotate (node1.rotation - initRotation1)) + node1.pos + + /**Returns the connection point on body two (in world coordinates).*/ + def connection2 = (a2 rotate (node2.rotation - initRotation2)) + node2.pos + + /**Damping.*/ + var damping = 0.0 + + /**Relative position of the connection points.*/ + def x = connection2 - connection1 + + /**Relative velocity of the connection points.*/ + def v = node2.velocityOfPoint(connection2) - node1.velocityOfPoint(connection1) + + /**Returns the spring force.*/ + def force = (x.length - initialLength) * springConstant + + /**Applies the spring force to the connection points.*/ + def applyForce() = { + 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 correctVelocity(h: Double) = { + /* + val x = this.x + val v = this.v + val r1 = (connection1 - node1.pos) + val r2 = (connection2 - node2.pos) + val cr1 = r1 cross x.unit + val cr2 = r2 cross x.unit + val Cdot = x.unit dot v + 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 + 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) = { + + } +} 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) = {} +} |