diff options
Diffstat (limited to 'src/main/scala/sims/collision')
26 files changed, 1583 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..68782d1 --- /dev/null +++ b/src/main/scala/sims/collision/AABB.scala @@ -0,0 +1,55 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import sims.math._ + +/* + * y + * ^ + * | + * | +-------+ + * | | max| + * | | | + * | |min | + * | +-------+ + * | + * 0-------------->x + * + */ + +/** 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) { + + /** Diagonal vector from `minVertex` to `maxVertex`. */ + def diagonal = maxVertex - minVertex + + /** Width of this AABB. */ + def width = maxVertex.x - minVertex.x + + /** Height of this AABB. */ + def height = maxVertex.y - minVertex.y + + /** Checks if the given point is located within this AABB. */ + def contains(point: Vector2D): Boolean = minVertex.x <= point.x && point.x <= maxVertex.x && minVertex.y <= point.y && point.y <= maxVertex.y + + /** Checks if the given AABB is located within this AABB. */ + def contains(box: AABB): Boolean = contains(box.minVertex) && contains(box.maxVertex) + + /** 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/CachedCollidable.scala b/src/main/scala/sims/collision/CachedCollidable.scala new file mode 100644 index 0000000..d3522a4 --- /dev/null +++ b/src/main/scala/sims/collision/CachedCollidable.scala @@ -0,0 +1,30 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +/** Implements features of a collidable object to be evaluated lazily and only + * to be be recomputed when the collidable object moves. */ +trait CachedCollidable extends Collidable { + + /** Invalidates all features and forces their evaluation on next call. + * Should be called by clients when this object moves. */ + def move() = { + _aabbValid = false + } + + private var _aabb: AABB = null + private var _aabbValid = false + abstract override def aabb = { + if (! _aabbValid) { + _aabb = super.aabb + _aabbValid = true + } + _aabb + } + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/Circle.scala b/src/main/scala/sims/collision/Circle.scala new file mode 100644 index 0000000..f40da14 --- /dev/null +++ b/src/main/scala/sims/collision/Circle.scala @@ -0,0 +1,41 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import sims.math.Vector2D + +/** Properties implemented by a collidable circle. + * Note: this class does not define any physical properties, see sims.dynamics.Circle for that. + * @see sims.dynamics.Circle */ +trait Circle extends Collidable { + + /** Position of this circle. */ + def position: Vector2D + + /** Radius of this circle. */ + def radius: Double + + /** Returns this circle's axis aligned bounding box. + * @see collision.AABB */ + override def aabb = new AABB(Vector2D(position.x - radius, position.y - radius), Vector2D(position.x + radius, position.y + radius)) + + /** Checks if the point `point` is contained in this circle. */ + override def contains(point: Vector2D) = (point - position).length <= radius + + /** Returns the projection of this polygon onto the line given by the directional vector `axis`. */ + override def project(axis: Vector2D) = { + val dir = axis.unit + new Projection( + axis, + (position - dir * radius) dot dir, + (position + dir * radius) dot dir + ) + } + + override def support(direction: Vector2D) = position + direction.unit * radius +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/Collidable.scala b/src/main/scala/sims/collision/Collidable.scala new file mode 100644 index 0000000..951ef0e --- /dev/null +++ b/src/main/scala/sims/collision/Collidable.scala @@ -0,0 +1,32 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import sims.math._ + +/** A base trait for all collidable objects. */ +trait Collidable extends AnyRef { + + /** Returns an axis aligned box, bounding this collidable object. */ + def aabb: AABB + + /** Projects this collidable object onto the given axis. */ + def project(axis: Vector2D): Projection + + /** Checks if the point `point` is contained in this collidable object. */ + def contains(point: Vector2D): Boolean + + /** Returns the farthest vertex of this collidable in the given direction. */ + def support(direction: Vector2D): Vector2D + + /** A fixed collidable object cannot collide with other fixed collidable objects. + * This is useful in improving collision detection performance, since a pair of fixed objects will + * be eliminated in the broadphase. */ + def fixed = false + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/Collision.scala b/src/main/scala/sims/collision/Collision.scala new file mode 100644 index 0000000..29af3be --- /dev/null +++ b/src/main/scala/sims/collision/Collision.scala @@ -0,0 +1,19 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import sims.math.Vector2D + +/** Contains information on the collision between two collidable items. */ +abstract class Collision[A <: Collidable] { + val item1: A + val item2: A + def normal: Vector2D + def points: Seq[Vector2D] + def overlap: Double +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/ContactPoint.scala b/src/main/scala/sims/collision/ContactPoint.scala new file mode 100644 index 0000000..1ebe787 --- /dev/null +++ b/src/main/scala/sims/collision/ContactPoint.scala @@ -0,0 +1,11 @@ +package sims.collision + +import sims.math._ + +class ContactPoint[A <: Collidable]( + val item1: A, + val item2: A, + val normal: Vector2D, + val overlap: Double, + val point: Vector2D + )
\ No newline at end of file diff --git a/src/main/scala/sims/collision/ConvexPolygon.scala b/src/main/scala/sims/collision/ConvexPolygon.scala new file mode 100644 index 0000000..3d7a81b --- /dev/null +++ b/src/main/scala/sims/collision/ConvexPolygon.scala @@ -0,0 +1,60 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import sims.math._ + +/** Common properties of all convex polygons. + * '''Note: convex polygons are not verified to be convex. It is up to the client to ensure this.''' */ +trait ConvexPolygon extends Polygon { + + /**Returns the projection of this polygon onto the line given by the directional vector <code>axis</code>. + * @param axis directional vector of the line + * @return projection of this polygon*/ + override def project(axis: Vector2D) = { + val dir = axis.unit + var min = vertices(0) dot dir + var max = vertices(0) dot dir + + for (v <- vertices) { + val d = v dot dir + if (d < min) min = d + if (d > max) max = d + } + + new Projection(axis, min, max) + } + + /**Checks if the point <code>point</code> is contained in this polygon. + * <p> + * A ray is created, originating from the point and following an arbitrary direction (X-Axis was chosen). + * The number of intersections between the ray and this polygon's sides (including vertices) is counted. + * The amount of intersections with vertices is subtracted form the previous number. + * If the latter number is odd, the point is contained in the polygon.*/ + override def contains(point: Vector2D) = { + val r = new Ray(point, Vector2D.i) + var intersections = 0 + for (s <- sides; if !(r intersection s).isEmpty) intersections += 1 + for (v <- vertices; if (r contains v)) intersections -= 1 + intersections % 2 != 0 + } + + override def support(direction: Vector2D) = { + var maxDistance = vertices(0) dot direction + var maxPoint = vertices(0) + for (v <- vertices) { + val s = v dot direction + if (s > maxDistance) { + maxDistance = s + maxPoint = v + } + } + maxPoint + } + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/Detector.scala b/src/main/scala/sims/collision/Detector.scala new file mode 100644 index 0000000..6b0559a --- /dev/null +++ b/src/main/scala/sims/collision/Detector.scala @@ -0,0 +1,81 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import sims.collision.broadphase._ +import sims.collision.narrowphase._ +import scala.collection.mutable.ArrayBuffer + +/** Collision detectors are used to compute collisions between + * a given collection of items. + * They use a [[sims.collision.BroadPhaseDetector]] to determine potentially + * colliding pairs of items. + * These pairs are then examined with a [[sims.collision.NarrowPhaseDetector]] + * to compute the final collisions. + * + * @param broadphase a broadphase collision detector + * @param narrowphase a narrowphase collision detector */ +class Detector[A <: Collidable: ClassManifest]( + broadphase: BroadPhaseDetector[A], + narrowphase: NarrowPhaseDetector[A] + ) { + + /** Collidable items managed by this collision detector. */ + def items: Seq[A] = broadphase.items + + /** Adds an item to this collision detector. */ + def +=(item: A): Unit = broadphase += item + + /** Adds a collection of items to this collision detector. */ + def ++=(items: Iterable[A]) = for (i <- items) this += i + + /** Removes an item from this collision detector. */ + def -=(item: A): Unit = broadphase -= item + + /** Removes a collection of items from this collision detector. */ + def --=(items: Iterable[A]) = for (i <- items) this -= i + + /** Removes all items from this collision detector. */ + def clear(): Unit = broadphase.clear + + /** Indicates the valid state of this collision detector. */ + private var valid = false + + /** Invalidates this detector. The next time `collisions()` is called, all collisions will be + * recomputed. */ + def invalidate() = valid = false + + /** Cache of collisions. */ + private var _collisions = new ArrayBuffer[Collision[A]] + + /** Returns a cached sequence of collisions between all items managed by this collision detector. + * If no collisions were calculated since the last time `invalidate()` was called, the collisions + * will be calculated. */ + def collisions(): Seq[Collision[A]] = { + + if (!valid) { + + _collisions.clear() + + for (pair <- broadphase) { + val collision = narrowphase.collision(pair) + if (collision != None) _collisions += collision.get + } + + valid = true + } + + _collisions + + } + +} + +class DetectorConstructor[A <: Collidable: ClassManifest] (broadphase: BroadPhaseDetector[A]) { + def narrowedBy(narrowphase: NarrowPhaseDetector[A]) = new Detector(broadphase, narrowphase) +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/Intersectable.scala b/src/main/scala/sims/collision/Intersectable.scala new file mode 100644 index 0000000..7232bd3 --- /dev/null +++ b/src/main/scala/sims/collision/Intersectable.scala @@ -0,0 +1,22 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import sims.math._ + +trait Intersectable[A <: Linear] { + + /**Computes the intersection between this linear element and `l`. + * The intersection method does <b>not</b> correspond to the geometrical intersection. + * Let A and B be two linear elements, + * + * A and B intersect (i.e. an intersection point exists) \u21d4 card(A \u22c2 B) = 1 + * */ + def intersection(l: A): Option[Vector2D] + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/Linear.scala b/src/main/scala/sims/collision/Linear.scala new file mode 100644 index 0000000..3c06515 --- /dev/null +++ b/src/main/scala/sims/collision/Linear.scala @@ -0,0 +1,67 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import scala.math._ +import sims.math._ + +/**A base trait for all linear geometric elements specified by one point and a direction. + * @throws IllegalArgumentException if the directional vector is the null vector*/ +trait Linear { + + /**A point contained in this linear element.*/ + val point: Vector2D + + /**Direction vector.*/ + val direction: Vector2D + + /**Unit directional vector.*/ + lazy val direction0 = direction.unit + + /**Right normal vector to <code>direction</code>.*/ + lazy val rightNormal = direction.rightNormal + + /**Right normal unit vector to <code>direction</code>.*/ + lazy val rightNormal0 = rightNormal.unit + + /**Left normal vector to <code>direction</code>.*/ + lazy val leftNormal = direction.leftNormal + + /**Left normal unit vector to <code>direction</code>.*/ + lazy val leftNormal0 = leftNormal.unit + + ///**Computes the closest point on this linear element to a given point.*/ + //def closest(point: Vector2D): Vector2D + + ///**Computes the shortest distance form this linear element to a given point.*/ + //def distance(point: Vector2D) = (closest(point) - point).length + + require(direction != 0, "A linear element's direction cannot be the null vector.") +} + +object Linear { + + /** Clips a segment passing through points `p1` and `p2` to a half plain + * given by a point `p` and a normal (pointing into the plain) `normal`. */ + def clip(p1: Vector2D, p2: Vector2D, p: Vector2D, normal: Vector2D): List[Vector2D] = { + val normal0 = normal.unit + val direction = p2 - p1 + + val d1 = (p1-p) dot normal0 + val d2 = (p2-p) dot normal0 + + if (d1 < 0 && d2 < 0) return Nil + if (d1 >= 0 && d2 >= 0) return List(p1, p2) + + val intersection = p1 + direction * abs(d1) / (abs(d1) + abs(d2)) + val inside = if (d1 > 0) p1 else p2 + + List(inside, intersection) + } + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/Polygon.scala b/src/main/scala/sims/collision/Polygon.scala new file mode 100644 index 0000000..c97b3d3 --- /dev/null +++ b/src/main/scala/sims/collision/Polygon.scala @@ -0,0 +1,32 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import sims.math._ + +/**Common properties of all polygons.*/ +trait Polygon extends Collidable { + + /**Returns positions of all vertices of this Polygon. Vertices are ordered counter-clockwise. + * @return position vectors of the vertices*/ + def vertices: Seq[Vector2D] + + /**Returns all sides of this polygon. The sides are ordered counter-clockwise, the first vertex of the side + * giving the side index.*/ + def sides: Seq[Segment] = for (i <- 0 until vertices.length) yield (new Segment(vertices(i), vertices((i + 1) % vertices.length))) + + /**Returns this polygon's axis aligned bounding box. + * @see collision.AABB*/ + override def aabb = { + val xs = vertices.view map (_.x) + val ys = vertices.view map (_.y) + new AABB(Vector2D(xs.min, ys.min), + Vector2D(xs.max, ys.max)) + } + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/Projection.scala b/src/main/scala/sims/collision/Projection.scala new file mode 100644 index 0000000..7617263 --- /dev/null +++ b/src/main/scala/sims/collision/Projection.scala @@ -0,0 +1,38 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import sims.math._ + +/**Projection on an axis. + * <p> + * Projections are commonly used in SiMS for collision detection. + * @param axis directional vector of the axis of the projection + * @param lower lower value of the projection + * @param upper upper value of the projection*/ +case class Projection(axis: Vector2D, + lower: Double, + upper: Double) { + require(axis != Vector2D.Null, "A projection's axis cannot be given by a null vector!") + require(lower <= upper, "Invalid bounds. Lower must be less than or equal to upper.") + + /**Checks this projection for overlap with another projection. + * @throws IllegalArgumentExcepion if both projections have different axes*/ + 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) + } + + /**Returns the overlap between this projection and another projection. + * @throws IllegalArgumentExcepion if both projections have different axes*/ + def overlap(other: Projection): Double = { + require(axis == other.axis, "Cannot compare two projections on different axes!") + math.min(upper, other.upper) - math.max(lower, other.lower) + + } +} diff --git a/src/main/scala/sims/collision/Ray.scala b/src/main/scala/sims/collision/Ray.scala new file mode 100644 index 0000000..80a63fb --- /dev/null +++ b/src/main/scala/sims/collision/Ray.scala @@ -0,0 +1,62 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import scala.math._ +import sims.math._ + +/**A ray. + * @param point starting point of this ray + * @param direction this ray's directional vector + * @throws IllegalArgumentException if the directional vector is the null vector*/ +case class Ray(point: Vector2D, direction: Vector2D) + extends Linear + with Intersectable[Segment] { + + /*def closest(point: Vector2D) = { + var t = ((point - this.point) dot (direction)) / (direction dot direction) + if (t < 0) t = 0 + this.point + direction * t + }*/ + + def intersection(segment: Segment): Option[Vector2D] = { + + val n = segment.leftNormal + + // Handle case when two segments parallel + if ((n dot direction) == 0) None + else { + val t = (n dot (segment.point1 - point)) / (n dot direction) + val i = point + direction * t + if (0 <= t && (i - segment.point1).length <= segment.length) Some(i) + else None + } + /* + // Returns 2 times the signed triangle area. The result is positive if + // abc is ccw, negative if abc is cw, zero if abc is degenerate. + def signed2DTriArea(a: Vector2D, b: Vector2D, c: Vector2D) = { + (a.x - c.x) * (b.y - c.y) - (a.y - c.y) * (b.x - c.x); + } + + if (signed2DTriArea(point, point + direction, segment.point1) * signed2DTriArea(point, point + direction, segment.point2) < 0) { + val ab = segment.point2 - segment.point1 + val ac = segment.point2 - point + val t = (ac.x * ab.y - ac.y * ab.x) / (direction.y * ab.x - direction.x - ab.y) + if (t >= 0) Some(point + direction * t) else None + } else None*/ + } + + /**Checks if this ray contains the point <code>p</code>.*/ + def contains(p: Vector2D) = { + val v = p - point + p == point || + v ~ direction && + signum(direction.x) == signum(v.x) && + signum(direction.y) == signum(v.y) + } +} diff --git a/src/main/scala/sims/collision/Segment.scala b/src/main/scala/sims/collision/Segment.scala new file mode 100644 index 0000000..45bd7b9 --- /dev/null +++ b/src/main/scala/sims/collision/Segment.scala @@ -0,0 +1,117 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision + +import scala.math._ +import sims.math._ + +/** A segment passing through two points. + * + * @param point1 position vector of the first point + * @param point2 position vector of the second point + * @throws IllegalArgumentException if both vertices are equal */ +case class Segment(point1: Vector2D, point2: Vector2D) + extends Linear + with Intersectable[Segment] { + + require(point1 != point2, "A segment must have two distinct vertices.") + + val point = point1 + + /**Vector from <code>vertex1</code> to <code>vertex2</code>.*/ + val direction = point2 - point + + /**Length of this segment.*/ + val length = direction.length + + + def closest(point: Vector2D) = { + var t = ((point - point1) dot (direction)) / (direction dot direction) + if (t < 0) t = 0 + if (t > 1) t = 1 + point1 + direction * t + } + + def distance(p: Vector2D) = { + // For more concise code, the following substitutions are made: + // * point1 -> a + // * point2 -> b + // * p -> c + + val ab = direction + val ac = p - point1 + val bc = p - point2 + + val e = ac dot ab + val distanceSquare = + // Handle cases where c projects outside ab + if (e <= 0) ac dot ac + else if (e >= (ab dot ab)) bc dot bc + // Handle cases where c projects onto ab + else (ac dot ac) - e * e / (ab dot ab) + + math.sqrt(distanceSquare) + } + + def clipped(reference: Segment): List[Vector2D] = { + val clipped = Linear.clip(this.point1, this.point2, reference.point1, reference.direction) + if (clipped.length == 0) Nil + else Linear.clip(clipped(0), clipped(1), reference.point2, -reference.direction) + } + + + def intersection(segment: Segment): Option[Vector2D] = { + val n = segment.leftNormal + + // Handle case when two segments parallel + if ((n dot direction) == 0) None + else { + val t = (n dot (segment.point1 - point1)) / (n dot direction) + val i = point + direction * t + if (0 <= t && t <= 1 && (i - segment.point1).length <= segment.length) Some(i) + else None + } + /* + // Returns 2 times the signed triangle area. The result is positive if + // abc is ccw, negative if abc is cw, zero if abc is degenerate. + def signed2DTriArea(a: Vector2D, b: Vector2D, c: Vector2D) = { + (a.x - c.x) * (b.y - c.y) - (a.y - c.y) * (b.x - c.x); + } + + val a = point1; val b = point2; val c = segment.point; val d = segment.point2 + + // Sign of areas correspond to which side of ab points c and d are + val a1 = signed2DTriArea(a, b, d); // Compute winding of abd (+ or -) + val a2 = signed2DTriArea(a, b, c); // To intersect, must have sign opposite of a1 + + // If c and d are on different sides of ab, areas have different signs + if (a1 * a2 < 0.0f) { + // Compute signs for a and b with respect to segment cd + val a3 = signed2DTriArea(c, d, a); // Compute winding of cda (+ or -) + // Since area is constant a1-a2 = a3-a4, or a4=a3+a2-a1 + // float a4 = Signed2DTriArea(c, d, b); // Must have opposite sign of a3 + val a4 = a3 + a2 - a1; + // Points a and b on different sides of cd if areas have different signs + if (a3 * a4 < 0.0f) { + // Segments intersect. Find intersection point along L(t)=a+t*(b-a). + // Given height h1 of a over cd and height h2 of b over cd, + // t = h1 / (h1 - h2) = (b*h1/2) / (b*h1/2 - b*h2/2) = a3 / (a3 - a4), + // where b (the base of the triangles cda and cdb, i.e., the length + // of cd) cancels out. + val t = a3 / (a3 - a4); + val p = a + (b - a) * t; + return Some(p); + } + } + // Segments not intersecting (or collinear) + return None; + */ + } + + +} diff --git a/src/main/scala/sims/collision/broadphase/BroadPhaseDetector.scala b/src/main/scala/sims/collision/broadphase/BroadPhaseDetector.scala new file mode 100644 index 0000000..637da7a --- /dev/null +++ b/src/main/scala/sims/collision/broadphase/BroadPhaseDetector.scala @@ -0,0 +1,39 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision.broadphase + +import sims.collision._ +import scala.collection.mutable.ArrayBuffer + +abstract class BroadPhaseDetector[A <: Collidable: ClassManifest] { + + protected var _items = new ArrayBuffer[A] + + /** Collidable items managed by this collision detector. */ + def items: Seq[A] = _items + + /** Adds an item to this collision detector. */ + def +=(item: A) = _items += item + + /** Adds a collection of items to this collision detector. */ + def ++=(items: Iterable[A]) = for (i <- items) this += i + + /**Removes an item from this collision detector. */ + def -=(item: A) = _items -= item + + /**Removes a collection of items from this collision detector. */ + def --=(items: Iterable[A]) = for (i <- items) this -= i + + /**Removes all items from this collision detector. */ + def clear() = _items.clear + + /** Applies a given function to every potentially colliding pair. + * @param f function applied to every potentially colliding pair */ + def foreach(f: ((A, A)) => Unit) + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/broadphase/SAP.scala b/src/main/scala/sims/collision/broadphase/SAP.scala new file mode 100644 index 0000000..664f620 --- /dev/null +++ b/src/main/scala/sims/collision/broadphase/SAP.scala @@ -0,0 +1,120 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision.broadphase + +import sims.collision._ +import scala.collection.mutable.ArrayBuffer + +/** A broadphase collision detector implementing the "Sweep and Prune" algorithm. + * + * The implementation of the broadphase algorithm was adapted from + * Real-Time Collision Detection by Christer Ericson, published by Morgan Kaufmann Publishers, (c) 2005 Elsevier Inc */ +class SAP[A <: Collidable: ClassManifest] extends BroadPhaseDetector[A]{ + + /*ordering along `axis` + * x axis => 0 + * y axis => 1 */ + private var axis = 0 + + //use insert sort + private var almostSorted = false + private var sortedCount = 0 + private val threshold = 3 + + private implicit def ordering: Ordering[A] = new Ordering[A] { + def compare(x: A, y: A) = { + val delta = x.aabb.minVertex.components(axis) - y.aabb.minVertex.components(axis) + if (delta < 0) -1 + else if(delta > 0) 1 + else 0 + } + } + + private def insertionSort(a: ArrayBuffer[A])(implicit ord: Ordering[A]) = { + import ord._ + val length = a.length + var i = 1; while(i < length) { + var j = i + val t = a(j); + while (j>0 && a(j-1) > t) { + a(j)=a(j-1) + j -= 1 + } + a(j)=t; + i += 1 + } + } + + def foreach(f: ((A, A)) => Unit): Unit = { + + if (almostSorted) + insertionSort(_items) + else + _items = _items.sorted //quicksort + + var sumX, sumY = 0.0 + var sumX2, sumY2 = 0.0 + var varianceX, varianceY = 0.0 + + var i = 0; while (i < _items.length) { + + //center point + val px = (_items(i).aabb.minVertex.x + _items(i).aabb.maxVertex.x) / 2 + val py = (_items(i).aabb.minVertex.y + _items(i).aabb.maxVertex.y) / 2 + + //update sum and sum2 for computing variance of AABB centers + sumX += px; sumY += py + sumX2 += px * px; sumY2 += py * py + + //collision test + var j = i + 1; var break = false; while(!break && j < _items.length) { + + //stop when tested AABBs are beyond the end of current AABB + if (_items(j).aabb.minVertex.components(axis) > _items(i).aabb.maxVertex.components(axis)) + break = true + + //collision test here + else if ( + (_items(i).fixed == false || _items(j).fixed == false) && + (_items(i).aabb overlaps _items(j).aabb) + ) f(_items(i), _items(j)) + + j += 1 + } + + i += 1 + } + + varianceX = sumX2 - sumX * sumX + varianceY = sumY2 - sumY * sumY + + //choose sorting axis with greatest variance + var newAxis = 0 + if (varianceX < varianceY) newAxis = 1 + + if (axis == newAxis) + sortedCount += 1 + else sortedCount = 0 + + almostSorted = sortedCount > threshold + //if sorting axis changes, items will no longer be almost sorted + //and thus quicksort should be used to reorder them + //almostSorted = axis == newAxis + + //update sorting axis + axis = newAxis + + } + +} + +object SAP { + + def apply[A <: Collidable: ClassManifest] = new SAP[A] + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/narrowphase/NarrowPhaseDetector.scala b/src/main/scala/sims/collision/narrowphase/NarrowPhaseDetector.scala new file mode 100644 index 0000000..783d366 --- /dev/null +++ b/src/main/scala/sims/collision/narrowphase/NarrowPhaseDetector.scala @@ -0,0 +1,16 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision.narrowphase + +import sims.collision._ + +abstract class NarrowPhaseDetector[A <: Collidable: ClassManifest] { + + def collision(pair: (A, A)): Option[Collision[A]] + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/narrowphase/SAT.scala b/src/main/scala/sims/collision/narrowphase/SAT.scala new file mode 100644 index 0000000..2e276b8 --- /dev/null +++ b/src/main/scala/sims/collision/narrowphase/SAT.scala @@ -0,0 +1,236 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.collision.narrowphase + +import sims.collision._ +import sims.math._ + +class SAT[A <: Collidable: ClassManifest] extends NarrowPhaseDetector[A] { + + def collision(pair: (A, A)): Option[Collision[A]] = { + var c = getCollision(pair) + c orElse getCollision(pair.swap) + } + + private def getCollision(pair: (A,A)): Option[Collision[A]] = pair match { + + case (c1: Circle, c2: Circle) => + collisionCircleCircle(c1, c2)(pair) + + case (p1: ConvexPolygon, p2: ConvexPolygon) => + sat(pair, p1.sides.map(_.rightNormal0) ++ p2.sides.map(_.rightNormal0)) + + case _ => None + + } + + private def collisionCircleCircle(c1: Circle, c2: Circle)(pair: (A, A)) = { + val d = (c2.position - c1.position) + val l = d.length + if (l <= c1.radius + c2.radius) { + val p = c1.position + d.unit * (l - c2.radius) + Some(new Collision[A] { + val item1 = pair._1 + val item2 = pair._2 + val normal = d.unit + val points = List(p) + val overlap = (c1.radius + c2.radius - l) + }) + } else None + } + + private def collisionPolyPoly(p1: ConvexPolygon, p2: ConvexPolygon)(pair: (A, A)): Option[Collision[A]] = { + var minOverlap = Double.PositiveInfinity + var reference: ConvexPolygon = null + var incident: ConvexPolygon = null + var referenceSide: Segment = null + var incidentVerticeNumber = 0 + + for (i <- 0 until p1.sides.length) { + var overlaps = false + + for (j <- 0 until p2.vertices.length) { + val s = p1.sides(i) + val v = p2.vertices(j) + + val overlap = (s.rightNormal0 dot s.point1) - (s.rightNormal0 dot v) + if (overlap > 0) overlaps = true + if (overlap > 0 && overlap < minOverlap.abs) { + minOverlap = overlap + + reference = p1 + referenceSide = s + incident = p2 + incidentVerticeNumber = j + } + } + if (!overlaps) return None + } + + for (i <- 0 until p2.sides.length) { + var overlaps = false + + for (j <- 0 until p1.vertices.length) { + val s = p2.sides(i) + val v = p1.vertices(j) + + val overlap = (s.rightNormal0 dot s.point1) - (s.rightNormal0 dot v) + if (overlap > 0) overlaps = true + if (overlap > 0 && overlap < minOverlap.abs) { + minOverlap = overlap + + reference = p2 + referenceSide = s + incident = p1 + incidentVerticeNumber = j + } + } + if (!overlaps) return None + } + + val i = incidentVerticeNumber + val side1 = incident.sides(i) + val side2 = incident.sides(mod(i-1, incident.sides.length)) + + val incidentSide = if ((side1.direction dot referenceSide.rightNormal0).abs < + (side2.direction dot referenceSide.rightNormal0).abs) side1 + else side2 + + val clipped: Segment = null //incidentSide clippedToSegment referenceSide + + Some(new Collision[A] { + val item1 = reference.asInstanceOf[A] + val item2 = incident.asInstanceOf[A] + val normal = referenceSide.rightNormal0 + val points = List(clipped.point1, clipped.point2) + val overlap = minOverlap + + }) + + } + + + + + def farthestFeature(collidable: Collidable, direction: Vector2D): Either[Vector2D, Segment] = collidable match { + + case c: Circle => Left(c.position + direction.unit * c.radius) + + case p: ConvexPolygon => { + var max = p.vertices(0) dot direction.unit + + //maximum vertice index + var i = 0 + + for (j <- 0 until p.vertices.length) { + val d = p.vertices(j) dot direction.unit + if (d > max) { + max = d + i = j + } + } + + /* 1) vertex is considered to be the first point of a segment + * 2) polygons vertices are ordered counter-clockwise + * + * implies: + * previous segment is the (i-1)th segment + * next segment is the ith segment */ + val prev = if (i == 0) p.sides.last else p.sides(i - 1) + val next = p.sides(i) + + // check which segment is less parallel to direction + val side = + if ((prev.direction.unit dot direction).abs <= (next.direction.unit dot direction).abs) prev + else next + + Right(side) + } + + case _ => throw new IllegalArgumentException("Collidable is of unknown type.") + + } + + + + //normal is considered pointing from _1 to _2 + //_1 reference, _2 incident + def getCollisionPoints(pair: (A, A), normal: Vector2D, overlap: Double): Array[Vector2D] = { + var points = new scala.collection.mutable.ArrayBuffer[Vector2D] + + val feature1 = farthestFeature(pair._1, normal) + + //is feature 1 a vertex? + if (feature1.isLeft) { + return Array(feature1.left.get) + } + + val feature2 = farthestFeature(pair._2, -normal) + + //is feature 2 a vertex? + if (feature2.isLeft) { + return Array(feature2.left.get) + } + + //neither feature is a vertex + val side1 = feature1.right.get + val side2 = feature2.right.get + + + val flipped = (side1.direction.unit dot normal).abs > (side2.direction.unit dot -normal).abs + val reference = if (!flipped) side1 else side2 + val incident = if (!flipped) side2 else side1 + + + //both features are sides, clip feature2 to feature1 + val clipped: Option[Segment] = None //incident clipped reference + + clipped match { + case None => Array() + case Some(Segment(point1, point2)) => Array(point1, point2) filter ((v: Vector2D) => ((v - reference.point1) dot reference.rightNormal0) <= 0) + } + } + + def sat(pair: (A, A), axes: Seq[Vector2D]): Option[Collision[A]] = { + var min = Double.PositiveInfinity + var n = axes(0) + + for (axis <- axes) { + val overlap = pair._1.project(axis) overlap pair._2.project(axis) + if (overlap < 0) return None + if (overlap < min) { + min = overlap + n = axis + } + } + + val pts = getCollisionPoints(pair, n, min) + + if (pts.length == 0) return None + + Some(new Collision[A] { + val item1 = pair._1 + val item2 = pair._2 + val normal = n + val points = pts.toSeq + val overlap = min + }) + + + + } + + + +} + +object SAT { + + def apply[A <: Collidable: ClassManifest] = new SAT[A] + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/narrowphase/gjk/CS.scala b/src/main/scala/sims/collision/narrowphase/gjk/CS.scala new file mode 100644 index 0000000..0d97b37 --- /dev/null +++ b/src/main/scala/sims/collision/narrowphase/gjk/CS.scala @@ -0,0 +1,83 @@ +package sims.collision.narrowphase +package gjk + +import scala.math._ +import sims.collision._ +import sims.math._ + +object CS { + + def farthestFeature(collidable: Collidable, direction: Vector2D): Either[Vector2D, Segment] = collidable match { + case c: Circle => Left(c.support(direction)) + + case p: ConvexPolygon => { + var max = p.vertices(0) dot direction.unit + + //maximum vertice index + var i = 0 + + for (j <- 0 until p.vertices.length) { + val d = p.vertices(j) dot direction.unit + if (d > max) { + max = d + i = j + } + } + + /* 1) vertex is considered to be the first point of a segment + * 2) polygons vertices are ordered counter-clockwise + * + * implies: + * previous segment is the (i-1)th segment + * next segment is the ith segment */ + val prev = if (i == 0) p.sides.last else p.sides(i - 1) + val next = p.sides(i) + + // check which segment is less parallel to direction + val side = + if ((prev.direction0 dot direction).abs <= (next.direction0 dot direction).abs) prev + else next + + Right(side) + } + + case _ => throw new IllegalArgumentException("Collidable is of unknown type.") + + } + + def getCollisionPoints(pair: (Collidable, Collidable), normal: Vector2D): Manifold = { + var points = new scala.collection.mutable.ArrayBuffer[Vector2D] + + val feature1 = farthestFeature(pair._1, normal) + + //is feature 1 a vertex? + if (feature1.isLeft) { + return new Manifold(List(feature1.left.get), normal) + } + + val feature2 = farthestFeature(pair._2, -normal) + + //is feature 2 a vertex? + if (feature2.isLeft) { + return new Manifold(List(feature2.left.get), -normal) + } + + //neither feature is a vertex + val side1 = feature1.right.get + val side2 = feature2.right.get + + + val flipped = (side1.direction0 dot normal).abs > (side2.direction0 dot normal).abs + val reference = if (!flipped) side1 else side2 + val incident = if (!flipped) side2 else side1 + val n = if (!flipped) normal else -normal + + //both features are sides, clip feature2 to feature1 + val clipped = incident clipped reference + + val c = clipped filter ((v: Vector2D) => ((v - reference.point1) dot n) > 0) + + new Manifold(c, n) + } + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/narrowphase/gjk/EPA.scala b/src/main/scala/sims/collision/narrowphase/gjk/EPA.scala new file mode 100644 index 0000000..9f53690 --- /dev/null +++ b/src/main/scala/sims/collision/narrowphase/gjk/EPA.scala @@ -0,0 +1,98 @@ +package sims.collision.narrowphase +package gjk + +import scala.collection.mutable.ListBuffer +import sims.collision._ +import sims.math._ + +/** The implementation was adapted from dyn4j by William Bittle (see http://www.dyn4j.org). */ +object EPA { + + val MaxIterations = 20 + + val DistanceEpsilon = 0.0001 + + private class Edge(val normal: Vector2D, val distance: Double, val index: Int) + + private def winding(simplex: ListBuffer[Vector2D]) = { + for (i <- 0 until simplex.size) { + val j = if (i + 1 == simplex.size) 0 else i + 1 + //val winding = math.signum(simplex(j) - simplex(i)) + } + + } + + + def penetration(simplex: ListBuffer[Vector2D], minkowskiSum: MinkowskiSum): Penetration = { + // this method is called from the GJK detect method and therefore we can assume + // that the simplex has 3 points + + // get the winding of the simplex points + // the winding may be different depending on the points added by GJK + // however EPA will preserve the winding so we only need to compute this once + val winding = math.signum((simplex(1) - simplex(0)) cross (simplex(2) - simplex(1))).toInt + // store the last point added to the simplex + var point = Vector2D.Null + // the current closest edge + var edge: Edge = null; + // start the loop + for (i <- 0 until MaxIterations) { + // get the closest edge to the origin + edge = this.findClosestEdge(simplex, winding); + // get a new support point in the direction of the edge normal + point = minkowskiSum.support(edge.normal); + + // see if the new point is significantly past the edge + val projection: Double = point dot edge.normal + if ((projection - edge.distance) < DistanceEpsilon) { + // then the new point we just made is not far enough + // in the direction of n so we can stop now and + // return n as the direction and the projection + // as the depth since this is the closest found + // edge and it cannot increase any more + return new Penetration(edge.normal, projection) + } + + // lastly add the point to the simplex + // this breaks the edge we just found to be closest into two edges + // from a -> b to a -> newPoint -> b + simplex.insert(edge.index, point); + } + // if we made it here then we know that we hit the maximum number of iterations + // this is really a catch all termination case + // set the normal and depth equal to the last edge we created + new Penetration(edge.normal, point dot edge.normal) + } + + @inline private def findClosestEdge(simplex: ListBuffer[Vector2D], winding: Int): Edge = { + // get the current size of the simplex + val size = simplex.size; + // create an edge + var edge = new Edge(Vector2D.Null, Double.PositiveInfinity, 0) + // find the edge on the simplex closest to the origin + for (i <- 0 until size) { + // compute j + val j = if (i + 1 == size) 0 else i + 1 + // get the points that make up the current edge + val a = simplex(i); + val b = simplex(j); + // create the edge + val direction = simplex(j) - simplex(i); + // depending on the winding get the edge normal + // it would findClosestEdge(List<Vector2> simplex, int winding) { + // get the current size of the simplex + val normal = if (winding > 0) direction.rightNormal.unit + else direction.leftNormal.unit + + // project the first point onto the normal (it doesnt matter which + // you project since the normal is perpendicular to the edge) + val d: Double = math.abs(simplex(i) dot normal); + // record the closest edge + if (d < edge.distance) + edge = new Edge(normal, d, j) + } + // return the closest edge + edge + } + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/narrowphase/gjk/GJK.scala b/src/main/scala/sims/collision/narrowphase/gjk/GJK.scala new file mode 100644 index 0000000..71ccee3 --- /dev/null +++ b/src/main/scala/sims/collision/narrowphase/gjk/GJK.scala @@ -0,0 +1,112 @@ +package sims.collision.narrowphase +package gjk + +import sims.collision._ +import sims.math._ +import scala.collection.mutable.ListBuffer + +class GJK[A <: Collidable: ClassManifest] extends narrowphase.NarrowPhaseDetector[A] { + + + def penetration(pair: (A, A)): Option[Penetration] = { + val ms = new MinkowskiSum(pair) + val s = ms.support(Vector2D.i) + val simplex = new ListBuffer[Vector2D] + simplex prepend s + var direction = -s + + while (true) { + + val a = ms.support(direction) + + if ((a dot direction) < 0) return None + + simplex prepend a + + val newDirection = checkSimplex(simplex, direction) + + if (newDirection == null) return Some(EPA.penetration(simplex, ms)) + else direction = newDirection + + } + + throw new IllegalArgumentException("Something went wrong, should not reach here.") + } + + /** Checks whether the given simplex contains the origin. If it does, `null` is returned. + * Otherwise a new search direction is returned and the simplex is updated. */ + private def checkSimplex(simplex: ListBuffer[Vector2D], direction: Vector2D): Vector2D = { + if (simplex.length == 2) { //simplex == 2 + val a = simplex(0) + val b = simplex(1) + val ab = b - a + val ao = -a + + if (ao directionOf ab) { + ab cross ao cross ab + } else { + simplex.remove(1) + ao + } + } // end simplex == 2 + + else if (simplex.length == 3) { //simplex == 3 + val a = simplex(0) + val b = simplex(1) + val c = simplex(2) + val ab = b - a + val ac = c - a + val ao = -a + val winding = ab cross ac + + if (ao directionOf (ab cross winding)) { + if (ao directionOf ab) { + simplex.remove(2) + ab cross ao cross ab + } else if (ao directionOf ac) { + simplex.remove(1) + ac cross ao cross ac + } else { + simplex.remove(2) + simplex.remove(1) + ao + } + } else { + if (ao directionOf (winding cross ac)) { + if (ao directionOf ac) { + simplex.remove(1) + ac cross ao cross ac + } else { + simplex.remove(2) + simplex.remove(1) + ao + } + } else { + null + } + } + } //end simplex == 3 + + else throw new IllegalArgumentException("Invalid simplex size.") + + } + + def collision(pair: (A, A)): Option[Collision[A]] = { + val p = penetration(pair) + if (p.isEmpty) return None + val manif = CS.getCollisionPoints(pair, p.get.normal) + Some(new Collision[A] { + val item1 = pair._1 + val item2 = pair._2 + val normal = manif.normal + val overlap = p.get.overlap + val points = manif.points + }) + + } + +} + +object GJK { + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/narrowphase/gjk/GJK2.scala b/src/main/scala/sims/collision/narrowphase/gjk/GJK2.scala new file mode 100644 index 0000000..81764d4 --- /dev/null +++ b/src/main/scala/sims/collision/narrowphase/gjk/GJK2.scala @@ -0,0 +1,168 @@ +package sims.collision.narrowphase.gjk + +/** + * Created by IntelliJ IDEA. + * User: jakob + * Date: 3/27/11 + * Time: 7:47 PM + * To change this template use File | Settings | File Templates. + */ + +import scala.collection.mutable.ListBuffer +import sims.math._ +import sims.collision._ +import sims.collision.narrowphase.NarrowPhaseDetector + +case class Separation(distance: Double, normal: Vector2D, point1: Vector2D, point2: Vector2D) +class GJK2[A <: Collidable: ClassManifest] extends NarrowPhaseDetector[A] { + + val margin = 0.01 + + case class MinkowskiPoint(convex1: Collidable, convex2: Collidable, direction: Vector2D) { + val point1 = convex1.support(direction) - direction.unit * margin + val point2 = convex2.support(-direction) + direction.unit * margin + val point = point1 - point2 + } + + def support(c1: A, c2: A, direction: Vector2D) = MinkowskiPoint(c1, c2, direction) + implicit def minkowsi2Vector(mp: MinkowskiPoint) = mp.point + + def separation(c1: A, c2: A): Option[(Separation)] = { + + //initial search direction + val direction0 = Vector2D.i + + //simplex points + var a = support(c1, c2, direction0) + var b = support(c1, c2, -direction0) + + var counter = 0 + while (counter < 100) { + + //closest point on the current simplex closest to origin + val point = segmentClosestPoint(a, b,Vector2D.Null) + + if (point.isNull) return None + + //new search direction + val direction = -point.unit + + //new Minkowski Sum point + val c = support(c1, c2, direction) + + if (containsOrigin(a, b, c)) return None + + val dc = (direction dot c) + val da = (direction dot a) + + if (dc - da < 0.0001) { + val (point1, point2) = findClosestPoints(a, b) + return Some(Separation(dc, direction, point1, point2)) + } + + if (a.lengthSquare < b.lengthSquare) b = c + else a = c + //counter += 1 + } + return None + } + + + def findClosestPoints(a: MinkowskiPoint, b: MinkowskiPoint): (Vector2D, Vector2D) = { + var p1 = Vector2D.Null + var p2 = Vector2D.Null + + // find lambda1 and lambda2 + val l: Vector2D = b - a + + // check if a and b are the same point + if (l.isNull) { + // then the closest points are a or b support points + p1 = a.point1 + p2 = a.point2 + } else { + // otherwise compute lambda1 and lambda2 + val ll = l dot l; + val l2 = -l.dot(a) / ll; + val l1 = 1 - l2; + + // check if either lambda1 or lambda2 is less than zero + if (l1 < 0) { + // if lambda1 is less than zero then that means that + // the support points of the Minkowski point B are + // the closest points + p1 = b.point1 + p2 = b.point2 + } else if (l2 < 0) { + // if lambda2 is less than zero then that means that + // the support points of the Minkowski point A are + // the closest points + p1 = a.point1 + p2 = a.point2 + } else { + // compute the closest points using lambda1 and lambda2 + // this is the expanded version of + // p1 = a.p1.multiply(l1).add(b.p1.multiply(l2)); + // p2 = a.p2.multiply(l1).add(b.p2.multiply(l2)); + p1 = a.point1 * l1 + b.point1 * l2 + p2 = a.point2 * l1 + b.point2 * l2 + } + } + + (p1, p2) + } + + def segmentClosestPoint(a: Vector2D, b: Vector2D, point: Vector2D): Vector2D = { + if (a == b) return a + val direction = b - a + var t = ((point - a) dot (direction)) / (direction dot direction) + if (t < 0) t = 0 + if (t > 1) t = 1 + a + direction * t + } + + def containsOrigin(a: Vector2D, b: Vector2D, c: Vector2D): Boolean = { + val sa = a.cross(b); + val sb = b.cross(c); + val sc = c.cross(a); + // this is sufficient (we do not need to test sb * sc) + sa * sb > 0 && sa * sc > 0 + } + + def collision(pair: (A, A)): Option[Collision[A]] = { + pair match { + case (c1: Circle, c2: Circle) => collisionCircleCircle(c1, c2)(pair) + case _ => gjkCollision(pair) + } + } + + private def gjkCollision(pair: (A, A)): Option[Collision[A]] = { + val so = separation(pair._1, pair._2) + if (so.isEmpty) return None //deep contact is not implemented yet + val s = so.get + Some(new Collision[A] { + val item1 = pair._1 + val item2 = pair._2 + val overlap = -(s.distance - 2 * margin) + val points = List(s.point1) + val normal = s.normal.unit + }) + + } + + private def collisionCircleCircle(c1: Circle, c2: Circle)(pair: (A, A)) = { + val d = (c2.position - c1.position) + val l = d.length + if (l <= c1.radius + c2.radius) { + val p = c1.position + d.unit * (l - c2.radius) + Some(new Collision[A] { + val item1 = pair._1 + val item2 = pair._2 + val normal = d.unit + val points = List(p) + val overlap = (c1.radius + c2.radius - l) + }) + } else None + } + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/narrowphase/gjk/Manifold.scala b/src/main/scala/sims/collision/narrowphase/gjk/Manifold.scala new file mode 100644 index 0000000..fb9a433 --- /dev/null +++ b/src/main/scala/sims/collision/narrowphase/gjk/Manifold.scala @@ -0,0 +1,5 @@ +package sims.collision.narrowphase.gjk + +import sims.math._ + +class Manifold(val points: Seq[Vector2D], val normal: Vector2D)
\ No newline at end of file diff --git a/src/main/scala/sims/collision/narrowphase/gjk/MinkowskiSum.scala b/src/main/scala/sims/collision/narrowphase/gjk/MinkowskiSum.scala new file mode 100644 index 0000000..f59905b --- /dev/null +++ b/src/main/scala/sims/collision/narrowphase/gjk/MinkowskiSum.scala @@ -0,0 +1,12 @@ +package sims.collision.narrowphase +package gjk + +import sims.collision._ +import sims.math._ + +class MinkowskiSum(pair: (Collidable, Collidable)) { + + def support(direction: Vector2D) = + pair._1.support(direction) - pair._2.support(-direction) + +}
\ No newline at end of file diff --git a/src/main/scala/sims/collision/narrowphase/gjk/Penetration.scala b/src/main/scala/sims/collision/narrowphase/gjk/Penetration.scala new file mode 100644 index 0000000..0ecfcb3 --- /dev/null +++ b/src/main/scala/sims/collision/narrowphase/gjk/Penetration.scala @@ -0,0 +1,9 @@ +package sims.collision.narrowphase.gjk + +import sims.collision._ +import sims.math._ + +class Penetration( + val normal: Vector2D, + val overlap: Double + )
\ No newline at end of file diff --git a/src/main/scala/sims/collision/package.scala b/src/main/scala/sims/collision/package.scala new file mode 100644 index 0000000..aa19a5d --- /dev/null +++ b/src/main/scala/sims/collision/package.scala @@ -0,0 +1,18 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims + +import sims.collision.broadphase._ +import sims.collision.narrowphase._ + +package object collision { + + implicit def broadphaseToConstructor[A <: Collidable: ClassManifest](b: BroadPhaseDetector[A]) = + new DetectorConstructor(b) + +}
\ No newline at end of file |