diff options
Diffstat (limited to 'src/main/scala/sims/collision/narrowphase')
9 files changed, 739 insertions, 0 deletions
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 |