diff options
Diffstat (limited to 'src/main/scala/sims/dynamics')
| -rw-r--r-- | src/main/scala/sims/dynamics/Body.scala | 141 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/Circle.scala | 34 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/Constraint.scala | 21 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/Rectangle.scala | 38 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/RegularPolygon.scala | 35 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/Shape.scala | 97 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/World.scala | 164 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/joints/DistanceJoint.scala | 77 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/joints/ForceJoint.scala | 14 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/joints/Joint.scala | 27 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/joints/RevoluteJoint.scala | 57 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/joints/SpringJoint.scala | 85 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/joints/test/PrismaticJoint.scala | 84 | ||||
| -rw-r--r-- | src/main/scala/sims/dynamics/joints/test/UnitCircleJoint.scala | 46 |
14 files changed, 920 insertions, 0 deletions
diff --git a/src/main/scala/sims/dynamics/Body.scala b/src/main/scala/sims/dynamics/Body.scala new file mode 100644 index 0000000..8c0e2ee --- /dev/null +++ b/src/main/scala/sims/dynamics/Body.scala @@ -0,0 +1,141 @@ + +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +import sims.geometry._ +import sims.dynamics.joints._ + +/**A two dimensional rigid body is made out of shapes. + * @param shps shapes that belong to this body.*/ +class Body(shps: Shape*){ + + /**Unique identification number.*/ + val uid = Body.nextUid + + /**Shapes that belong to this body.*/ + val shapes: List[Shape] = shps.toList + + //Shapes are added during initialisation. + for (s <- shapes) { + s.body = this + s.refLocalPos = s.pos - pos + s.rotation0 = s.rotation + } + + private var isFixed: Boolean = false + + /**Returns whether this body is fixed or not.*/ + def fixed = isFixed + + /**Fixes or frees this body. By fixing, linear and angular velocities are set to zero.*/ + def fixed_=(value: Boolean) = { + if (value) {linearVelocity = Vector2D.Null; angularVelocity = 0.0} + isFixed = value + } + + /**Flag for a world to monitor the properties of this body. + * @see World#monitors*/ + var monitor: Boolean = false + + /**Returns the position of this body. The position is equivalent to the center of mass. + * @return position of this body*/ + def pos: Vector2D = // COM = sum(pos*mass)/M + (Vector2D.Null /: shapes)((v: Vector2D, s: Shape) => v + s.pos * s.mass) / + (0.0 /: shapes)((i: Double, s: Shape) => i + s.mass) + + /**Sets the position of this body. By doing so all its shapes are translated. + * @param newPos new position*/ + def pos_=(newPos: Vector2D) = { + val stepPos = pos + shapes.foreach((s: Shape) => s.pos = s.pos - stepPos + newPos) + } + + /**Contains the current rotation of this body.*/ + private var _rotation: Double = 0.0 + + /**Returns the current rotation of this body.*/ + def rotation: Double = _rotation + + /**Sets the rotation of this body. Position and rotation of shapes are modified accordingly. + * @param r new rotation*/ + def rotation_=(newRotation: Double) = { + _rotation = newRotation + val stepPos = pos + for (s <- shapes) { + s.rotation = newRotation + s.rotation0 + s.pos = stepPos + (s.refLocalPos rotate (newRotation)) + } + } + + /**Linear velocity of this body.*/ + var linearVelocity: Vector2D = Vector2D.Null + + /**Angular velocity of this body.*/ + var angularVelocity: Double = 0 + + /**Linear velocity of the given point on this body (in world coordinates).*/ + def velocityOfPoint(point: Vector2D) = linearVelocity + ((point - pos).leftNormal * angularVelocity) + + /**Resulting force on the COM of this body.*/ + var force: Vector2D = Vector2D.Null + + /**Resulting torque on this body.*/ + var torque: Double = 0 + + /**Returns the mass of this body. If the body is free, its mass is the sum of the masses of its shapes. + * If the body is fixed, its mass is infinite (<code>Double.PositiveInfinity</code>). + * @return this body's mass*/ + def mass: Double = if (fixed) Double.PositiveInfinity else (0.0 /: shapes)((i: Double, s: Shape) => i + s.mass) + + /**Returns the moment of inertia for rotations about the COM of this body. + * It is calculated using the moments of inertia of this body's shapes and the parallel axis theorem. + * If the body is fixed, its moment of inertia is infinite (<code>Double.PositiveInfinity</code>). + * @return moment of inertia for rotations about the COM of this body*/ + def I: Double = if (fixed) Double.PositiveInfinity else + (0.0 /: (for (s <- shapes) yield (s.I + s.mass * ((s.pos - pos) dot (s.pos - pos)))))(_+_) + + /**Applies a force to the COM of this body. + * @param force applied force*/ + def applyForce(force: Vector2D) = if (!fixed) this.force += force + + /**Applies a force to a point on this body. Warning: the point is considered to be contained within this body. + * @param force applied force + * @param point position vector of the point (in world coordinates)*/ + def applyForce(force: Vector2D, point: Vector2D) = if (!fixed) {this.force += force; torque += (point - pos) cross force} + + /**Applies an impulse to the COM of this body. + * @param impulse applied impulse*/ + def applyImpulse(impulse: Vector2D) = if (!fixed) linearVelocity += impulse / mass + + /**Applies an impulse to a point on this body. Warning: the point is considered to be contained within this body. + * @param impulse applied impulse + * @param point position vector of the point (in world coordinates)*/ + def applyImpulse(impulse: Vector2D, point: Vector2D) = if (!fixed) {linearVelocity += impulse / mass; angularVelocity += ((point - pos) cross impulse) / I} + + /**Checks if the point <code>point</code> is contained in this body.*/ + def contains(point: Vector2D) = shapes.exists(_.contains(point)) + + override def toString: String = { + "Body" + uid + " " + shapes + " fixed=" + fixed + " m=" + mass + " I=" + I + " pos=" + pos + " rot=" + rotation + " v=" + linearVelocity + " w=" + angularVelocity + " F=" + force + " tau=" + torque + } + + /**Creates a new body containing this body's shapes and the shape <code>s</code>. + * @param s new shape + * @return new body*/ + def ~(s: Shape) = new Body((s :: shapes): _*) + + /**Creates a new body containing this body's shapes and the shapes of another body <code>b</code>. + * @param b body with extra shapes + * @return new body*/ + def ~(b: Body) = new Body((this.shapes ::: b.shapes): _*) +} + +object Body { + private var uidCounter = -1 + private def nextUid = {uidCounter += 1; uidCounter} +}
\ No newline at end of file diff --git a/src/main/scala/sims/dynamics/Circle.scala b/src/main/scala/sims/dynamics/Circle.scala new file mode 100644 index 0000000..d6db07d --- /dev/null +++ b/src/main/scala/sims/dynamics/Circle.scala @@ -0,0 +1,34 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +import sims.geometry._ +import sims.collision._ + +/** + * A circle. + * @param radius radius of this circle + * @param density density of this circle + */ +case class Circle(radius: Double, density: Double) extends Shape{ + + val volume = math.Pi * radius * radius + + val I = mass * radius * radius / 2 + + def AABB = new AABB(pos - Vector2D(radius,radius), + pos + Vector2D(radius,radius)) + + def project(axis: Vector2D) = if (axis.x != 0) Projection(axis, + (pos.project(axis).x / axis.x) - radius, + (pos.project(axis).x / axis.x) + radius) + else Projection(axis, + (pos.project(axis).y / axis.y) - radius, + (pos.project(axis).y / axis.y) + radius) + + def contains(point: Vector2D) = (point - pos).length <= radius +} diff --git a/src/main/scala/sims/dynamics/Constraint.scala b/src/main/scala/sims/dynamics/Constraint.scala new file mode 100644 index 0000000..eaa6952 --- /dev/null +++ b/src/main/scala/sims/dynamics/Constraint.scala @@ -0,0 +1,21 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +/**All constraints in SiMS implement this trait. + * Position and velocity can be corrected for each constraint. + * The implementation of constraints was inspired by Erin Catto's box2d.*/ +trait Constraint { + + /**Corrects the velocities of bodies according to this constraint. + * @param h a time interval, used for converting forces and impulses*/ + def correctVelocity(h: Double): Unit + + /**Corrects the positions of bodies according to this constraint. + * @param h a time interval, used for converting forces and impulses*/ + def correctPosition(h: Double): Unit +} diff --git a/src/main/scala/sims/dynamics/Rectangle.scala b/src/main/scala/sims/dynamics/Rectangle.scala new file mode 100644 index 0000000..89ab4c0 --- /dev/null +++ b/src/main/scala/sims/dynamics/Rectangle.scala @@ -0,0 +1,38 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +import sims.geometry._ +import sims.collision._ + +/**A rectangle is a polygon. + * @param halfWidth this rectangle's half width + * @param halfHeight this rectangle's half height + * @param density density of this rectangle + */ +case class Rectangle(halfWidth: Double, + halfHeight : Double, + density: Double) extends Shape with ConvexPolygon{ + + val volume = halfWidth * halfHeight * 4 + + val I = 1.0 / 12.0 * mass * ((2 * halfWidth) * (2 * halfWidth) + (2 * halfHeight) * (2 * halfHeight)) + + /**Returns the vectors from the center to the vertices of this rectangle. + * The first vertex is the upper-right vertex at a rotation of 0. + * Vertices are ordered counter-clockwise.*/ + def halfDiags: Array[Vector2D] = Array(Vector2D(halfWidth, halfHeight), + Vector2D(-halfWidth, halfHeight), + Vector2D(-halfWidth, -halfHeight), + Vector2D(halfWidth, -halfHeight)) map (_ rotate rotation) + + /**Returns the position vectors of this rectangle's vertices. + * The first vertex is the upper-right vertex at a rotation of 0. + * Vertices are ordered counter-clockwise.*/ + def vertices = for (h <- halfDiags) yield pos + h + +}
\ No newline at end of file diff --git a/src/main/scala/sims/dynamics/RegularPolygon.scala b/src/main/scala/sims/dynamics/RegularPolygon.scala new file mode 100644 index 0000000..6f08ca1 --- /dev/null +++ b/src/main/scala/sims/dynamics/RegularPolygon.scala @@ -0,0 +1,35 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +import math._ +import sims.geometry._ + +/**A regular polygon with <code>n</code> sides whose excircle has a radius <code>radius</code>. + * @param n nmber of sides. + * @param radius radius of the excircle + * @param density density of this regular polygon + * @throws IllegalArgumentException if <code>n</code> is smaller than 3 */ +case class RegularPolygon(n: Int, radius: Double, density: Double) extends Shape with ConvexPolygon{ + require(n >= 3, "A polygon must have at least 3 sides.") + + /**Height of one of the constituting triangles.*/ + private val h: Double = radius * cos(Pi / n) + /**Half width of one of the constituting triangles.*/ + private val b: Double = radius * sin(Pi / n) + + def halfDiags = (for (i: Int <- (0 until n).toArray) yield (Vector2D(0, radius) rotate (2 * Pi * i / n))) map (_ rotate rotation) + + def vertices = for (h <- halfDiags) yield pos + h + + val volume = n * h * b + + /**Moment of inertia of one of the constituting triangles about the center of this polygon.*/ + private val Ic: Double = density * b * (3 * b + 16) * h * h * h * h / 54 + + val I = n * Ic +} diff --git a/src/main/scala/sims/dynamics/Shape.scala b/src/main/scala/sims/dynamics/Shape.scala new file mode 100644 index 0000000..47a4199 --- /dev/null +++ b/src/main/scala/sims/dynamics/Shape.scala @@ -0,0 +1,97 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +import sims.geometry._ +import sims.collision._ + +/** +* An abstract shape. +*/ +abstract class Shape{ + + /**Unique identification number.*/ + val uid: Int = Shape.nextUid + + /**Flag determining this shapes ability to collide with other shapes.*/ + var collidable: Boolean = true + + /**Part of the coefficient of restitution for a collision between this shape and another. + * The coefficient of restitution is calculated out of the product of this part and the other shape's part.*/ + var restitution: Double = 0.7 + + /**Part of the coefficient of friction for a collision between this shape and another. + * The coefficient of friction is calculated out of the product of this part and the other shape's part.*/ + var friction: Double = 0.707 + + /**Position of this shape's COM (in world coordinates).*/ + var pos: Vector2D = Vector2D.Null + + /**Rotation of this shape about its COM.*/ + var rotation: Double = 0 + + /**Initial rotation. Rotation of this shape before it was added to a body.*/ + var rotation0 = 0.0 + + /**Local position of this shape's body COM to its COM at a body rotation of zero.*/ + var refLocalPos: Vector2D = Vector2D.Null + + /**Density. (Mass per area)*/ + val density: Double + + /**Volume. The volume is actually equivalent to this shape's area (SiMS is in 2D) + * and is used with this shape's density to calculate its mass.*/ + val volume: Double + + /**Returns the mass of this shape. The mass is given by volume times density. + @return mass of this shape*/ + def mass = volume * density + + /**Moment of inertia for a rotation about this shape's COM.*/ + val I: Double + + /**Containing body.*/ + private var _body: Body = _ + + /**Returns this shape's containing body.*/ + def body = _body + + /**Sets this shape's containing body.*/ + private[dynamics] def body_=(b: Body) = _body = b + + /**Returns this shape's axis aligned bounding box.*/ + def AABB: AABB + + /**Returns the projection of this shape onto the line given by the directional vector <code>axis</code>. + * @param axis directional vector of the line + * @return projection of this shape*/ + def project(axis: Vector2D): Projection + + /**Checks if the point <code>point</code> is contained in this shape.*/ + def contains(point: Vector2D): Boolean + + /**Creates a new body made out of tis shape. + @return a body made out of tis shape*/ + def asBody = new Body(this) + + /**Shapes with which this shape cannot collide.*/ + val transientShapes: collection.mutable.Set[Shape] = collection.mutable.Set() + + /**Creates a new body out of this shape and the shape <code>s</code>.*/ + def ~(s: Shape) = new Body(this, s) + + /**Creates a new body out of this shape and the shapes of body <code>b</code>.*/ + def ~(b: Body) = { + val shapes = this :: b.shapes + new Body(shapes: _*) + } +} + +object Shape { + private var uidCounter = -1 + private def nextUid = {uidCounter += 1; uidCounter} +} diff --git a/src/main/scala/sims/dynamics/World.scala b/src/main/scala/sims/dynamics/World.scala new file mode 100644 index 0000000..f24a3fd --- /dev/null +++ b/src/main/scala/sims/dynamics/World.scala @@ -0,0 +1,164 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +import sims.geometry._ +import sims.collision._ +import sims.dynamics.joints._ +import scala.collection.mutable.ArrayBuffer + +/**A world contains and simulates a system of rigid bodies and joints.*/ +class World { + + /**Time intervals in which this world simulates.*/ + var timeStep: Double = 1.0 / 60 + + /**Number of constraint corrections per time step.*/ + var iterations: Int = 10 + + /**Gravity in this world.*/ + var gravity = Vector2D(0, -9.81) + + /**Bodies contained in this world.*/ + val bodies = new ArrayBuffer[Body] + + /**Joints contained in this world.*/ + val joints = new ArrayBuffer[Joint] + + /**Monitoring methods for bodies. + * <p> + * The first element of the tuple is the method's title and the second the method. + * Example usage: monitors += ("Y-Position", _.pos.y.toString) + * This will calculate all bodies - whose <code>monitor</code> field is set to + * <code>true</code> - second position components.*/ + val monitors = new ArrayBuffer[(String, Body => Any)] + + /**Collsion detector who manages collision detection in this world.*/ + val detector: Detector = new GridDetector(this) + + /**Warning if a body's velocity exceeds the speed of light.*/ + var overCWarning = false + + /**Flag to enable collision detection.*/ + var enableCollisionDetection = true + + /**Flag to enable position correction for constraints.*/ + var enablePositionCorrection = true + + /**Minimal, non-zero linear velocity.*/ + var minLinearVelocity: Double = 0.0001 + + /**Minimal, non-zero angular velocity.*/ + var minAngularVelocity: Double = 0.0001 + + /**Returns all shapes of all bodies in this world.*/ + def shapes = for (b <- bodies; s <- b.shapes) yield s + + /**Adds the given body to this world.*/ + def +=(body: Body) = bodies += body + + /**Adds the given joint to this world.*/ + def +=(joint: Joint): Unit = joints += joint + + /**Adds the given prefabricated system of bodies and joints to this world.*/ + def +=(p: sims.prefabs.Prefab): Unit = { + for (b <- p.bodies) this += b + for (j <- p.joints) this += j + } + + /**Adds the given sequence of bodies to this world.*/ + def ++=(bs: Seq[Body]): Unit = for(b <- bs) this += b + + /**Removes the given body from this world.*/ + def -=(body: Body): Unit = bodies -= body + + /**Removes the given joint from this world.*/ + def -=(joint: Joint): Unit = joints -= joint + + /**Removes the given prefabricated system of bodies and joints from this world.*/ + def -=(p: sims.prefabs.Prefab): Unit = { + for (b <- p.bodies) this -= b + for (j <- p.joints) this -= j + } + + /**Removes the given sequence of bodies from this world.*/ + def --=(bs: Seq[Body]) = for(b <- bs) this -= b + + /**Removes all bodies, joints and monitoring methods from this world.*/ + def clear() = {joints.clear(); bodies.clear(); monitors.clear()} + + /**Current time in this world.*/ + var time: Double = 0.0 + + /**Simulates a time step of the duration <code>timeStep</code>. + * <p> + * The time step is simulated in the following phases: + * <ol> + * <li>Forces are applied to bodies.</li> + * <li>Accelerations are integrated.</li> + * <li>Velocities are corrected.</li> + * <li>Velocities are integrated.</li> + * <li>Postions are corrected.</li> + * <li>The method <code>postStep()</code> is executed.</li> + * </ol>*/ + def step() = { + time += timeStep + + //force applying objects + for (j <- joints) j match {case f: ForceJoint => f.applyForce; case _ => ()} + + //integration of acclerations, yields velocities + for (b <- bodies) { + b.applyForce(gravity * b.mass) + b.linearVelocity = b.linearVelocity + (b.force / b.mass) * timeStep + b.angularVelocity = b.angularVelocity + (b.torque / b.I) * timeStep + } + + //correction of velocities + for (i <- 0 until iterations){ + for(c <- joints) c.correctVelocity(timeStep) + if (enableCollisionDetection) for (c <- detector.collisions) c.correctVelocity(timeStep) + } + + //integration of velocities, yields positions + for (b <- bodies) { + //warning when body gets faster than speed of light + if (b.linearVelocity.length >= 300000000) overCWarning = true + if (b.linearVelocity.length < minLinearVelocity) b.linearVelocity = Vector2D.Null + if (b.angularVelocity.abs < minAngularVelocity) b.angularVelocity = 0.0 + b.pos = b.pos + b.linearVelocity * timeStep + b.rotation = b.rotation + b.angularVelocity * timeStep + b.force = Vector2D.Null + b.torque = 0.0 + } + + //correction of positions + if (enablePositionCorrection) for (i <- 0 until iterations){ + for (c <- joints) c.correctPosition(timeStep) + if (enableCollisionDetection) for (c <- detector.collisions) c.correctPosition(timeStep) + } + + postStep() + } + + /**Initially empty method that is executed after each time step. This method + * may be overriden to create custom behaviour in a world.*/ + def postStep() = {} + + /**Returns information about this world.*/ + def info = { + "Bodies = " + bodies.length + "\n" + + "Shapes = " + shapes.length + "\n" + + "Joints = " + joints.length + "\n" + + "Collisions = " + detector.collisions.length + "\n" + + "Monitors = " + monitors.length + "\n" + + "Gravity = " + gravity + "m/s^2\n" + + "Timestep = " + timeStep + "s\n" + + "Time = " + time + "s\n" + + "Iterations = " + iterations + } +} 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) = {} +} |