diff options
author | Jakob Odersky <jodersky@gmail.com> | 2015-06-29 12:32:00 +0200 |
---|---|---|
committer | Jakob Odersky <jodersky@gmail.com> | 2015-06-29 12:32:00 +0200 |
commit | 01c5c700647feba596e02cb7a2e672f5301504ff (patch) | |
tree | bb0b0e1c2efde6b5362a294089fe081db0724833 /src/main/scala/sims/collision | |
parent | 998e545e8e42fee597f7274e47428d35dcf29e7b (diff) | |
download | sims-01c5c700647feba596e02cb7a2e672f5301504ff.tar.gz sims-01c5c700647feba596e02cb7a2e672f5301504ff.tar.bz2 sims-01c5c700647feba596e02cb7a2e672f5301504ff.zip |
Port to scala 2.11
Diffstat (limited to 'src/main/scala/sims/collision')
-rw-r--r-- | src/main/scala/sims/collision/AABB.scala | 28 | ||||
-rw-r--r-- | src/main/scala/sims/collision/CircleCollision.scala | 22 | ||||
-rw-r--r-- | src/main/scala/sims/collision/Collision.scala | 108 | ||||
-rw-r--r-- | src/main/scala/sims/collision/Detector.scala | 21 | ||||
-rw-r--r-- | src/main/scala/sims/collision/GridDetector.scala | 123 | ||||
-rw-r--r-- | src/main/scala/sims/collision/Overlap.scala | 11 | ||||
-rw-r--r-- | src/main/scala/sims/collision/Pair.scala | 24 | ||||
-rw-r--r-- | src/main/scala/sims/collision/PolyCircleCollision.scala | 36 | ||||
-rw-r--r-- | src/main/scala/sims/collision/PolyCollision.scala | 53 |
9 files changed, 426 insertions, 0 deletions
diff --git a/src/main/scala/sims/collision/AABB.scala b/src/main/scala/sims/collision/AABB.scala new file mode 100644 index 0000000..f3a0b71 --- /dev/null +++ b/src/main/scala/sims/collision/AABB.scala @@ -0,0 +1,28 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import sims.geometry._ + +/** + * Axis Aligned Bounding Boxes (AABBs) are rectangles that frame a shape. + * Their X-Axis and Y-Axis orientation makes it easy to test two AABBs for overlap. + * @param minVertex Position vector of the bottom-left vertex + * @param maxVertex Position vector of the upper-right vertex + */ +case class AABB(val minVertex: Vector2D, + val maxVertex: Vector2D) +{ + /** + * Checks this AABB with <code>box</code> for overlap. + * @param box AABB with which to check for overlap*/ + 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/main/scala/sims/collision/CircleCollision.scala b/src/main/scala/sims/collision/CircleCollision.scala new file mode 100644 index 0000000..04cf2d7 --- /dev/null +++ b/src/main/scala/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 sims.geometry._ +import sims.dynamics._ + +/**Collision between two circles.*/ +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/main/scala/sims/collision/Collision.scala b/src/main/scala/sims/collision/Collision.scala new file mode 100644 index 0000000..65111c6 --- /dev/null +++ b/src/main/scala/sims/collision/Collision.scala @@ -0,0 +1,108 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import sims.dynamics._ +import sims.geometry._ + +/**Collision between two shapes. Contains methods to compute the collision response.*/ +abstract class Collision extends Constraint { + + /**First colliding shape (reference shape).*/ + val shape1: Shape + + /**Second colliding shape (incident shape).*/ + val shape2: Shape + + /**Collision points.*/ + val points: Iterable[Vector2D] + + /**Normal vector to the collision face.*/ + 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 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 { + + /**Tolerated overlap. Collision response will only be applied if the overlap of two shapes exceeds the tolerated overlap.*/ + val ToleratedOverlap: Double = 0.01 +} diff --git a/src/main/scala/sims/collision/Detector.scala b/src/main/scala/sims/collision/Detector.scala new file mode 100644 index 0000000..96af5dc --- /dev/null +++ b/src/main/scala/sims/collision/Detector.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.geometry._ +import sims.dynamics._ + +/**A world detects its collisions through concrete implementations of this class.*/ +abstract class Detector { + + /**The world whose shapes are to be checked for collisions.*/ + val world: World + + /**Returns all collisions between shapes in the world <code>world</code>.*/ + def collisions: Seq[Collision] +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/GridDetector.scala b/src/main/scala/sims/collision/GridDetector.scala new file mode 100644 index 0000000..2196195 --- /dev/null +++ b/src/main/scala/sims/collision/GridDetector.scala @@ -0,0 +1,123 @@ +/* + * 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.ArrayBuffer +import scala.collection.mutable.HashMap + +/**A concrete implementation of <code>Detector</code>. <code>GridDetector</code> divides the world into a grid + * for faster collision detection.*/ +class GridDetector(override val world: World) extends Detector { + + /**Array of collision detection methods. These methods return <code>true</code> if two shapes are colliding.*/ + val detectionMethods = new ArrayBuffer[PartialFunction[(Shape, Shape), Boolean]] + detectionMethods += { + case (c1: Circle, c2: Circle) => { //collision if distance <= sum of radiuses + 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) => { //distance form center to sides or vertices + 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) => { //distance form center to sides or vertices + val distances = for (s <- p.sides) yield (s distance c.pos) + distances.exists(_ - c.radius <= 0) || (p contains c.pos) + } + } + + /**Array of methods returning collisions. It is assumed that both shapes are colliding.*/ + 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) + } + + /**Checks the pair of shapes <code>p</code> for collision. + * @param p Pair of shapes.*/ + 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!") + } + + /**Returns the collision between both shapes of the pair <code>p</code>. + * @param p Pair of shapes.*/ + 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!") + } + + /**Width and height of a grid cell.*/ + var gridSide: Double = 2 + + /**Returns potential colliding pairs of shapes of the world <code>world</code>. + * <p> + * A potential colliding pair is a pair of two shapes that comply with the following criteria: + * <ul> + * <li>The shapes are situated in the same grid cell.</li> + * <li>Their AABBs overlap.</li> + * <li>The shapes do not belong to the same body.</li> + * <li>At least one shape is not fixed.</li> + * <li>Both shapes are {@link dynamics.Shape#collidable}.</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.valuesIterator) { + 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) + ).distinct + } + ps.toSeq + } + + private var cache = (world.time, getPairs) + + /**All potential colliding pairs of the world. + * @see getPairs*/ + def pairs = {if (world.time != cache._1) cache = (world.time, getPairs); cache._2} + + /**Returns all colliding pairs.*/ + def collidingPairs: Seq[Pair] = for(p <- pairs; if (colliding(p))) yield p + + /**Returns all collisions.*/ + def collisions: Seq[Collision] = for(p <- pairs; if (colliding(p))) yield collision(p) +} diff --git a/src/main/scala/sims/collision/Overlap.scala b/src/main/scala/sims/collision/Overlap.scala new file mode 100644 index 0000000..97ecdd6 --- /dev/null +++ b/src/main/scala/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/main/scala/sims/collision/Pair.scala b/src/main/scala/sims/collision/Pair.scala new file mode 100644 index 0000000..a01fb00 --- /dev/null +++ b/src/main/scala/sims/collision/Pair.scala @@ -0,0 +1,24 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import sims.dynamics._ + +/**Pair of shapes.*/ +case class Pair(s1: Shape, s2: Shape){ + + 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 + } + } +} + +object Pair { + implicit def pair2Tuple(x: Pair) = (x.s1, x.s2) +} diff --git a/src/main/scala/sims/collision/PolyCircleCollision.scala b/src/main/scala/sims/collision/PolyCircleCollision.scala new file mode 100644 index 0000000..20f1d49 --- /dev/null +++ b/src/main/scala/sims/collision/PolyCircleCollision.scala @@ -0,0 +1,36 @@ +/* + * Simple Mechanics Simulator (SiMS) + * copyright (c) 2009 Jakob Odersky + * made available under the MIT License +*/ + +package sims.collision + +import sims.dynamics._ +import sims.geometry._ + +/**Collision between a convex polygon and a circle.*/ +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/main/scala/sims/collision/PolyCollision.scala b/src/main/scala/sims/collision/PolyCollision.scala new file mode 100644 index 0000000..5296f41 --- /dev/null +++ b/src/main/scala/sims/collision/PolyCollision.scala @@ -0,0 +1,53 @@ +/* + * 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 + +/**Collision between two convex polygons.*/ +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 (v <- refPoly.vertices; if incPoly.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) + */ +} |