diff options
Diffstat (limited to 'src/sims')
35 files changed, 1823 insertions, 0 deletions
diff --git a/src/sims/collision/AABB.scala b/src/sims/collision/AABB.scala new file mode 100644 index 0000000..51b3e12 --- /dev/null +++ b/src/sims/collision/AABB.scala @@ -0,0 +1,29 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import geometry._ + +/** + * Axis Aligned Bounding Boxes, kurz AABBs, sind Rechtecke die eine bestimmte Form umhuellen. + * Da AABBs nach den X- und Y-Achsen orientiert sind, ermoeglichen sie eine schnelle + * und einfache Feststellung ob zwei AABBs sich ueberschneiden. + * @param minVertex Ortsvektor der minimalen Ecke des AABBs + * @param maxVertex Ortsvektor der maximalen Ecke des AABBs + */ +case class AABB(val minVertex: Vector2D, + val maxVertex: Vector2D) +{ + /** + * Ueberprueft ob dieses AABB sich mit dem AABB <code>box</code> ueberschneidet. + * @param box das mit diesem auf Ueberschneidung zu ueberpruefende AABB*/ + def overlaps(box: AABB): Boolean = { + val d1 = box.minVertex - maxVertex + val d2 = minVertex - box.maxVertex + !(d1.x > 0 || d1.y > 0 || d2.x > 0 || d2.y > 0) + } +} diff --git a/src/sims/collision/CircleCollision.scala b/src/sims/collision/CircleCollision.scala new file mode 100644 index 0000000..baf401a --- /dev/null +++ b/src/sims/collision/CircleCollision.scala @@ -0,0 +1,22 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import geometry._ +import dynamics._ + +/**Kollision zwischen zwei Kreisen.*/ +case class CircleCollision(c1: Circle, c2: Circle) extends Collision { + val shape1 = c1 + val shape2 = c2 + val normal = (c2.pos - c1.pos).unit + val points = { + val distance = (c2.pos - c1.pos).length + val p = shape1.pos + normal * (distance - c2.radius) + List(p) + } +} diff --git a/src/sims/collision/Collision.scala b/src/sims/collision/Collision.scala new file mode 100644 index 0000000..ad23f4d --- /dev/null +++ b/src/sims/collision/Collision.scala @@ -0,0 +1,109 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import dynamics._ +import geometry._ + +/**Kollisionen zwischen zwei Formen enthalten Methoden zur Berrechnen der Kollisionsreaktion.*/ +abstract class Collision extends Constraint { + + /**Erste Kollisionsform (Referenz).*/ + val shape1: Shape + + /**Zweite Kollisionsform (eindringend).*/ + val shape2: Shape + + /**Kollisionspunkte.*/ + val points: Iterable[Vector2D] + + /**Normalenvektor zu der Kollisionsebene.*/ + val normal: Vector2D + + /* C = delta + * Cdot = (vp2 - vp1) dot n + * = v2 + (w2 cross r2) - v2 - (w1 cross r1) + * = v2 + (w2 cross (p - x2)) - v2 - (w1 cross(p - x1)) + * J = [-n -((p-x1) cross n) n ((p-x2) cross n)]*/ + def correctVelocity(h: Double) = { + val coefficientOfRestitution = shape1.restitution * shape2.restitution + for (p <- points) { + val b1 = shape1.body + val b2 = shape2.body + val relativeNormalVelocity = (b2.velocityOfPoint(p) - b1.velocityOfPoint(p)) dot normal + val Cdot = relativeNormalVelocity + relativeNormalVelocity * coefficientOfRestitution + if (Cdot <= 0) { + val r1 = p - b1.pos + val r2 = p - b2.pos + val cr1 = r1 cross normal + val cr2 = r2 cross normal + val invMass = 1/b1.mass * (normal dot normal) + 1/b1.I * cr1 * cr1 + 1/b2.mass * (normal dot normal) + 1/b2.I * cr2 * cr2 + val m = if (invMass == 0.0) 0.0 else 1/invMass + val lambda = -m * Cdot + //wenn fixed, dann ist Masse unendlich => kein 'if (fixed != true)' + b1.linearVelocity += -normal * lambda / b1.mass + b1.angularVelocity += -(r1 cross normal) * lambda / b1.I + b2.linearVelocity += normal * lambda / b2.mass + b2.angularVelocity += (r2 cross normal) * lambda / b2.I + correctFriction(p, (-normal * lambda).length / h, h) + } + } + } + + /* Cdot = vt = [v2 + (w2 cross r2) - v1 - (w2 cross r2)] dot t + * J = [-t -(r2 cross t) t (r1 cross t)] + * 1/m = J * M * JT + * = 1/m1 * (t dot t) + 1/m2 * (t dot t) + 1/I1 * (r1 cross u)^2 + 1/I2 * (r2 cross u)^2*/ + def correctFriction(point: Vector2D, normalForce: Double, h: Double) = { + val b1 = shape1.body + val b2 = shape2.body + val tangent = normal.leftNormal + val Cdot = (b2.velocityOfPoint(point) - b1.velocityOfPoint(point)) dot tangent + val r1 = point - b1.pos + val r2 = point - b2.pos + val cr1 = r1 cross tangent + val cr2 = r2 cross tangent + val invMass = 1/b1.mass * (tangent dot tangent) + 1/b1.I * cr1 * cr1 + 1/b2.mass * (tangent dot tangent) + 1/b2.I * cr2 * cr2 + val m = if (invMass == 0.0) 0.0 else 1/invMass + val mu = shape1.friction * shape2.friction + val lambda = -m * Cdot + val cf = shape1.friction * shape2.friction + val cl = Math.min(Math.max(-normalForce * cf * h, lambda), normalForce * cf * h) + val impulse = tangent * cl + b1.applyImpulse(-impulse, point) + b2.applyImpulse(impulse, point) + } + + def correctPosition(h: Double) = { + val b1 = shape1.body + val b2 = shape2.body + + for (p <- points) { + val overlap = shape1.project(normal) overlap shape2.project(normal) + val C = Collision.ToleratedOverlap - overlap + if (C <= 0.0) { + val r1 = p - b1.pos + val r2 = p - b2.pos + val cr1 = r1 cross normal + val cr2 = r2 cross normal + val invMass = 1/b1.mass + 1/b1.I * cr1 * cr1 + 1/b2.mass + 1/b2.I * cr2 * cr2 + val m = if (invMass == 0.0) 0.0 else 1/invMass + val impulse = -normal.unit * m * C + b1.pos += -impulse / b1.mass + b1.rotation += -(r1 cross impulse) / b1.I + b2.pos += impulse / b2.mass + b2.rotation += (r2 cross impulse) / b2.I + } + } + } +} + +object Collision { + + /**Erlaubte Ueberlappung.*/ + val ToleratedOverlap: Double = 0.01 +} diff --git a/src/sims/collision/Detector.scala b/src/sims/collision/Detector.scala new file mode 100644 index 0000000..e847235 --- /dev/null +++ b/src/sims/collision/Detector.scala @@ -0,0 +1,23 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + + +import sims.geometry._ +import sims.dynamics._ +import scala.collection._ +import scala.collection.mutable._ + +/**Eine Welt ermittelt ihre Kollisionen durch konkrete Implementierungen dieser Klasse.*/ +abstract class Detector { + + /**Die Welt dessen Formen auf Kollisionen ueberprueft werden sollen.*/ + val world: World + + /**Ergibt alle Kollisionen zwischen Formen der Welt <code>world</code>.*/ + def collisions: Seq[Collision] +}
\ No newline at end of file diff --git a/src/sims/collision/GridDetector.scala b/src/sims/collision/GridDetector.scala new file mode 100644 index 0000000..2c027a3 --- /dev/null +++ b/src/sims/collision/GridDetector.scala @@ -0,0 +1,122 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import sims.dynamics._ +import sims.geometry._ +import scala.collection._ +import scala.collection.mutable._ + +/**Eine konkrete Implementierung von <code>Detector</code>. <code>GridDetector</code> ermittelt + * alle Kollisionen mit einem Gittersystem.*/ +class GridDetector(override val world: World) extends Detector { + + /**Array von Kollisionserkennungsmethoden fuer Formenpaare.*/ + val detectionMethods = new ArrayBuffer[PartialFunction[(Shape, Shape), Boolean]] + detectionMethods += { + case (c1: Circle, c2: Circle) => { //Kollision wenn Distanz <= Summe der Radien + val d = (c1.pos - c2.pos).length + val rSum = c1.radius + c2.radius + d - rSum <= 0 + } + + case (p1: ConvexPolygon, p2: ConvexPolygon) => { //SAT + val sides = p1.sides ++ p2.sides + val axes = sides map (_.n0) + axes.forall((a: Vector2D) => p1.project(a) overlaps p2.project(a)) + } + + case (p: ConvexPolygon, c: Circle) => { //Distanz von Zentrum zu Seiten oder Eckpunkten + val distances = for (s <- p.sides) yield (s distance c.pos) + distances.exists(_ - c.radius <= 0) || (p contains c.pos) + } + + case (c: Circle, p: ConvexPolygon) => { //Distanz von Zentrum zu Seiten oder Eckpunkten + val distances = for (s <- p.sides) yield (s distance c.pos) + distances.exists(_ - c.radius <= 0) || (p contains c.pos) + } + } + + /**Array von Kollisionsmethoden fuer Formenpaare.*/ + val collisionMethods = new ArrayBuffer[PartialFunction[(Shape, Shape), Collision]] + collisionMethods += { + case (c1: Circle, c2: Circle) => CircleCollision(c1, c2) + case (p1: ConvexPolygon, p2: ConvexPolygon) => PolyCollision(p1, p2) + case (p: ConvexPolygon, c: Circle) => PolyCircleCollision(p, c) + case (c: Circle, p: ConvexPolygon) => PolyCircleCollision(p, c) + } + + /**Gibt an, ob das Formenpaar <code>p</code> kollidiert. + * @param p Formenpaar.*/ + def colliding(p: Pair) = { + if (detectionMethods.exists(_.isDefinedAt(p))) + detectionMethods.find(_.isDefinedAt(p)).get.apply(p) + else throw new IllegalArgumentException("No collision method for colliding pair!") + } + + /**Gibt die Kollision des Formenpaares <code>p</code> zurueck. + * @param p Formenpaar.*/ + def collision(p: Pair): Collision = { + if (collisionMethods.exists(_.isDefinedAt(p))) + collisionMethods.find(_.isDefinedAt(p)).get.apply(p) + else throw new IllegalArgumentException("No collision found in colliding pair!") + } + + /**Breite und Hoehe einer Gitterzelle.*/ + var gridSide: Double = 2 + + /**Ergibt potenzielle Kollisionspaare der Welt <code>world</code>. + * <p> + * Ein Kollisionspaar ist ein Paar aus zwei verschiedenen Formen, das folgenden Bedingungen unterliegt: + * <ul> + * <li>Die Formen muessen sich in der gleichen Gitterzelle befinden.</li> + * <li>Ihre AABBs muessen sich ueberlappen.</li> + * <li>Die Formen duerfen nicht von dem gleichen Koerper sein.</li> + * <li>Mindestens eine Form darf nicht Fixiert sein.</li> + * <li>Beide muessen {@link dynamics.Shape#collidable collidierbar} sein.</li> + * </ul>*/ + def getPairs = { + val grid = new HashMap[(Int, Int), List[Shape]] + def gridCoordinates(v: Vector2D) = ((v.x / gridSide).toInt, (v.y / gridSide).toInt) + def addToGrid(s: Shape) = { + val aabb = s.AABB + val minCell = gridCoordinates(aabb.minVertex) + val maxCell = gridCoordinates(aabb.maxVertex) + val coords = for(i <- (minCell._1 to maxCell._1); j <- (minCell._2 to maxCell._2)) yield (i, j) + for (c <- coords) { + if (grid.contains(c)) + {if (grid(c).forall(_ ne s)) grid(c) = s :: grid(c)} + else + grid += (c -> List(s)) + } + } + for(s <- world.shapes) addToGrid(s) + var ps: List[Pair] = Nil + for(cell <- grid.values) { + ps = ps ::: (for (s1: Shape <- cell; s2: Shape <- cell; + if (s1 ne s2); + if (s1.body ne s2.body); + if (s1.collidable && s2.collidable); + if (s1.AABB overlaps s2.AABB); + if (!s1.transientShapes.contains(s2) && !s2.transientShapes.contains(s1))) yield Pair(s1, s2) + ).removeDuplicates + } + ps.toSeq + } + + private var cache = (world.time, getPairs) + + /**Alle potentiellen Kollisionspaare der Welt. + * @see getPairs*/ + def pairs = {if (world.time != cache._1) cache = (world.time, getPairs); cache._2} + + /**Ergibt alle kollidierenden Paare.*/ + def collidingPairs: Seq[Pair] = for(p <- pairs; if (colliding(p))) yield p + + /**Ergibt alle Kollisionen.*/ + def collisions: Seq[Collision] = for(p <- pairs; if (colliding(p))) yield collision(p) +} diff --git a/src/sims/collision/Overlap.scala b/src/sims/collision/Overlap.scala new file mode 100644 index 0000000..97ecdd6 --- /dev/null +++ b/src/sims/collision/Overlap.scala @@ -0,0 +1,11 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import sims.geometry._ + +case class Overlap(poly: ConvexPolygon, sideNum: Int, overlap: Double) diff --git a/src/sims/collision/Pair.scala b/src/sims/collision/Pair.scala new file mode 100644 index 0000000..048748d --- /dev/null +++ b/src/sims/collision/Pair.scala @@ -0,0 +1,21 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import sims.dynamics._ + +/**Formenpaar.*/ +case class Pair(s1: Shape, s2: Shape) extends Tuple2(s1, s2){ + def this(t: Tuple2[Shape, Shape]) = this(t._1, t._2) + + override def equals(other: Any) = { //overriden to prevent removal during "GridDetector.getPairs" + other match { + case Pair(a, b) => ((a eq this.s1) && (b eq this.s2)) || ((b eq this.s1) && (a eq this.s2)) + case _ => false + } + } +} diff --git a/src/sims/collision/PolyCircleCollision.scala b/src/sims/collision/PolyCircleCollision.scala new file mode 100644 index 0000000..1bf982e --- /dev/null +++ b/src/sims/collision/PolyCircleCollision.scala @@ -0,0 +1,38 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import sims.dynamics._ +import sims.geometry._ + +/**Kollision zwischen einem konvexen Polygon und einem Kreis.*/ +case class PolyCircleCollision(p: ConvexPolygon, c: Circle) extends Collision { + require(p.isInstanceOf[Shape]) + val shape1 = p.asInstanceOf[Shape] + val shape2 = c + + val normal = { + //minimum overlap + var min = (p.sides(0) distance c.pos) - c.radius + var axis = p.sides(0).n0 + for (s <- p.sides; val delta = (s distance c.pos) - c.radius) if (delta <= 0 && delta < min) { + min = delta + axis = s.n0 + } + for (v <- p.vertices; val delta = (v - c.pos).length - c.radius) if (delta <= 0 && delta <= min){ + min = delta + axis = (c.pos - v).unit + } + axis + } + + val points = List( + c.pos - normal * c.radius + ) + + +} diff --git a/src/sims/collision/PolyCollision.scala b/src/sims/collision/PolyCollision.scala new file mode 100644 index 0000000..bbe5568 --- /dev/null +++ b/src/sims/collision/PolyCollision.scala @@ -0,0 +1,50 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import sims.geometry._ +import sims.dynamics._ +import scala.collection.mutable.Map +import scala.collection.mutable._ + +/**Kollision zwischen zwei konvexen Polygonen.*/ +case class PolyCollision(p1: ConvexPolygon, p2: ConvexPolygon) extends Collision { + require(p1.isInstanceOf[Shape]) + require(p2.isInstanceOf[Shape]) + + def overlap(axis: Vector2D) = { + // println((p1.project(axis) overlap p2.project(axis)).toString + " to " + (p2.project(axis) overlap p1.project(axis))) + p1.project(axis) overlap p2.project(axis) + } + + lazy val overlaps = (for (i <- 0 until p2.sides.length) yield Overlap(p2, i, overlap(p2.sides(i).n0))) ++ + (for (i <- 0 until p1.sides.length) yield Overlap(p1, i, overlap(p1.sides(i).n0))) + + private var potMinOverlap = overlaps.find(_.overlap > 0.0) + require(potMinOverlap != None) + private var _minOverlap: Overlap = potMinOverlap.get + var minOverlap: Overlap = { + for (o <- overlaps) if ((o.overlap < _minOverlap.overlap) && (o.overlap > 0.0)) _minOverlap = o + _minOverlap + } + + + private lazy val refPoly = minOverlap.poly + private lazy val incPoly = if (minOverlap.poly eq p1) p2 else p1 + + lazy val shape1 = refPoly.asInstanceOf[Shape] + lazy val shape2 = incPoly.asInstanceOf[Shape] + + lazy val normal = refPoly.sides(minOverlap.sideNum).n0 + lazy val points = (for (v <- incPoly.vertices; if refPoly.contains(v)) yield v) ++ + (for (s <- incPoly.sides; + val clip = s.clipToSegment(refPoly.sides((refPoly.sides.length - (minOverlap.sideNum + 1)) % refPoly.sides.length)); + if (clip != None)) yield clip.get) ++ + (for (s <- incPoly.sides; + val clip = s.clipToSegment(refPoly.sides((refPoly.sides.length - (minOverlap.sideNum - 1)) % refPoly.sides.length)); + if (clip != None)) yield clip.get) +} diff --git a/src/sims/dynamics/Body.scala b/src/sims/dynamics/Body.scala new file mode 100644 index 0000000..d5b2a0e --- /dev/null +++ b/src/sims/dynamics/Body.scala @@ -0,0 +1,143 @@ +/* + * 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._ + +/**Ein 2-Dimensionaler Koerper besteht aus mehreren Formen. Im gegensatz zu letzteren, enthaelt ein Koerper dynamische Informationen (v, F, etc...). + * @param shps zu dem Koerper gehoerende Formen.*/ +class Body(shps: Shape*){ + + /**Einzigartige Identifikationsnummer dieses Koerpers.*/ + val uid = Body.nextUid + + /**Formen aus denen dieser Koerper besteht.*/ + val shapes: List[Shape] = shps.toList + + //Formen werden bei Initialisierung eingefuegt + for (s <- shapes) { + s.body = this + s.refLocalPos = s.pos - pos + s.rotation0 = s.rotation + } + + private var isFixed: Boolean = false + + /**Gibt an ob dieser Koerper fixiert ist.*/ + def fixed = isFixed + + /**Fixiert oder unfixiert diesen Koerper.*/ + def fixed_=(value: Boolean) = { + if (value) {linearVelocity = Vector2D.Null; angularVelocity = 0.0} + isFixed = value + } + + /**Gibt an ob die Eigenschaften dieses Koerpers ueberwacht werden sollen. + * @see World#monitors*/ + var monitor: Boolean = false + + /**Ermittelt die Position dieses Koerpers. Die Position entspricht dem Schwerpunkt. + * @return Position dieses Koerpers*/ + def pos: Vector2D = // Shwerpunkt = 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) + + /**Setzt die Position dieses Koerpers und verschiebt dadurch die Positionen seiner Formen. + * @param newPos neue Position*/ + def pos_=(newPos: Vector2D) = { + val stepPos = pos + shapes.foreach((s: Shape) => s.pos = s.pos - stepPos + newPos) + } + + /**Enthaelt die aktuelle Rotation dieses Koerpers.*/ + private var _rotation: Double = 0.0 //shapes(0).rotation + + /**Ergibt die aktuelle Rotation dieses Koerpers. + * @return aktuelle Rotation dieses Koerpers*/ + def rotation: Double = _rotation + + /**Setzt die Rotation dieses Koerpers. Dazu werden auch die Positionen und Rotationen seiner Formen entsprechend veraendert. + * @param r neue 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)) + } + } + + /**Lineargeschwindigkeit dieses Koerpers.*/ + var linearVelocity: Vector2D = Vector2D.Null + + /**Winkelgeschwindigkeit dieses Koerpers.*/ + var angularVelocity: Double = 0 + + /**Lineargeschwindigkeit des gegebenen Punktes auf diesem Koerper. In Weltkoordinaten.*/ + def velocityOfPoint(point: Vector2D) = linearVelocity + ((point - pos).leftNormal * angularVelocity) + + /**Resultierende Kraft auf den Schwerpunkt dieses Koerpers.*/ + var force: Vector2D = Vector2D.Null + + /**Resultierender Drehmoment zu dem Schwerpunkt dieses Koerpers.*/ + var torque: Double = 0 + + /**Ergibt die Masse dieses Koerpers. Die Masse ist gleich die Summe aller Massen seiner Formen. + * @return Masse des Koerpers*/ + def mass: Double = if (fixed) Double.PositiveInfinity else (0.0 /: shapes)((i: Double, s: Shape) => i + s.mass) + + /**Ergibt den Traegheitsmoment zu dem Schwerpunkt dieses Koerpers. Der Traegheitsmoment wird mit Hilfe des Steinerschen Satzes errechnet. + * @return Traegheitsmoment relativ zu dem Schwerpunkt dieses Koerpers*/ + 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)))))(_+_) + + /**Wendet eine Kraft auf den Schwerpunkt dieses Koerpers an. + * @param force anzuwendender Kraftvektor*/ + def applyForce(force: Vector2D) = if (!fixed) this.force += force + + /**Wendet eine Kraft auf einen Punkt dieses Koerpers an. Achtung: der gegebene Punkt wird nicht auf angehoerigkeit dieses + * Koerpers ueberprueft. + * @param force anzuwendender Kraftvektor + * @param point Ortsvektor des Punktes auf den die Kraft wirken soll (gegeben in Weltkoordinaten).*/ + def applyForce(force: Vector2D, point: Vector2D) = if (!fixed) {this.force += force; torque += (point - pos) cross force} + + /**Wendet einen Impuls auf den Schwerpunkt dieses Koerpers an. + * @param impulse anzuwendender Impulsvektor*/ + def applyImpulse(impulse: Vector2D) = if (!fixed) linearVelocity += impulse / mass + + /**Wendet einen Impuls auf einen Punkt dieses Koerpers an. Achtung: der gegebene Punkt wird nicht auf angehoerigkeit dieses + * Koerpers ueberprueft. + * @param impulse anzuwendender Impulsvektor + * @param point Ortsvektor des Punktes auf den der Impuls wirken soll (gegeben in Weltkoordinaten).*/ + def applyImpulse(impulse: Vector2D, point: Vector2D) = if (!fixed) {linearVelocity += impulse / mass; angularVelocity += ((point - pos) cross impulse) / I} + + /**Ueberprueft ob der gegebene Punkt <code>point</code> sich in diesem Koerper befindet.*/ + 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 + } + + /**Erstellt einen neuen Koerper der zusaetzlich die Form <code>s</code> enthaelt. + * @param s zusaetzliche Form + * @return neuer Koerper*/ + def ^(s: Shape) = new Body((s :: shapes): _*) + + /**Erstellt einen neuen Koerper der zusaetzlich die Formen von dem Koerper <code>b</code> enthaelt. + * @param b Koerper mit zusaetzlichen Formen + * @return neuer Koerper*/ + def ^(b: Body) = { + val shapes = this.shapes ::: b.shapes + new Body(shapes: _*) + } +} + +object Body { + private var uidCounter = -1 + private def nextUid = {uidCounter += 1; uidCounter} +}
\ No newline at end of file diff --git a/src/sims/dynamics/Circle.scala b/src/sims/dynamics/Circle.scala new file mode 100644 index 0000000..26f3ad4 --- /dev/null +++ b/src/sims/dynamics/Circle.scala @@ -0,0 +1,37 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +import sims.geometry._ +import sims.collision._ + +/** + * Circle ist die Definition eines Kreises. + * @param radius Radius dieses Kreises + * @param density Dichte dieses Kreises + */ +case class Circle(radius: Double, // Radius + density: Double) extends Shape{ // Dichte + + val volume = Math.Pi * radius * radius + + val I = mass * radius * radius / 2 + + // AABB(Zentrum - Radius, Zentrum + Radius) + 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) + + //Ist der gegebene punkt im Radius dieses kreises? + def contains(point: Vector2D) = (point - pos).length <= radius +} diff --git a/src/sims/dynamics/Constraint.scala b/src/sims/dynamics/Constraint.scala new file mode 100644 index 0000000..74c2af3 --- /dev/null +++ b/src/sims/dynamics/Constraint.scala @@ -0,0 +1,19 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +/**Randbedingungen erben von dem Trait <code>Constraint</code>. + * Fuer jeden Constraint koennen Position und Geschwindigkeit korrigiert werden. + * Ihre Implementierung wurde von Erin Catto's box2d inspiriert.*/ +trait Constraint { + + /**Korrigiert die Geschwindigkeit der Koerper damit diese den Randbedingungen entsprechen.*/ + def correctVelocity(h: Double): Unit + + /**Korrigiert die Position der Koerper damit diese den Randbedingungen entsprechen.*/ + def correctPosition(h: Double): Unit +} diff --git a/src/sims/dynamics/Rectangle.scala b/src/sims/dynamics/Rectangle.scala new file mode 100644 index 0000000..adaa634 --- /dev/null +++ b/src/sims/dynamics/Rectangle.scala @@ -0,0 +1,40 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +import sims.geometry._ +import sims.collision._ + +/**Rechteck ist eine Art Polygon. + * @param halfWidth halbe Breite dieses Rechtecks + * @param halfHeight halbe Hoehe dieses Rechtecks + * @param density dichte dieses Rechtecks + */ +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)) + + /**Ergibt Vektoren vom Zentrum dieses Rectecks bis zu den Ecken. + * Erste Ecke entspricht der Ecke oben rechts bei einer Rotation von 0. + * Folgende Ecken sind gegen den Uhrzeigersinn geordnet. + * @return Vektoren vom Zentrum dieses Rectecks bis zu den Ecken*/ + def halfDiags: Array[Vector2D] = Array(Vector2D(halfWidth, halfHeight), + Vector2D(-halfWidth, halfHeight), + Vector2D(-halfWidth, -halfHeight), + Vector2D(halfWidth, -halfHeight)) map (_ rotate rotation) + + /**Ergibt die Ortsvektoren der Ecken dieses Rechtecks. + * Erste Ecke entspricht der Ecke oben rechts bei einer Rotation von 0. + * Folgende Ecken sind gegen den Uhrzeigersinn geordnet. + * @return Ortsvektoren der Ecken dieses Rechtecks*/ + def vertices = for (h <- halfDiags) yield pos + h + +}
\ No newline at end of file diff --git a/src/sims/dynamics/RegularPolygon.scala b/src/sims/dynamics/RegularPolygon.scala new file mode 100644 index 0000000..c5b8a13 --- /dev/null +++ b/src/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._ + +/**Ein regelmaessiges Polygon mit <code>n</code> Seiten, dass der Kreis mit radius <code>radius</code> umschreibt. + * @param n Anzahl der Seiten. + * @param radius Radius des umschreibenden Kreises. + * @param density Dichte. + */ +case class RegularPolygon(n: Int, radius: Double, density: Double) extends Shape with ConvexPolygon{ + require(n >= 3, "Polygon must have at least 3 sides.") + + /**Hoehe eines der konstituierneden Dreiecke des Polygons.*/ + private val h: Double = radius * cos(Pi / n) + /**Halbe Breite eines der konstituierneden Dreiecke des Polygons.*/ + 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 + + /**Traegheitsmoment eines der konstituierneden Dreiecke im Zentrum des Polygons.*/ + private val Ic: Double = density * b * (3 * b + 16) * h * h * h * h / 54 + + val I = n * Ic +} diff --git a/src/sims/dynamics/Shape.scala b/src/sims/dynamics/Shape.scala new file mode 100644 index 0000000..f57bbc6 --- /dev/null +++ b/src/sims/dynamics/Shape.scala @@ -0,0 +1,93 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.dynamics + +import sims.geometry._ +import sims.collision._ + +/** +* Eine abstrakte Form. +*/ +abstract class Shape{ + + /**Einzigartige Identifikationsnummer.*/ + val uid: Int = Shape.nextUid + + /**Kollisionsfaehigkeit.*/ + var collidable: Boolean = true + + /**Teil der Stosszahl bei einer Kollision zwischen dieser Form und einer anderen. + * Die Stosszahl wird aus dem Produkt der beiden Teile der Formen errechnet.*/ + var restitution: Double = 0.7 + + /**Teil des Reibungskoeffizienten bei einer Kollision zwischen dieser Form und einer anderen. + * Der Reibungskoeffizient wird aus dem Produkt der beiden Teile der Formen errechnet.*/ + var friction: Double = 0.707 + + /**Position des Schwerpunktes in Welt.*/ + var pos: Vector2D = Vector2D.Null + + /**Rotation. Entspricht Laenge des Rotationsvektors.*/ + var rotation: Double = 0 + + /**Initiale Rotation. (Rotation ohne Koerper)*/ + var rotation0 = 0.0 + + /**Referenzposition in Koerper. Wird zur Rotation von Formen in Koerpern verwendet.*/ + var refLocalPos: Vector2D = Vector2D.Null + + /**Dichte. (Masse pro Flaeche)*/ + val density: Double + + /**Volumen. Entspricht eigentlich der Flaeche dieser Form (in 2D) wird aber zum Errechnen der Masse verwendet.*/ + val volume: Double + + /**Errechnet die Masse dieser Form. Masse ist gleich Volumen mal Dichte. + @return Masse der Form*/ + def mass = volume * density + + /**Errechnet Traegheitsmoment zum Schwerpunkt dieser Form. + @return Traegheitsmoment zum Schwerpunkt*/ + val I: Double + + /**Beinhaltender Koerper. Sollte nicht selbst bei Initialisierung definiert werden.*/ + var body: Body = _ + + /**Gibt das umfassende AABB dieser Form zurueck. + @return umfassendes AABB*/ + def AABB: AABB + + /**Ergibt die Projektion dieser Form auf eine Gerade gegeben durch den + * Richtungsvektor <code>axis</code>. + * @param axis Richtungsvektor der Geraden + * @return Projektion dieser Form*/ + def project(axis: Vector2D): Projection + + /**Ermittelt ob der gebene Punkt <code>point</code> in dieser Form enthalten ist.*/ + def contains(point: Vector2D): Boolean + + /**Baut einen Koerper aus dieser Form. + @return ein Koerper bestehend aus dieser Form. */ + def asBody = new Body(this) + + /**Formen mit denen diese Form nicht Kollidiert.*/ + val transientShapes: collection.mutable.Set[Shape] = collection.mutable.Set() + + /**Erstellt einen Koerper aus dieser Form und der Form <code>s</code>.*/ + def ^(s: Shape) = new Body(this, s) + + /**Erstellt einen Koerper aus dieser Form und den Formen des Koerpers <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/sims/dynamics/World.scala b/src/sims/dynamics/World.scala new file mode 100644 index 0000000..d7ac8ae --- /dev/null +++ b/src/sims/dynamics/World.scala @@ -0,0 +1,163 @@ +/* + * 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._ + +/**Eine Welt enthaelt und Simuliert ein System aus Koerpern und Verbindungen.*/ +class World { + + /**Zeitschritt in dem diese Welt die Simulation vorranschreiten laesst.*/ + var timeStep: Double = 1.0 / 60 + + /**Anzahl der Constraint-Korrekturen pro Zeitschritt.*/ + var iterations: Int = 10 + + /**Schwerkraft die in dieser Welt herrscht.*/ + var gravity = Vector2D(0, -9.81) + + /**Alle Koerper die diese Welt simuliert.*/ + val bodies = new ArrayBuffer[Body] + + /**Alle Verbindungen die diese Welt simuliert.*/ + val joints = new ArrayBuffer[Joint] + + /**Ueberwachungsfunktionen fuer Koerper. + * <p> + * Das erste Element des Tuples ist die Ueberschrift und das zweite Element, der Wert.*/ + val monitors = new ArrayBuffer[(String, Body => String)] + + /**Kollisionsdetektor dieser Welt.*/ + val detector: Detector = new GridDetector(this) + + /**Warnung wenn Koerper schneller als Lichtgeschwindigkeit.*/ + var overCWarning = false + + /**Kollisionerkennung.*/ + var enableCollisionDetection = true + + /**Positionskorrekturen.*/ + var enablePositionCorrection = true + + /**Die minimale, nicht als null geltende Geschwindigkeit.*/ + var minLinearVelocity: Double = 0.0001 + + /**Die minimale, nicht als null geltende Winkelgeschwindigkeit.*/ + var minAngularVelocity: Double = 0.0001 + + /**Ergibt alle Formen aus allen Koerpern in dieser Welt.*/ + def shapes = for (b <- bodies; s <- b.shapes) yield s + + /**Fuegt dieser Welt einen Koerper hinzu.*/ + def +=(body: Body) = bodies += body + + /**Fuegt dieser Welt eine Verbindung hinzu.*/ + def +=(joint: Joint): Unit = joints += joint + + /**Fuegt dieser Welt ein vorangefertigtes System vaus Koerpern und Verbindungen hinzu.*/ + def +=(p: prefabs.Prefab): Unit = { + for (b <- p.bodies) this += b + for (j <- p.joints) this += j + } + + def ++=(bs: Seq[Body]) = for(b <- bs) this += b + + /**Entfernt den gegebenen Koerper aus dieser Welt.*/ + def -=(body: Body): Unit = bodies -= body + + /**Entfernt die gegebene Verbindung aus dieser Welt.*/ + def -=(joint: Joint): Unit = joints -= joint + + /**Entfernt das gegebene System aus Koerpern und Verbindungen aus dieser Welt.*/ + def -=(p: prefabs.Prefab): Unit = { + for (b <- p.bodies) this -= b + for (j <- p.joints) this -= j + } + + def --=(bs: Seq[Body]) = for(b <- bs) this -= b + + /**Entfernt alle Koerper, Verbindungen und Ueberwachungsfunktionen dieser Welt.*/ + def clear() = {joints.clear(); bodies.clear(); monitors.clear()} + + /**Aktuelle Zeit in Sekunden dieser Welt. Nach jedem Zeitschritt wird die Zeit erhoeht.*/ + var time: Double = 0.0 + + /**Simuliert einen von <code>timeStep</code> angegebenen Zeitschritt. + * Ihre Aufgabe ist es die Koerper dieser Welt so zu simulieren wie diese sich in einer Welt mit den gegebenen + * Bedingungen verhalten wuerden. + * <p> + * Der Zeitschritt wird in folgenden Phasen ausgefuehrt: + * <ol> + * <li>Kraefte wirken auf die Koerper (z.B Schwerkraft, andere Kraftfaehige Objekte).</li> + * <li>Beschleunigungen werden integriert.</li> + * <li>Geschwindigkeiten werden korrigiert.</li> + * <li>Geschwindigkeiten werden integriert.</li> + * <li>Positionen werden korrigiert.</li> + * <li>Die Methode <code>postStep()</code> wird ausgefuehrt.</li> + * </ol>*/ + def step() = { + time += timeStep + + //Kraftobjekte + for (j <- joints) j match {case f: ForceJoint => f.applyForce; case _ => ()} + + //integriert v + for (b <- bodies) { + val m = b.mass + b.applyForce(gravity * b.mass) + val a = b.force / b.mass + val alpha = b.torque / b.I + b.linearVelocity = b.linearVelocity + a * timeStep + b.angularVelocity = b.angularVelocity + alpha * timeStep + } + + //korrigiert v + for (i <- 0 until iterations){ + for(c <- joints) c.correctVelocity(timeStep) + if (enableCollisionDetection) for (c <- detector.collisions) c.correctVelocity(timeStep) + } + + //integriert pos + 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 + } + + //korrigiert pos + if (enablePositionCorrection) for (i <- 0 until iterations){ + for (c <- joints) c.correctPosition(timeStep) + if (enableCollisionDetection) for (c <- detector.collisions) c.correctPosition(timeStep) + } + + postStep() + } + + /**Wird nach jedem Zeitschritt ausgefuehrt.*/ + def postStep() = {} + + /**Ergibt Informationen ueber diese Welt.*/ + 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/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) = {} +} diff --git a/src/sims/geometry/ConvexPolygon.scala b/src/sims/geometry/ConvexPolygon.scala new file mode 100644 index 0000000..7bf881c --- /dev/null +++ b/src/sims/geometry/ConvexPolygon.scala @@ -0,0 +1,61 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.geometry + +import sims.collision._ +import sims.geometry._ + +/**Gemeinsame Eigenschaften aller konvexen Polygone.*/ +trait ConvexPolygon { + + /**Ergibt Position aller Ecken dieses Polygons. Die Ecken sind gegen den Uhrzeigersinn folgend. + * @return Ortsvektoren der Ecken*/ + def vertices: Seq[Vector2D] + + /**Ergibt alle Seiten dieses Polygons. + * @return Seiten dieses Polygons*/ + def sides = (for (i <- 0 until vertices.length) yield (new Segment(vertices(i), vertices((i + 1) % vertices.length)))).toArray + + /**Ergibt die Projektion dieses Polygons auf eine Gerade gegeben durch den + * Richtungsvektor <code>axis</code> + * @param axis Richtungsvektor der Geraden + * @return Projektion dieses Polygons*/ + def project(axis: Vector2D) = { + val points = for (v <- vertices) yield {v project axis} + val bounds = for (p <- points) yield {if (axis.x != 0) p.x / axis.x else p.y / axis.y} + Projection(axis, + (bounds(0) /: bounds)(Math.min(_,_)), + (bounds(0) /: bounds)(Math.max(_,_))) + } + + /**Errechnet das AABB dieses Polygons + * @return umfassendes AABB + * @see collision.AABB*/ + def AABB = { + val xs = vertices map (_.x) + val ys = vertices map (_.y) + new AABB(Vector2D(Iterable.min(xs), Iterable.min(ys)), + Vector2D(Iterable.max(xs), Iterable.max(ys))) + } + + /**Ueberprueft ob sich der gegebene Punkt <code>point</code> in diesem Polygon befindet. + * <p> + * Hierzu wird eine Halbgerade von dem Punkt in Richtung der X-Achse gezogen (koennte aber auch beliebig sein). + * Dann wird die Anzahl der Ueberschneidungen der Halbgeraden mit den Seiten und Ecken des Polygons ermittelt. + * Ist die Anzahl der Ueberschneidungen ungerade, so befindet sich der Punkt in dem Polygon. + * Es gibt jedoch Ausnahmen, und zwar wenn die Halbgerade eine Ecke ueberschneidet, ueberschneidet sie sowohl auch zwei Seiten. + * Daher wird eine generelle Anzahl von Uerberschneidungen errechnet, gegeben durch die Anzahl der Ueberschneidungen mit den Seiten minus + * die mit den Ecken. + * Diese Zahl wird dann wie oben geschildert geprueft.*/ + def contains(point: Vector2D) = { + val r = new Ray(point, Vector2D.i) + var intersections = 0 + for (s <- sides; if (r intersects s)) intersections += 1 + for (v <- vertices; if (r contains v)) intersections -= 1 + intersections % 2 != 0 + } +} diff --git a/src/sims/geometry/Projection.scala b/src/sims/geometry/Projection.scala new file mode 100644 index 0000000..5f2d0f0 --- /dev/null +++ b/src/sims/geometry/Projection.scala @@ -0,0 +1,34 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.geometry + +import sims.math._ + +/**Projektion auf eine Achse. + * <p> + * Ueblicherweise werden Projektionen in SiMS fuer Kollisionserkennung benutzt. + * @param axis Achse der Projektion + * @param lower unterer Wert der Projektion + * @param upper oberer Wert der Projektion*/ +case class Projection(axis: Vector2D, + lower: Double, + upper: Double) { + require(axis != Vector2D.Null) + + /**Ueberprueft ob sich diese Projektion mit der Projektion <code>other</code> ueberschneidet.*/ + def overlaps(other: Projection): Boolean = { + require(axis == other.axis, "Cannot compare two projections on different axes!") + !((other.lower - this.upper) > 0 || (this.lower - other.upper) > 0) + } + + + /**Ergibt die Ueberlappung dieser Projektion und der Projektion <code>other</code>.*/ + def overlap(other: Projection): Double = { + require(axis == other.axis, "Cannot compare two projections on different axes!") + (Math.max(lower, other.lower) - Math.min(upper, other.upper)).abs + } +} diff --git a/src/sims/geometry/Ray.scala b/src/sims/geometry/Ray.scala new file mode 100644 index 0000000..c898e03 --- /dev/null +++ b/src/sims/geometry/Ray.scala @@ -0,0 +1,53 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.geometry + +import sims.math._ +import Math._ + +/**Eine Halbgerade wird definiert durch: + * @param point ein Aufpunkt + * @param direction ein Richtungsvektor*/ +case class Ray(point: Vector2D, direction: Vector2D) { + + //Ein Nullvektor hat keine Richtung + require(direction != Vector2D.Null) + + /**Ueberprueft ob diese Halbgerade das gegebene Segment ueberschneidet. + * @param das auf Ueberschneidung zu uerberpruefende Segment*/ + def intersects(s: Segment) = { + val p1 = point + val p2 = point + direction + val p3 = s.vertex1 + val p4 = s.vertex2 + val d = (p4.y - p3.y) * (p2.x - p1.x) - (p4.x - p3.x) * (p2.y - p1.y) + val na = (p4.x - p3.x) * (p1.y - p3.y) - (p4.y - p3.y) * (p1.x - p3.x) + val nb = (p2.x - p1.x) * (p1.y - p3.y) - (p2.y - p1.y) * (p1.x - p3.x) + if (d == 0 && na == 0 && nb == 0) + true //lines are coincident + else if (d == 0) + false //parallel + else { + val ua = na / d + val ub = nb / d + (ub >= 0) && (ub <= 1) && (ua >= 0) + } + } + + /**Ueberprueft ob diese Halbgerade den gegebenen Punkt enthaelt. + * <br> + * Hierzu wird der Vektor von dem Ursprungspunkt zu dem zu ueberpruefenden Punkt gebildet. Dieser wird dann mit dem Richtungsvektor + * auf Kolinearitaet geprueft. + * @param p Ortsvektor des oben genannten Punkt*/ + def contains(p: Vector2D) = { + val v = p - point + p == point || + Matrix22(direction, v).det == 0 && + signum(direction.x) == signum(v.x) && + signum(direction.y) == signum(v.y) + } +} diff --git a/src/sims/geometry/Segment.scala b/src/sims/geometry/Segment.scala new file mode 100644 index 0000000..8700979 --- /dev/null +++ b/src/sims/geometry/Segment.scala @@ -0,0 +1,70 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.geometry + +/**Ein Segment wird durch seine beiden Extrempunkte gegeben. + * @param vertex1 Ortsvektor des 1. Extrempunkts + * @param vertex2 Ortsvektor des 2. Extrempunkts*/ +case class Segment(vertex1: Vector2D, vertex2: Vector2D){ + require(vertex1 != vertex2, "A segment must have 2 distinct vertices!") + + /**Laenge dieses Segments.*/ + val length = (vertex2 - vertex1).length + + /**Vektor von EP1 zu EP2.*/ + val d = vertex2 - vertex1 + + /**Einheitsrichtungsvektor.*/ + val d0 = d.unit + + /**Normalenvektor. Richtung: 90 Grad rechts zu d.*/ + val n = d.rightNormal + + /**Normaleneinheitsvektor. Richtung: 90 Grad rechts zu d.*/ + val n0 = n.unit + + /**Kleinster Abstand zwischen diesem Segment und dem Punkt <code>p</code>.*/ + def distance(point: Vector2D): Double = { + val v = point - vertex1 //Vektor von EP1 zu point + val projection = v project d + val alpha = if (d.x != 0) d.x / projection.x else d.y / projection.y + if (alpha >= 0 && projection.length <= length) //Punkt ist naeher zu der Geraden zwischen EP1 und EP2 + (v project n0).length + else if (alpha < 0) //Punkt ist naeher zu EP1 + (point - vertex1).length + else if (alpha > 0) //Punkt ist naeher zu EP2 + (point - vertex2).length + else + throw new IllegalArgumentException("Error occured trying to compute distance between segment and point.") + } + + def clipToSegment(s: Segment): Option[Vector2D] = { + + val distance1 = (vertex1 - s.vertex1) dot s.n0 + val distance2 = (vertex2 - s.vertex1) dot s.n0 + + if (distance1 * distance2 < 0) { //auf anderen Seiten + /* Geradengleichungen + * ================== + * Segment1: s1: x = a + alpha * r | alpha in [0,1] + * Segment2: s2: x = b + beta * s | beta in [0,1] + * + * alpha = [s2(a1-b1)-s1(a2-b2)] / [r2s1-r1s2] + * beta = [r2(b1-a1)-r1(b2-a2)] / [r1s2-r2s1] + * = [r1(b2-a2)]-r2(b1-a1) / [r2s1-r1s2] + * s1: vertex1 + alpha * d + * s2: s.vertex1 + beta * s.d + */ + val denom: Double = d.y * s.d.x - d.x * s.d.y + val alpha: Double = (s.d.y * (vertex1.x - s.vertex1.x) - s.d.x * (vertex1.y - s.vertex1.y)) / denom + val beta: Double = (d.x * (s.vertex1.y - vertex1.y) - d.y * (s.vertex1.x - vertex1.x)) / denom + if (0.0 <= alpha && alpha <= 1.0 && 0.0 <= beta && beta <= 1.0) Some(vertex1 + d * alpha) + else None + } + else None + } +} diff --git a/src/sims/geometry/Vector2D.scala b/src/sims/geometry/Vector2D.scala new file mode 100644 index 0000000..03d1ea4 --- /dev/null +++ b/src/sims/geometry/Vector2D.scala @@ -0,0 +1,99 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.geometry + +import scala.Math._ + +/**Ein 2-dimensionaler Vektor. + * @param x 1. Komponente + * @param y 2. Komponente*/ +case class Vector2D(x: Double, y: Double) { + + /**Vektoraddition. + * @param v zu addierender Vektor + * @return dieser Vektor addiert mit <code>v</code>*/ + def +(v: Vector2D): Vector2D = Vector2D(x + v.x, y + v.y) + + /**Vektorsubstraktion. + * @param v zu substrahierender Vektor + * @return dieser Vektor substrahiert mit <code>v</code>*/ + def -(v: Vector2D): Vector2D = this + (v * -1) + + /**Multiplikation mit einem Skalar. + * @param n Faktor + * @return dieser Vektor multipliziert mit <code>n</code>*/ + def *(n: Double): Vector2D = Vector2D(x * n, y * n) + + /**Division durch ein Skalar. + * @param n Nenner + * @return dieser Vektor dividiert durch <code>n</code>*/ + def /(n: Double): Vector2D = this * (1/n) + + /**Minusvorzeichen.*/ + def unary_- : Vector2D = Vector2D(-x, -y) + + /**Skalarprodukt. + * @param v ein anderer Vektor + * @return Skalarprodukt von diesem Vektor mit <code>v</code>*/ + def dot(v: Vector2D): Double = x * v.x + y * v.y + + /**Kreuzprodukt. (Norm des Kreuzproduktes) + * @param v ein anderer Vektor + * @return Norm des Kreuzproduktes dieses Vektors mit <code>v</code>. Die Richtung wuerde der x3-Achse entsprechen.*/ + def cross(v: Vector2D): Double = x * v.y - y * v.x + + /**Norm dieses Vektors.*/ + val length: Double = Math.sqrt(x * x + y * y) + + /**Einheitsvektor dieses Vektors.*/ + def unit: Vector2D = if (!(x == 0.0 && y == 0.0)) Vector2D(x / length, y / length) + else throw new IllegalArgumentException("Null vector does not have a unit vector.") + + /**Errechnet die Projektion dieses- auf einen anderen Vektor. + * @param v oben gennanter Vektor + * @return Projektion dieses Vektors auf <code>v</code>*/ + def project(v: Vector2D): Vector2D = { + if (v != Vector2D.Null) + v * ((this dot v) / (v dot v)) + else + Vector2D.Null + } + + /**Errechnet eine Rotation dieses Vektors. + * @param angle Winkel in Radian + * @return der um <code>angle</code> rad rotierte Vektor*/ + def rotate(angle: Double): Vector2D = { + Vector2D(cos(angle) * x - sin(angle) * y, + cos(angle) * y + sin(angle) * x) + } + + /**Linker Normalenvektor. (-y, x)*/ + def leftNormal: Vector2D = Vector2D(-y, x) + + /**Rechter Normalenvektor. (y, -x)*/ + def rightNormal: Vector2D = Vector2D(y, -x) + + /**Ueberprueft, ob die Komponenten dieses Vektors gleich Null sind.*/ + def isNull: Boolean = this == Vector2D.Null + + /**Ergibt eine Liste der Komponenten dieses Vektors.*/ + def components = List(x, y) +} + +/**Dieses Objekt enthaelt spezielle Vektoren.*/ +object Vector2D { + + /**Nullvektor.*/ + val Null = Vector2D(0,0) + + /**Ein horizontaler Einheitsvektor mit den Komponenten (1;0).*/ + val i = Vector2D(1,0) + + /**Ein vertikaler Einheitsvektor mit den Komponenten (0;1).*/ + val j = Vector2D(0,1) +} + diff --git a/src/sims/math/Matrix22.scala b/src/sims/math/Matrix22.scala new file mode 100644 index 0000000..b10e02a --- /dev/null +++ b/src/sims/math/Matrix22.scala @@ -0,0 +1,57 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.math + +import sims.geometry._ + +/**Eine 2x2, quadratische Matrix. + * @param c11 Komponente 1,1 + * @param c12 Komponente 1,2 + * @param c21 Komponente 2,1 + * @param c22 Komponente 2,2 + */ +case class Matrix22(c11: Double, c12: Double, c21: Double, c22: Double) { + + /**Eine 2x2-dimensionale, quadratische Matrix kann auch mit zwei 2-dimensionalen + * Vektoren erstellt werden. In diesem Fall repraesentiert jeder Vektor eine Spalte. + * @param c1 erste Spalte + * @param c2 zweite Spalte*/ + def this(c1: Vector2D, c2: Vector2D) = this(c1.x, c2.x, c1.y, c2.y) + + /**Ergibt die Determinante dieser Matrix. + * @return Determinante dieser Matrix*/ + def det = c11 * c22 - c21 * c12 + + /**Addition.*/ + def +(m: Matrix22) = + new Matrix22(c11 + m.c11, c12 + m.c12, + c21 + m.c21, c22 + m.c22) + + /**Multiplikation mit einem Skalar.*/ + def *(n: Double) = + new Matrix22(c11 * n, c12 * n, + c21 * n, c22 * n) + + /**Multiplikation mit einer anderen 2x2-Matrix.*/ + def *(m: Matrix22) = + new Matrix22(c11 * m.c11 + c12 * m.c21, c11 * m.c12 + c12 * m.c22, + c21 * m.c11 + c22 * m.c21, c21 * m.c12 + c22 * m.c22) + + /**Multiplikation mit einer 2x1-Matrix (2-dimensionaler Vektor).*/ + def *(v: Vector2D) = + new Vector2D(c11 * v.x + c12 * v.y, + c21 * v.x + c22 * v.y) + + /**Inverse.*/ + def invert = + new Matrix22(c22 / det, -c12 / det, + -c21 / det, c11 / det) + } + +object Matrix22 { + def apply(c1: Vector2D, c2: Vector2D) = new Matrix22(c1.x, c2.x, c1.y, c2.y) +} diff --git a/src/sims/prefabs/Net.scala b/src/sims/prefabs/Net.scala new file mode 100644 index 0000000..d3f4d57 --- /dev/null +++ b/src/sims/prefabs/Net.scala @@ -0,0 +1,47 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.prefabs + +import sims.geometry._ +import sims.dynamics._ +import sims.dynamics.joints._ + +class Net(width: Int, height: Int, initPos: Vector2D) extends Prefab { + val nodeDistance: Double = 0.2 + val nodeRadius: Double = 0.05 + val nodeDensity: Double = 4 + + val springConstant: Double = 50 + val springDamping: Double = 0 + + private val connectors: Array[Array[Body]] = + makeConnectors(width, height) + + override val bodies: List[Body] = for (row <- connectors.toList; elem <- row) yield elem + override val joints = connect(connectors) + + private def makeConnectors(w: Int, h: Int): Array[Array[Body]] = { + for(i <- (0 until w).toArray[Int]) yield + for(j <- (0 until h).toArray[Int]) yield + new Body(new Circle(nodeRadius, nodeDensity) {pos = Vector2D(nodeDistance * i, nodeDistance * j) + initPos}) + } + + private def connect(connectors: Array[Array[Body]]): List[DistanceJoint] = { + var r: List[DistanceJoint] = Nil + for(i <- 0 to connectors.length - 1; j <- 0 to connectors(i).length - 1) { + if (i > 0) + r = connect(connectors(i-1)(j),connectors(i)(j)) :: r + if (j > 0) + r = connect(connectors(i)(j-1),connectors(i)(j)) :: r + } + r + } + + private def connect(s1: Body, s2: Body): DistanceJoint = + new DistanceJoint(s1, s1.pos, s2, s2.pos) + +}
\ No newline at end of file diff --git a/src/sims/prefabs/Prefab.scala b/src/sims/prefabs/Prefab.scala new file mode 100644 index 0000000..84bb3b7 --- /dev/null +++ b/src/sims/prefabs/Prefab.scala @@ -0,0 +1,15 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.prefabs + +import sims.dynamics._ +import sims.dynamics.joints._ + +trait Prefab { + val bodies: Iterable[Body] = Nil + val joints: Iterable[Joint] = Nil +} diff --git a/src/sims/prefabs/Pylon.scala b/src/sims/prefabs/Pylon.scala new file mode 100644 index 0000000..7f3211f --- /dev/null +++ b/src/sims/prefabs/Pylon.scala @@ -0,0 +1,47 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.prefabs + +import sims.geometry._ +import sims.dynamics._ +import sims.dynamics.joints._ + +class Pylon extends Prefab{ + val position: Vector2D = Vector2D(2,1) + val nodeDensity: Double = 100 + val beamHeight: Double = 1 + val beamWidth: Double = 0.5 + val beamNumber: Int = 10 + + private val nodeRow1 = (for (i <- 0 to beamNumber) yield (new Circle(0.01, nodeDensity) { + pos = position + Vector2D(0, i * beamHeight)}).asBody).toList + private val nodeRow2 = (for (i <- 0 to beamNumber) yield (new Circle(0.01, nodeDensity) { + pos = position + Vector2D(beamWidth, i * beamHeight)}).asBody).toList + + private val beamRow1 = (for (i <- 0 until nodeRow1.length - 1) yield + new DistanceJoint(nodeRow1(i), nodeRow1(i).pos, nodeRow1(i+1), nodeRow1(i + 1).pos)).toList + private val beamRow2 = (for (i <- 0 until nodeRow1.length - 1) yield + new DistanceJoint(nodeRow2(i), nodeRow2(i).pos, nodeRow2(i+1), nodeRow2(i + 1).pos)).toList + + private val latBeams = (for (i <- 0 to beamNumber) yield + new DistanceJoint(nodeRow1(i), nodeRow2(i))).toList + private val diagBeams1 = (for (i <- 0 until beamNumber) yield + new DistanceJoint(nodeRow1(i), nodeRow2(i + 1))).toList + private val diagBeams2 = (for (i <- 0 until beamNumber) yield + new DistanceJoint(nodeRow2(i), nodeRow1(i + 1))).toList + + + + + lazy val nodes = nodeRow1 ++ nodeRow2 + lazy val beams = beamRow1 ++ beamRow2 ++ latBeams ++ diagBeams1 ++ diagBeams2 + + override val bodies = nodes + override val joints = beams + + +} diff --git a/src/sims/prefabs/Ragdoll.scala b/src/sims/prefabs/Ragdoll.scala new file mode 100644 index 0000000..ad58653 --- /dev/null +++ b/src/sims/prefabs/Ragdoll.scala @@ -0,0 +1,43 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.prefabs + +import sims.dynamics._ +import sims.dynamics.joints._ +import sims.geometry._ + +class Ragdoll(position: Vector2D) extends Prefab { + val headTorso = new Body(new Circle(0.1, 1) {pos = position}, + new Rectangle(0.09, 0.35, 1) {pos = position + Vector2D(0, -0.45)}) + val thigh1 = (new Rectangle(0.075, 0.17, 1) {pos = position + Vector2D(0, -0.97)}).asBody + val thigh2 = (new Rectangle(0.075, 0.17, 1) {pos = position + Vector2D(0, -0.97)}).asBody + val tibia1 = (new Rectangle(0.075, 0.25, 1) {pos = position + Vector2D(0, -1.39)}).asBody + val tibia2 = (new Rectangle(0.075, 0.25, 1) {pos = position + Vector2D(0, -1.39)}).asBody + val foot1 = (new Circle(0.08, 1) {pos = position + Vector2D(0, -1.72)}).asBody + val foot2 = (new Circle(0.08, 1) {pos = position + Vector2D(0, -1.72)}).asBody + val upperArm1 = (new Rectangle(0.17, 0.06, 1) {pos = position + Vector2D(0.17, -0.16)}).asBody + val upperArm2 = (new Rectangle(0.17, 0.06, 1) {pos = position + Vector2D(-0.17, -0.16)}).asBody + val forearm1 = (new Rectangle(0.15, 0.06, 1) {pos = position + Vector2D(0.49, -0.16)}).asBody + val forearm2 = (new Rectangle(0.15, 0.06, 1) {pos = position + Vector2D(-0.49, -0.16)}).asBody + val hand1 = (new Circle(0.07, 1) {pos = position + Vector2D(0.71, -0.16)}).asBody + val hand2 = (new Circle(0.07, 1) {pos = position + Vector2D(-0.71, -0.16)}).asBody + + override val bodies = List(headTorso, + thigh1, thigh2, + tibia1, tibia2, + foot1, foot2, + upperArm1, upperArm2, + forearm1, forearm2, + hand1, hand2) + private val shapes = bodies.flatMap(_.shapes) + for (s <- shapes) s.transientShapes ++= shapes + + val shoulder1 = RevoluteJoint(headTorso, upperArm1, position + Vector2D(0, -0.16)) + val shoulder2 = RevoluteJoint(headTorso, upperArm2, position + Vector2D(0, -0.16)) + override val joints = List(shoulder1, shoulder2) + +} diff --git a/src/sims/util/Polar.scala b/src/sims/util/Polar.scala new file mode 100644 index 0000000..931be08 --- /dev/null +++ b/src/sims/util/Polar.scala @@ -0,0 +1,17 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.util + +import sims.geometry._ +import scala.Math._ + +/**Polare Koordinaten.*/ +case class Polar(distance: Double, angle: Double) { + + /**Ergibt die Vektorrepraesantation dieser polaren Koordinaten.*/ + def toCarthesian = Vector2D(distance * sin(angle), distance * cos(angle)) +} diff --git a/src/sims/util/Positioning.scala b/src/sims/util/Positioning.scala new file mode 100644 index 0000000..433feaf --- /dev/null +++ b/src/sims/util/Positioning.scala @@ -0,0 +1,24 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.util + +import sims.geometry._ +import sims.dynamics._ + +/**Objekt mit Hiflsfunktionen fuer komfortables Positionieren von Koerpern.*/ +object Positioning { + + implicit def int2RelativeVector(x: Int): RelativeVector = new RelativeVector(x, 0) + implicit def double2RelativeVector(x: Double): RelativeVector = new RelativeVector(x, 0) + implicit def vector2RelativeVector(v: Vector2D): RelativeVector = new RelativeVector(v.x, v.y) + implicit def polar2Carthesian(p: Polar): Vector2D = p.toCarthesian + implicit def polar2RelativeVector(p: Polar): RelativeVector = vector2RelativeVector(p.toCarthesian) + + def position(s: Shape)(a: Vector2D) = { + s.pos = a + } +} diff --git a/src/sims/util/RelativeVector.scala b/src/sims/util/RelativeVector.scala new file mode 100644 index 0000000..374f92c --- /dev/null +++ b/src/sims/util/RelativeVector.scala @@ -0,0 +1,24 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.util + +import sims.geometry._ +import sims.dynamics._ + +class RelativeVector(val x: Double, val y: Double) { + def above(point: Vector2D): Vector2D = point + Vector2D(0, x) + def below(point: Vector2D): Vector2D = point - Vector2D(0, x) + def left(point: Vector2D): Vector2D = point - Vector2D(x, 0) + def right(point: Vector2D): Vector2D = point + Vector2D(x, 0) + def from(point: Vector2D): Vector2D = point + Vector2D(x, y) + + def above(s: Shape): Vector2D = this.above(s.pos) + def below(s: Shape): Vector2D = this.below(s.pos) + def left(s: Shape): Vector2D = this.left(s.pos) + def right(s: Shape): Vector2D = this.right(s.pos) + def from(s: Shape): Vector2D = this.from(s.pos) +} |