From 2750bc0277c3d929603daceee2e8a1e88368a306 Mon Sep 17 00:00:00 2001 From: Jakob Odersky Date: Fri, 26 Aug 2011 20:29:25 +0200 Subject: import from local directory --- .gitignore | 2 + LICENSE | 24 +++ README | 0 build.sbt | 7 + lib/processing/core.jar | Bin 0 -> 175618 bytes src/main/scala/sims/collision/AABB.scala | 55 +++++ .../scala/sims/collision/CachedCollidable.scala | 30 +++ src/main/scala/sims/collision/Circle.scala | 41 ++++ src/main/scala/sims/collision/Collidable.scala | 32 +++ src/main/scala/sims/collision/Collision.scala | 19 ++ src/main/scala/sims/collision/ContactPoint.scala | 11 + src/main/scala/sims/collision/ConvexPolygon.scala | 60 ++++++ src/main/scala/sims/collision/Detector.scala | 81 +++++++ src/main/scala/sims/collision/Intersectable.scala | 22 ++ src/main/scala/sims/collision/Linear.scala | 67 ++++++ src/main/scala/sims/collision/Polygon.scala | 32 +++ src/main/scala/sims/collision/Projection.scala | 38 ++++ src/main/scala/sims/collision/Ray.scala | 62 ++++++ src/main/scala/sims/collision/Segment.scala | 117 ++++++++++ .../collision/broadphase/BroadPhaseDetector.scala | 39 ++++ src/main/scala/sims/collision/broadphase/SAP.scala | 120 +++++++++++ .../narrowphase/NarrowPhaseDetector.scala | 16 ++ .../scala/sims/collision/narrowphase/SAT.scala | 236 +++++++++++++++++++++ .../scala/sims/collision/narrowphase/gjk/CS.scala | 83 ++++++++ .../scala/sims/collision/narrowphase/gjk/EPA.scala | 98 +++++++++ .../scala/sims/collision/narrowphase/gjk/GJK.scala | 112 ++++++++++ .../sims/collision/narrowphase/gjk/GJK2.scala | 168 +++++++++++++++ .../sims/collision/narrowphase/gjk/Manifold.scala | 5 + .../collision/narrowphase/gjk/MinkowskiSum.scala | 12 ++ .../collision/narrowphase/gjk/Penetration.scala | 9 + src/main/scala/sims/collision/package.scala | 18 ++ src/main/scala/sims/dsl/BodyPoint.scala | 20 ++ src/main/scala/sims/dsl/RichBody.scala | 33 +++ src/main/scala/sims/dsl/dsl.scala | 17 ++ src/main/scala/sims/dynamics/Body.scala | 113 ++++++++++ src/main/scala/sims/dynamics/Circle.scala | 24 +++ src/main/scala/sims/dynamics/Collision.scala | 58 +++++ src/main/scala/sims/dynamics/ContactPoint.scala | 64 ++++++ src/main/scala/sims/dynamics/DistanceJoint.scala | 54 +++++ src/main/scala/sims/dynamics/Joint.scala | 21 ++ src/main/scala/sims/dynamics/Rectangle.scala | 38 ++++ src/main/scala/sims/dynamics/RegularPolygon.scala | 39 ++++ src/main/scala/sims/dynamics/RevoluteJoint.scala | 55 +++++ src/main/scala/sims/dynamics/Shape.scala | 67 ++++++ src/main/scala/sims/dynamics/World.scala | 101 +++++++++ .../sims/dynamics/constraints/Constraining.scala | 28 +++ .../sims/dynamics/constraints/Constraint.scala | 126 +++++++++++ .../scala/sims/dynamics/constraints/Jacobian.scala | 26 +++ src/main/scala/sims/math/PseudoVector3D.scala | 16 ++ src/main/scala/sims/math/math.scala | 23 ++ src/main/scala/sims/math/vector.scala | 127 +++++++++++ src/test/scala/sims/test/ClippingTest.scala | 58 +++++ src/test/scala/sims/test/LinearOverlapTest.scala | 92 ++++++++ src/test/scala/sims/test/gjk/EPA.scala | 98 +++++++++ src/test/scala/sims/test/gjk/FeatureManifold.scala | 89 ++++++++ src/test/scala/sims/test/gjk/GJK.scala | 115 ++++++++++ src/test/scala/sims/test/gjk/GJK2.scala | 168 +++++++++++++++ src/test/scala/sims/test/gjk/GJKTest.scala | 115 ++++++++++ src/test/scala/sims/test/gjk/MinkowskiSum.scala | 11 + src/test/scala/sims/test/gui/DebugWorld.scala | 29 +++ src/test/scala/sims/test/gui/KeyManager.scala | 53 +++++ src/test/scala/sims/test/gui/Main.scala | 143 +++++++++++++ src/test/scala/sims/test/gui/MouseJoint.scala | 38 ++++ src/test/scala/sims/test/gui/RichJoint.scala | 51 +++++ src/test/scala/sims/test/gui/RichShape.scala | 49 +++++ src/test/scala/sims/test/gui/Scene.scala | 13 ++ src/test/scala/sims/test/gui/SceneManager.scala | 95 +++++++++ src/test/scala/sims/test/gui/events.scala | 49 +++++ src/test/scala/sims/test/gui/graphicals.scala | 13 ++ .../scala/sims/test/gui/scenes/BasicScene.scala | 18 ++ .../scala/sims/test/gui/scenes/CloudScene.scala | 47 ++++ .../sims/test/gui/scenes/CollisionScene.scala | 41 ++++ .../scala/sims/test/gui/scenes/EmptyScene.scala | 6 + .../scala/sims/test/gui/scenes/JointScene.scala | 107 ++++++++++ .../sims/test/gui/scenes/LongCollisionScene.scala | 54 +++++ .../scala/sims/test/gui/scenes/PyramidScene.scala | 40 ++++ .../sims/test/gui/scenes/ShiftedStackScene.scala | 30 +++ 77 files changed, 4288 insertions(+) create mode 100644 .gitignore create mode 100644 LICENSE create mode 100644 README create mode 100644 build.sbt create mode 100644 lib/processing/core.jar create mode 100644 src/main/scala/sims/collision/AABB.scala create mode 100644 src/main/scala/sims/collision/CachedCollidable.scala create mode 100644 src/main/scala/sims/collision/Circle.scala create mode 100644 src/main/scala/sims/collision/Collidable.scala create mode 100644 src/main/scala/sims/collision/Collision.scala create mode 100644 src/main/scala/sims/collision/ContactPoint.scala create mode 100644 src/main/scala/sims/collision/ConvexPolygon.scala create mode 100644 src/main/scala/sims/collision/Detector.scala create mode 100644 src/main/scala/sims/collision/Intersectable.scala create mode 100644 src/main/scala/sims/collision/Linear.scala create mode 100644 src/main/scala/sims/collision/Polygon.scala create mode 100644 src/main/scala/sims/collision/Projection.scala create mode 100644 src/main/scala/sims/collision/Ray.scala create mode 100644 src/main/scala/sims/collision/Segment.scala create mode 100644 src/main/scala/sims/collision/broadphase/BroadPhaseDetector.scala create mode 100644 src/main/scala/sims/collision/broadphase/SAP.scala create mode 100644 src/main/scala/sims/collision/narrowphase/NarrowPhaseDetector.scala create mode 100644 src/main/scala/sims/collision/narrowphase/SAT.scala create mode 100644 src/main/scala/sims/collision/narrowphase/gjk/CS.scala create mode 100644 src/main/scala/sims/collision/narrowphase/gjk/EPA.scala create mode 100644 src/main/scala/sims/collision/narrowphase/gjk/GJK.scala create mode 100644 src/main/scala/sims/collision/narrowphase/gjk/GJK2.scala create mode 100644 src/main/scala/sims/collision/narrowphase/gjk/Manifold.scala create mode 100644 src/main/scala/sims/collision/narrowphase/gjk/MinkowskiSum.scala create mode 100644 src/main/scala/sims/collision/narrowphase/gjk/Penetration.scala create mode 100644 src/main/scala/sims/collision/package.scala create mode 100644 src/main/scala/sims/dsl/BodyPoint.scala create mode 100644 src/main/scala/sims/dsl/RichBody.scala create mode 100644 src/main/scala/sims/dsl/dsl.scala create mode 100644 src/main/scala/sims/dynamics/Body.scala create mode 100644 src/main/scala/sims/dynamics/Circle.scala create mode 100644 src/main/scala/sims/dynamics/Collision.scala create mode 100644 src/main/scala/sims/dynamics/ContactPoint.scala create mode 100644 src/main/scala/sims/dynamics/DistanceJoint.scala create mode 100644 src/main/scala/sims/dynamics/Joint.scala create mode 100644 src/main/scala/sims/dynamics/Rectangle.scala create mode 100644 src/main/scala/sims/dynamics/RegularPolygon.scala create mode 100644 src/main/scala/sims/dynamics/RevoluteJoint.scala create mode 100644 src/main/scala/sims/dynamics/Shape.scala create mode 100644 src/main/scala/sims/dynamics/World.scala create mode 100644 src/main/scala/sims/dynamics/constraints/Constraining.scala create mode 100644 src/main/scala/sims/dynamics/constraints/Constraint.scala create mode 100644 src/main/scala/sims/dynamics/constraints/Jacobian.scala create mode 100644 src/main/scala/sims/math/PseudoVector3D.scala create mode 100644 src/main/scala/sims/math/math.scala create mode 100644 src/main/scala/sims/math/vector.scala create mode 100644 src/test/scala/sims/test/ClippingTest.scala create mode 100644 src/test/scala/sims/test/LinearOverlapTest.scala create mode 100644 src/test/scala/sims/test/gjk/EPA.scala create mode 100644 src/test/scala/sims/test/gjk/FeatureManifold.scala create mode 100644 src/test/scala/sims/test/gjk/GJK.scala create mode 100644 src/test/scala/sims/test/gjk/GJK2.scala create mode 100644 src/test/scala/sims/test/gjk/GJKTest.scala create mode 100644 src/test/scala/sims/test/gjk/MinkowskiSum.scala create mode 100644 src/test/scala/sims/test/gui/DebugWorld.scala create mode 100644 src/test/scala/sims/test/gui/KeyManager.scala create mode 100644 src/test/scala/sims/test/gui/Main.scala create mode 100644 src/test/scala/sims/test/gui/MouseJoint.scala create mode 100644 src/test/scala/sims/test/gui/RichJoint.scala create mode 100644 src/test/scala/sims/test/gui/RichShape.scala create mode 100644 src/test/scala/sims/test/gui/Scene.scala create mode 100644 src/test/scala/sims/test/gui/SceneManager.scala create mode 100644 src/test/scala/sims/test/gui/events.scala create mode 100644 src/test/scala/sims/test/gui/graphicals.scala create mode 100644 src/test/scala/sims/test/gui/scenes/BasicScene.scala create mode 100644 src/test/scala/sims/test/gui/scenes/CloudScene.scala create mode 100644 src/test/scala/sims/test/gui/scenes/CollisionScene.scala create mode 100644 src/test/scala/sims/test/gui/scenes/EmptyScene.scala create mode 100644 src/test/scala/sims/test/gui/scenes/JointScene.scala create mode 100644 src/test/scala/sims/test/gui/scenes/LongCollisionScene.scala create mode 100644 src/test/scala/sims/test/gui/scenes/PyramidScene.scala create mode 100644 src/test/scala/sims/test/gui/scenes/ShiftedStackScene.scala diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..7e07ecf --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +target/ +project/boot/ diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..4257d21 --- /dev/null +++ b/LICENSE @@ -0,0 +1,24 @@ +Copyright (c) 2010-2011 Jakob Odersky +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. +3. The name of the author may not be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR +IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README b/README new file mode 100644 index 0000000..e69de29 diff --git a/build.sbt b/build.sbt new file mode 100644 index 0000000..7c993ee --- /dev/null +++ b/build.sbt @@ -0,0 +1,7 @@ +name := "SiMS2" + +normalizedName := "sims2" + +version := "2.0" + +scalaVersion := "2.9.0-1" diff --git a/lib/processing/core.jar b/lib/processing/core.jar new file mode 100644 index 0000000..8d5f0c8 Binary files /dev/null and b/lib/processing/core.jar differ 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 box 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 axis. + * @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 point is contained in this polygon. + *

+ * 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 not 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 direction.*/ + lazy val rightNormal = direction.rightNormal + + /**Right normal unit vector to direction.*/ + lazy val rightNormal0 = rightNormal.unit + + /**Left normal vector to direction.*/ + lazy val leftNormal = direction.leftNormal + + /**Left normal unit vector to direction.*/ + 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. + *

+ * 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 p.*/ + 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 vertex1 to vertex2.*/ + 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 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 diff --git a/src/main/scala/sims/dsl/BodyPoint.scala b/src/main/scala/sims/dsl/BodyPoint.scala new file mode 100644 index 0000000..5572b48 --- /dev/null +++ b/src/main/scala/sims/dsl/BodyPoint.scala @@ -0,0 +1,20 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dsl + +import sims.dynamics._ +import sims.math._ + +class BodyPoint(val body: Body, val point: Vector2D) { + def this(body: Body) = this(body, body.position) + + def distance(bp: BodyPoint) = new DistanceJoint(body, point, bp.body, bp.point) + + def revolute(bp: BodyPoint) = new RevoluteJoint(body, bp.body, point) + +} \ No newline at end of file diff --git a/src/main/scala/sims/dsl/RichBody.scala b/src/main/scala/sims/dsl/RichBody.scala new file mode 100644 index 0000000..0c7b234 --- /dev/null +++ b/src/main/scala/sims/dsl/RichBody.scala @@ -0,0 +1,33 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dsl + +import sims.dynamics._ +import sims.math._ + +class RichBody(val body: Body) { + + def at(point: Vector2D) = new BodyPoint(body, point) + def atLocal(point: Vector2D) = new BodyPoint(body, body.position + point) + + def :@(point: Vector2D) = at(point) + def :@@(point: Vector2D) = atLocal(point) + def @:(point: Vector2D) = at(point) + def @@:(point: Vector2D) = atLocal(point) + + def @@(point: Vector2D) = at(point) +} + + +object Test { + val b = new Body(null) + + + val q = b :@@ (1.0, 2.0) revolute b + +} \ No newline at end of file diff --git a/src/main/scala/sims/dsl/dsl.scala b/src/main/scala/sims/dsl/dsl.scala new file mode 100644 index 0000000..de5fa8a --- /dev/null +++ b/src/main/scala/sims/dsl/dsl.scala @@ -0,0 +1,17 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims + +/** @todo implement DSL properly */ +package object dsl { + + implicit def body2Rich(b: sims.dynamics.Body) = new RichBody(b) + + implicit def body2BodyPoint(b: sims.dynamics.Body) = new BodyPoint(b) + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/Body.scala b/src/main/scala/sims/dynamics/Body.scala new file mode 100644 index 0000000..5262468 --- /dev/null +++ b/src/main/scala/sims/dynamics/Body.scala @@ -0,0 +1,113 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics + +import sims.math._ + +class Body(shapes0: Shape*) { + + val shapes: List[Shape] = shapes0.toList + + var force: Vector2D = Vector2D.Null + + var torque: Double = 0.0 + + var linearVelocity: Vector2D = Vector2D.Null + + var angularVelocity: Double = 0.0 + + private var _position: Vector2D = + (Vector2D.Null /: shapes)((v: Vector2D, s: Shape) => v + s.position * s.mass) / shapes.map(_.mass).sum + + def position = _position + + def position_=(pos: Vector2D) = { + val delta = pos - _position + _position = pos + for (s <- shapes) s.position += delta + } + + private var _rotation: Double = 0.0 + + def rotation = _rotation + + def rotation_=(r: Double) = { + val delta = _rotation - r + _rotation = r + for (s <- shapes) { + s.rotation += delta + s.position = position + (s.local.get rotate r) + } + } + + var fixed = false + + /**Returns the mass of this body. If the body is free, its mass is the sum of the masses of its shapes. + * If the body is fixed, its mass is infinite (`Double.PositiveInfinity`). + * @return this body's mass*/ + lazy val mass: Double = if (!fixed) shapes.map(_.mass).sum else Double.PositiveInfinity + + /**Returns the moment of inertia for rotations about the COM of this body. + * It is calculated using the moments of inertia of this body's shapes and the parallel axis theorem. + * If the body is fixed, its moment of inertia is infinite (`Double.PositiveInfinity`). + * @return moment of inertia for rotations about the center of mass of this body*/ + lazy val inertia: Double = if (!fixed) shapes.map((s: Shape) => s.inertia + s.mass * (s.local.get dot s.local.get)).sum else Double.PositiveInfinity + + /**Applies a force to the center of mass of this body. + * @param force applied force*/ + def applyForce(force: Vector2D) = if (!fixed) this.force += force + + /**Applies a force to a point on this body. The point is considered to be contained within this body. + * @param force applied force + * @param point position vector of the point (in world coordinates)*/ + def applyForce(force: Vector2D, point: Vector2D) = if (!fixed) {this.force += force; torque += (point - position) cross force} + + /**Applies a torque to the center of mass.*/ + def applyTorque(torque: Double) = if (!fixed) this.torque += torque + + /**Applies an impulse to the center of mass of this body. + * @param impulse applied impulse*/ + def applyImpulse(impulse: Vector2D) = if (!fixed) linearVelocity += impulse / mass + + /**Applies an impulse to a point on this body. The point is considered to be contained within this body. + * @param impulse applied impulse + * @param point position vector of the point (in world coordinates)*/ + def applyImpulse(impulse: Vector2D, point: Vector2D) = if (!fixed) {linearVelocity += impulse / mass; angularVelocity += ((point - position) cross impulse) / inertia} + + /**Applies an angular impulse to the center of mass.*/ + def applyAngularImpulse(impulse: Double) = if (!fixed) angularVelocity += impulse / inertia + + /**Linear velocity of the given point on this body (in world coordinates).*/ + def velocityOfPoint(point: Vector2D) = linearVelocity + (angularVelocity cross (point - position)) + + /**Linear momentum.*/ + def linearMomentum = linearVelocity * mass + + for (s0 <- shapes0) { + s0.local = Some(s0.position - _position) + s0.body = this + } + + def contains(point: Vector2D) = shapes.exists(_.contains(point)) + + def info = { + "Body@" + hashCode + "(" + this.getClass() + ")\n" + + "\tPosition: " + position + "\n" + + "\tRotation: " + rotation + "\n" + + "\tLinear velocity: " + linearVelocity + "\n" + + "\tAngular velocity: " + angularVelocity + "\n" + + "\tForce: " + force + "\n" + + "\tTorque: " + torque + "\n" + + "\tMass: " + mass + "\n" + + "\tInertia: " + inertia + "\n" + + "\tFixed: " + fixed + "\n" + + "\tShape count" + shapes.length + + } + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/Circle.scala b/src/main/scala/sims/dynamics/Circle.scala new file mode 100644 index 0000000..9e00815 --- /dev/null +++ b/src/main/scala/sims/dynamics/Circle.scala @@ -0,0 +1,24 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics + +import scala.math.Pi +import sims.collision.{AABB, Projection} +import sims.math._ + +/** A circle. + * @define shape circle + * + * @param radius radius of this circle */ +case class Circle(radius: Double) extends sims.collision.Circle with Shape { + + val area: Double = Pi * radius * radius + + lazy val inertia: Double = mass * radius * radius / 2 + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/Collision.scala b/src/main/scala/sims/dynamics/Collision.scala new file mode 100644 index 0000000..e47c346 --- /dev/null +++ b/src/main/scala/sims/dynamics/Collision.scala @@ -0,0 +1,58 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics + +import sims.dynamics._ +import sims.dynamics.constraints._ + +import sims.collision.{Collision => CCollision} + +/** A class representing a physical collision, + * implementing constraints to handle collision response. */ +class Collision(collision: CCollision[Shape]) extends Constraining { + + + + private def getNonPenetrationConstraints() = for (point <- collision.points) yield + new Constraint { + val body1 = collision.item1.body + val body2 = collision.item2.body + def v = body2.velocityOfPoint(point) - body1.velocityOfPoint(point) + val e = { + if ((v dot collision.normal.unit) > 0) 0.0 + else if ((v dot collision.normal.unit) > -1) 0.0 + else math.min(collision.item1.restitution, collision.item2.restitution) + } + def jacobian = new Jacobian(-collision.normal, -((point - body1.position) cross collision.normal), + collision.normal, ((point - body2.position) cross collision.normal)) + + override def bias = (v dot collision.normal.unit) * e + def value = -collision.overlap + override def inequality = true + override val limit = Some((0.0, Double.PositiveInfinity)) + + val slop = 0.005 + override def error = + if (collision.overlap > slop) + -(collision.overlap - slop) + else 0.0 + } + + val constraints = getNonPenetrationConstraints() + +} + +object Collision { + + /**Converts a collision to a physical collision + * (sims.collision.Collision to a sims.dynamics.Collision)*/ + implicit def collision2Physical(c: sims.collision.Collision[Shape]) = new Collision(c) + + implicit def collision2Constructor(c: sims.collision.Collision[Shape]) = new { def toPhysical = new Collision(c) } + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/ContactPoint.scala b/src/main/scala/sims/dynamics/ContactPoint.scala new file mode 100644 index 0000000..31eb1b0 --- /dev/null +++ b/src/main/scala/sims/dynamics/ContactPoint.scala @@ -0,0 +1,64 @@ +package sims.dynamics + +import sims.collision.{Collision => CCollision} +import sims.dynamics.constraints._ +import sims.math._ + +class RestingPoint(collision: CCollision[Shape], point: Vector2D) extends Constraint { + import collision._ + val body1 = item1.body + val body2 = item2.body + + def relativeVelocity = body2.velocityOfPoint(point) - body1.velocityOfPoint(point) + + override def inequality = true + + override val limit = Some((0.0, Double.PositiveInfinity)) + + override def value = -overlap + + override def jacobian = + new Jacobian(-normal.unit, -((point - body1.position) cross normal.unit), + normal.unit, ((point - body2.position) cross normal.unit)) + + val slop = 0.005 + override def error = + if (collision.overlap > slop) + -(collision.overlap - slop) + else 0.0 +} + +class ImpactPoint(collision: CCollision[Shape], point: Vector2D) extends Constraint { + import collision._ + val body1 = item1.body + val body2 = item2.body + + def relativeVelocity = body2.velocityOfPoint(point) - body1.velocityOfPoint(point) + + override def inequality = true + + override val limit = Some((0.0, Double.PositiveInfinity)) + + override def value = -overlap + + override def jacobian = + new Jacobian(-normal.unit, -((point - body1.position) cross normal.unit), + normal.unit, ((point - body2.position) cross normal.unit)) + + val restitution = math.min(collision.item1.restitution, collision.item2.restitution) + override def bias = (relativeVelocity dot collision.normal.unit) * restitution + + override def error = 0 +} + +object ContactResolver { + + def relativeVelocity(collision: CCollision[Shape], point: Vector2D) = + collision.item2.body.velocityOfPoint(point) - collision.item2.body.velocityOfPoint(point) + + def resolve(collision: CCollision[Shape]): Seq[Constraint] = for (p <- collision.points) yield { + val v = (relativeVelocity(collision, p) dot collision.normal.unit) + if (v < -1) new RestingPoint(collision, p) + else new ImpactPoint(collision, p) + } +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/DistanceJoint.scala b/src/main/scala/sims/dynamics/DistanceJoint.scala new file mode 100644 index 0000000..2e24e30 --- /dev/null +++ b/src/main/scala/sims/dynamics/DistanceJoint.scala @@ -0,0 +1,54 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics + +import sims.dynamics.constraints._ +import sims.math._ + +/** A distance joint, used to keep two bodies at a fixed distance. + * The distance is given by the initial distance of the anchor points. + * + * This joint removes one degree of freedom between two bodies, + * hence it contains one [[sims.dynamics.constraints.Constraint]]. + * + * @param body1 first body connected by this joint + * @param anchor1 anchor point on first body (in world coordinates) + * @param body2 second body connected by this joint + * @param anchor2 anchor point on second body (in world coordinates) */ +class DistanceJoint(val body1: Body, val anchor1: Vector2D, val body2: Body, val anchor2: Vector2D) extends Joint { + def this(body1: Body, body2: Body) = this(body1, Vector2D.Null, body2, Vector2D.Null) + + private val self = this + + val local1 = anchor1 - body1.position + val local2 = anchor2 - body2.position + private val l = (anchor2 - anchor1).length //(body2.position + local2 - body1.position - local1).length + private val rotation01 = body1.rotation + private val rotation02 = body2.rotation + + def r1 = (local1 rotate (body1.rotation - rotation01)) + def r2 = (local2 rotate (body2.rotation - rotation02)) + + def x1 = body1.position + r1 + def x2 = body2.position + r2 + + private def v1 = body1 velocityOfPoint x1 + private def v2 = body2 velocityOfPoint x2 + + private def x = x2 - x1 + private def v = v2 - v1 + + val constraints = List(new Constraint{ + val body1 = self.body1 + val body2 = self.body2 + def value = x.length - l + def jacobian = new Jacobian(-x.unit, -(r1 cross x.unit), x.unit, (r2 cross x.unit)) + }) + + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/Joint.scala b/src/main/scala/sims/dynamics/Joint.scala new file mode 100644 index 0000000..a3eb847 --- /dev/null +++ b/src/main/scala/sims/dynamics/Joint.scala @@ -0,0 +1,21 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics + +import sims.dynamics.constraints.Constraining + +/** A base trait for all joints between two bodies. */ +trait Joint extends Constraining { + + /** First body connected by this joint. */ + def body1: Body + + /** Second body connected by this joint. */ + def body2: Body + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/Rectangle.scala b/src/main/scala/sims/dynamics/Rectangle.scala new file mode 100644 index 0000000..90b8d9d --- /dev/null +++ b/src/main/scala/sims/dynamics/Rectangle.scala @@ -0,0 +1,38 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics + +import sims.math._ +import sims.collision.ConvexPolygon + +/**A rectangle is a polygon. + * @define shape rectangle + * + * @param halfWidth this rectangle's half width + * @param halfHeight this rectangle's half height + * @param density density of this rectangle */ +case class Rectangle(halfWidth: Double, halfHeight : Double) extends ConvexPolygon with Shape { + + val area = halfWidth * halfHeight * 4 + + val inertia = 1.0 / 12.0 * mass * ((2 * halfWidth) * (2 * halfWidth) + (2 * halfHeight) * (2 * halfHeight)) + + /**Returns the vectors from the center to the vertices of this rectangle. + * The first vertex is the upper-right vertex at a rotation of 0. + * Vertices are ordered counter-clockwise.*/ + def halfDiags: Array[Vector2D] = Array(Vector2D(halfWidth, halfHeight), + Vector2D(-halfWidth, halfHeight), + Vector2D(-halfWidth, -halfHeight), + Vector2D(halfWidth, -halfHeight)) map (_ rotate rotation) + + /**Returns the position vectors of this rectangle's vertices. + * The first vertex is the upper-right vertex at a rotation of 0. + * Vertices are ordered counter-clockwise.*/ + def vertices = for (h <- halfDiags) yield position + h + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/RegularPolygon.scala b/src/main/scala/sims/dynamics/RegularPolygon.scala new file mode 100644 index 0000000..7f62ea8 --- /dev/null +++ b/src/main/scala/sims/dynamics/RegularPolygon.scala @@ -0,0 +1,39 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics + +import scala.math._ +import sims.collision.ConvexPolygon +import sims.math._ + +/**A regular polygon with n sides whose excircle has a radius radius. + * @define shape regular polygon + * + * @param n number of sides. + * @param radius radius of the excircle + * @param density density of this regular polygon + * @throws IllegalArgumentException if n is smaller than 3 */ +case class RegularPolygon(n: Int, radius: Double) extends ConvexPolygon with Shape { + require(n >= 3, "A polygon must have at least 3 sides.") + + /**Height of one of the constituting triangles.*/ + private val h: Double = radius * cos(Pi / n) + /**Half width of one of the constituting triangles.*/ + private val b: Double = radius * sin(Pi / n) + + def halfDiags = (for (i <- (0 until n).toArray) yield (Vector2D(0, radius) rotate (2 * Pi * i / n))) map (_ rotate rotation) + + def vertices = for (h <- halfDiags) yield position + h + + val area = n * h * b / 2 + + /**Moment of inertia of one of the constituting triangles about the center of this polygon.*/ + private val Ic: Double = density * b * (3 * b + 16) * pow(h, 4) / 54 + + val inertia = n * Ic +} diff --git a/src/main/scala/sims/dynamics/RevoluteJoint.scala b/src/main/scala/sims/dynamics/RevoluteJoint.scala new file mode 100644 index 0000000..5d7a85f --- /dev/null +++ b/src/main/scala/sims/dynamics/RevoluteJoint.scala @@ -0,0 +1,55 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics + +import sims.dynamics.constraints._ +import sims.math._ + +/** A revolute joint, used to keep two bodies fixed relatively at one point. + * + * This joint removes two degrees of freedom between two bodies, + * hence it contains two [[sims.dynamics.constraints.Constraint]]s. + * + * @param body1 first body connected by this joint + * @param body2 second body connected by this joint + * @param anchor anchor point where bodies are fixed relatively */ +class RevoluteJoint(val body1: Body, val body2: Body, val anchor: Vector2D) extends Joint { + def this(body1: Body, body2: Body) = this(body1, body2, body1.position) + + private val self = this + + private val local1 = anchor - body1.position + private val local2 = anchor - body2.position + + private val rotation01 = body1.rotation + private val rotation02 = body2.rotation + + def r1 = (local1 rotate (body1.rotation - rotation01)) + def r2 = (local2 rotate (body2.rotation - rotation02)) + + def x1 = body1.position + r1 + def x2 = body2.position + r2 + + def x = x2 - x1 + + val constraints = List( + new Constraint { + val body1 = self.body1 + val body2 = self.body2 + def value = x.x + def jacobian = new Jacobian(-Vector2D(1, 0), r1.y, Vector2D(1, 0), -r2.y) + }, + new Constraint { + val body1 = self.body1 + val body2 = self.body2 + def value = x.y + def jacobian = new Jacobian(-Vector2D(0, 1), -r1.x, Vector2D(0, 1), r2.x) + } + ) + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/Shape.scala b/src/main/scala/sims/dynamics/Shape.scala new file mode 100644 index 0000000..d1aa1ae --- /dev/null +++ b/src/main/scala/sims/dynamics/Shape.scala @@ -0,0 +1,67 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics + +import sims.collision._ +import sims.math._ + + +/** + * @define shape shape */ +trait Shape extends AnyRef with Collidable with CachedCollidable { + + override def equals(that: Any) = super.equals(that) + override def hashCode = super.hashCode + + /** (temporary solution) Local position of $shape to body. + * @deprecated find solution avoiding NullPointers + * @todo find solution avoiding NullPointers */ + var local: Option[Vector2D] = None + + var body: sims.dynamics.Body = null + + override def fixed = body.fixed + + var restitution = 1.0 + + /**Position of this $shape's center of mass in world coordinates.*/ + private var _position: Vector2D = Vector2D.Null + def position = _position + def position_=(pos: Vector2D) = { + _position = pos + move() + } + + /**Rotation of this shape about its center of mass.*/ + var rotation: Double = 0.0 + + /** Mass per area. */ + def density: Double = 1.0 + + /** Area of this $shape. */ + def area: Double + + /** Mass of this $shape. The mass is given by volume times density. */ + def mass: Double = area * density + + /**Moment of inertia for a rotation about this $shape's center of mass.*/ + def inertia: Double + + /**Returns this $shape's axis aligned bounding box.*/ + def aabb: sims.collision.AABB + + /**Returns the projection of this $shape onto the line given by the directional vector axis. + * @param axis directional vector of the line + * @return projection of this shape*/ + def project(axis: Vector2D): Projection + + /**Checks if the point point is contained in this $shape.*/ + def contains(point: Vector2D): Boolean + + def asBody = new sims.dynamics.Body(this) +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/World.scala b/src/main/scala/sims/dynamics/World.scala new file mode 100644 index 0000000..e02f38a --- /dev/null +++ b/src/main/scala/sims/dynamics/World.scala @@ -0,0 +1,101 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics + +import scala.collection.mutable.ArrayBuffer +import sims.collision._ +import sims.collision.broadphase.SAP +import sims.collision.narrowphase.SAT +import sims.math.Vector2D + +class World { + + val detector = SAP[sims.dynamics.Shape] narrowedBy + new sims.collision.narrowphase.gjk.GJK2[sims.dynamics.Shape] + //SAT[sims.dynamics.Shape] + + var collisionDetection = true + + private val _bodies = new ArrayBuffer[Body] + def bodies: Seq[Body] = _bodies + + def +=(b: Body): Unit = { + _bodies += b + detector ++= b.shapes + } + def -=(b: Body): Unit = { + _bodies -= b + detector --= b.shapes + } + + def shapes: Seq[sims.dynamics.Shape] = for (b <- bodies; s <- b.shapes) yield s + + private val _joints = new ArrayBuffer[Joint] + def joints: Seq[Joint] = _joints + + def +=(j: Joint): Unit = _joints += j + def -=(j: Joint): Unit = _joints -= j + + def clear() = { + detector.clear() + for (b <- bodies) _bodies -= b + for (j <- joints) _joints -= j + } + + + var h = 1.0 / 60 + + var iterations = 10 + + var errorReduction = 0.2 + + var gravity = sims.math.Vector2D(0, -9.8) + + def preStep() = {} + + def step() = { + + preStep() + + for (b <- _bodies) { + b applyForce gravity * b.mass + b.linearVelocity += (b.force / b.mass) * h + b.angularVelocity += (b.torque / b.inertia) * h + } + + for (j <- joints) { + j.preSolve() + } + for (i <- 0 until iterations) + for (j <- joints) j.correctVelocity(h, errorReduction) + + if (collisionDetection) { + import Collision._ + val physicalCollisions: Seq[Collision] = detector.collisions.map(_.toPhysical) + for (c <- physicalCollisions) c.preSolve() + for (i <- 0 until iterations) + for (c <- physicalCollisions) c.correctVelocity(h, errorReduction) + + } + + + for (b <- _bodies) { + b.position += b.linearVelocity * h + b.rotation += b.angularVelocity * h + b.force = Vector2D.Null + } + + postStep() + } + + def postStep(): Unit = { + detector.invalidate() + } + + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/constraints/Constraining.scala b/src/main/scala/sims/dynamics/constraints/Constraining.scala new file mode 100644 index 0000000..f1834a2 --- /dev/null +++ b/src/main/scala/sims/dynamics/constraints/Constraining.scala @@ -0,0 +1,28 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics.constraints + +import sims.dynamics._ + +/**A base trait implemented by objects representing constraints (such as joints or collisions). + * @see sims.dynamics.constraints.Constraint + */ +trait Constraining { + + /**All constraints represented by this constraining object.*/ + def constraints: Seq[Constraint] + + /**Invoke `preSolve()` on each constraint. + * @see sims.dynamics.constraints.Constraint*/ + def preSolve() = for (c <- constraints) c.preSolve() + + /**Solves all constraints of this constraining object. + * @see sims.dynamics.constraints.Constraint*/ + def correctVelocity(h: Double, erp: Double): Unit = for (c <- constraints) c.correctVelocity(h, erp) + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/constraints/Constraint.scala b/src/main/scala/sims/dynamics/constraints/Constraint.scala new file mode 100644 index 0000000..367c84d --- /dev/null +++ b/src/main/scala/sims/dynamics/constraints/Constraint.scala @@ -0,0 +1,126 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics.constraints + +import sims.dynamics._ +import sims.math._ + +/** A base trait for implementing and solving constraints. + * Constraints are solved using the 'sequential impulse' method presented by Erin Catto + * at Game Developpers Conference 2008 (see http://www.box2d.org/documentation.html). + * + * One instance of this trait represents one constraint for one degree of freedom between + * two bodies. + * @see sims.dynamics.constraints.Constrained */ +trait Constraint { + + /** First constrained body. */ + def body1: Body + + /** Second constrained body. */ + def body2: Body + + /** Value of the position constraint function ('C' in the presentation). */ + def value: Double + + /** Jacobian for the velocity constraint ('J' in 'Ċ=Jv+b' in the presentation). */ + def jacobian: Jacobian + + /** Velocity bias ('b' in 'Ċ=Jv+b' in the presentation).*/ + def bias: Double = 0 + + /** Lower and upper limits of the total corrective impulse allowed + * when solving this constraint. + * The first element represents the lower limit, + * the second the upper limit. + * None represents no limits. */ + def limit: Option[(Double, Double)] = None + + /** Defines whether or not this constraint should be treated as an inequality constraint. + * An inequality constraint will only be solved if the constraint function is less than + * zero (i.e. `value < 0`). */ + def inequality = false + + /** Position error used for Baumgarte sabilization. + * Corresponds to `value` by default. */ + def error = value + + /** Accumulated impulse. */ + private var accumulated = 0.0 + + /** Constraint mass cache, constant for all iterations. */ + private var m = 0.0 + + /** Jacobian cache, constant for all iterations. */ + private var J: Jacobian = null + + /** Velocity bias cache, constant for all iterations. */ + private var b = 0.0 + + /** Method that should be called before solving this constraint. + * Computes constraint masses and other invariable data for iterating over constraints. */ + def preSolve() = { + //compute jacobian and bias for next solving + J = jacobian + b = bias + + //invMass=(J*invert(M)*transpose(J)) + val invMass: Double = ( + (J.v1.x * J.v1.x + J.v1.y * J.v1.y) / body1.mass + J.w1 * J.w1 / body1.inertia + + (J.v2.x * J.v2.x + J.v2.y * J.v2.y) / body2.mass + J.w2 * J.w2 / body2.inertia + ) + + //if invMass == 0, both bodies have infinite mass (i.e. are fixed) + m = if (invMass == 0.0) 0.0 else 1.0 / invMass + + //apply accumulated impulse from prevoius step + body1.applyImpulse(J.v1 * accumulated) + body1.applyAngularImpulse(J.w1 * accumulated) + body2.applyImpulse(J.v2 * accumulated) + body2.applyAngularImpulse(J.w2 * accumulated) + + //initialize accumulated impulse + //accumulated = 0.0 + } + + /** Solves this constraint by applying corrective impulses to its constrained bodies. + * @todo implement error correction properly for collisiions (add slop tolerance, etc) + * @param h time interval over which the correction should be applied + * @param erp error reduction parameter. */ + def correctVelocity(h: Double, erp: Double): Unit = { + + //if m == 0, both bodies are fixed and there is no point in applying corrective impulse + if (m == 0) return () + + //C > 0 => ignore + if (inequality && value > 0.0) return () + + //lambda + var lambda = -m * ( + J.v1.x * body1.linearVelocity.x + J.v1.y * body1.linearVelocity.y + J.w1 * body1.angularVelocity + + J.v2.x * body2.linearVelocity.x + J.v2.y * body2.linearVelocity.y + J.w2 * body2.angularVelocity + + b + (erp / h * error) + ) + + + //clamp accumulated impulse + if (limit != None) { + val temp = accumulated + accumulated = math.max(limit.get._1, math.min(accumulated + lambda, limit.get._2)) + lambda = accumulated - temp + } + + + //apply accumulated impulse + body1.applyImpulse(J.v1 * lambda) + body1.applyAngularImpulse(J.w1 * lambda) + body2.applyImpulse(J.v2 * lambda) + body2.applyAngularImpulse(J.w2 * lambda) + } + +} \ No newline at end of file diff --git a/src/main/scala/sims/dynamics/constraints/Jacobian.scala b/src/main/scala/sims/dynamics/constraints/Jacobian.scala new file mode 100644 index 0000000..3b16bde --- /dev/null +++ b/src/main/scala/sims/dynamics/constraints/Jacobian.scala @@ -0,0 +1,26 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.dynamics.constraints + +import sims.math.Vector2D + +/** A Jacobian matrix used for solving constraints. + * + * In SiMS 2 constraints are considered to remove one degree of freedom for two bodies. + * Let '''v''',,1,,, w,,1,,, '''v''',,2,,, w,,2,, be the linear and angular velocities of both bodies + * respectively. + * Let v = ['''v''',,1,,, w,,1,,, '''v''',,2,,, w,,2,,]. + * The velocity constraint function is then given by 'Ċ='''Jv'''+b' and + * the Jacobian ('''J''') is given by '''J''' = ['''Jv''',,1,,, Jw,,1,,, '''Jv''',,2,,, Jw,,2,,] = [Jv,,1,x,,, Jv,,1,y,,, Jw,,1,,, Jv,,2,x,,, Jv,,2,y,,, Jw,,2,,] + * + * @param v1 corresponds to '''Jv''',,1,, in the description above + * @param w1 corresponds to Jw,,1,, in the description above + * @param v2 corresponds to '''Jv''',,2,, in the description above + * @param w2 corresponds to Jw,,2,, in the description above + * @see sims.dynamics.constraints.Constraint */ +class Jacobian(val v1: Vector2D, val w1: Double, val v2: Vector2D, val w2: Double) \ No newline at end of file diff --git a/src/main/scala/sims/math/PseudoVector3D.scala b/src/main/scala/sims/math/PseudoVector3D.scala new file mode 100644 index 0000000..ea51c10 --- /dev/null +++ b/src/main/scala/sims/math/PseudoVector3D.scala @@ -0,0 +1,16 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.math + +/**An x3-axis aligned vector. Since SIMS is in 2D, 3D vectors are only used as a convenience to simulate operations that only exist + * in three dimensions such as the cross product.*/ +case class PseudoVector3D(x3: Double) { + + def cross(v: Vector2D): Vector2D = v.leftNormal * x3 + +} \ No newline at end of file diff --git a/src/main/scala/sims/math/math.scala b/src/main/scala/sims/math/math.scala new file mode 100644 index 0000000..36f7a8b --- /dev/null +++ b/src/main/scala/sims/math/math.scala @@ -0,0 +1,23 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims + +package object math { + + implicit def double2PseudoVector3D(x3: Double) = new PseudoVector3D(x3) + + implicit def tuple2Vector[A: Numeric, B: Numeric](t: (A, B)): Vector2D = { + val x = implicitly[Numeric[A]].toDouble(t._1) + val y = implicitly[Numeric[B]].toDouble(t._2) + new Vector2D(x, y) + } + + def mod(x: Int, y: Int): Int = {val r = x % y; if (r < 0) r + y else r} + + +} \ No newline at end of file diff --git a/src/main/scala/sims/math/vector.scala b/src/main/scala/sims/math/vector.scala new file mode 100644 index 0000000..8e7c716 --- /dev/null +++ b/src/main/scala/sims/math/vector.scala @@ -0,0 +1,127 @@ +/* _____ _ __ ________ ___ *\ +** / ___/(_) |/ / ___/ |__ \ Simple Mechanics Simulator 2 ** +** \__ \/ / /|_/ /\__ \ __/ / copyright (c) 2011 Jakob Odersky ** +** ___/ / / / / /___/ / / __/ ** +** /____/_/_/ /_//____/ /____/ ** +\* */ + +package sims.math + +import scala.math._ + +/** A 2D vector. + * @param x 1st component + * @param y 2nd component */ +case class Vector2D(x: Double, y: Double) { + + /** Components of this vector. */ + lazy val components = List(x, y) + + /** Vector addition. */ + def +(v: Vector2D): Vector2D = new Vector2D(x + v.x, y + v.y) + + /** Scalar multiplication. */ + def *(n: Double): Vector2D = new Vector2D(x * n, y * n) + + /** Inverse of this vector. */ + lazy val unary_- = this * (-1) + + /** Add inverse of another vector. */ + def -(v: Vector2D) = this + -v + + /** Multiply by inverse of scalar. */ + def /(n: Double) = this * (1 / n) + + /** Dot product. */ + def dot(v: Vector2D): Double = x * v.x + y * v.y + + /** Cross product. Length only because in 2D. The direction would be given by the x3-axis. */ + def cross(v: Vector2D): Double = x * v.y - y * v.x + + /** Cross product with an imaginary vector parallel to the x3-axis. + * Its magnitude is given by |p| and its direction sign(p). */ + def cross(p: PseudoVector3D): Vector2D = rightNormal * p.x3 + + /** Magnitude of this vector. */ + lazy val length: Double = math.sqrt(lengthSquare) + + lazy val lengthSquare: Double = x * x + y * y + + /** Unit vector. */ + lazy val unit: UnitVector2D = + if (!(x == 0.0 && y == 0.0)) new UnitVector2D(x / length, y / length) + else throw new UnsupportedOperationException("Null vector does not have a unit vector.") + + /** Returns the projection of this vector onto the vector `v`. */ + def project(v: Vector2D): Vector2D = { + if (v != Vector2D.Null) + v * ((this dot v) / (v.lengthSquare)) + else + Vector2D.Null + } + + /** Returns the projection of this vector onto the unit vector `v`. */ + def project(v: UnitVector2D): Vector2D = { + if (v != Vector2D.Null) + v * (this dot v) + else + Vector2D.Null + } + + /** Returns this vector rotated by `angle` radians. */ + def rotate(angle: Double): Vector2D = { + Vector2D(cos(angle) * x - sin(angle) * y, + cos(angle) * y + sin(angle) * x) + } + + /** Left normal vector. (-y, x) */ + lazy val leftNormal: Vector2D = Vector2D(-y, x) + + /** Right normal vector. (y, -x) */ + lazy val rightNormal: Vector2D = Vector2D(y, -x) + + /** Checks if this vector is the null vector. */ + def isNull: Boolean = this == Vector2D.Null + + /** Colinearity check. */ + def ~(v: Vector2D): Boolean = x * v.y - v.x * y == 0 + + + def leftOf(v: Vector2D): Boolean = (this dot v.leftNormal) > 0 + def rightOf(v: Vector2D): Boolean = (this dot v.rightNormal) > 0 + def directionOf(v: Vector2D): Boolean = (this dot v) > 0 + +} + +object Vector2D { + + /**Null vector.*/ + val Null = Vector2D(0,0) + + /**Horizontal unit vector. (1,0)*/ + val i = new UnitVector2D(1,0) + + /**Vertical unit vector. (0,1)*/ + val j = new UnitVector2D(0,1) + +} + +/** A two dimensional vector considered having a magnitude of one. + * Unit vectors are used internally for increasing performance + * (i.e. the length of a unit vector is always one, the unit vector of + * a unit vector is always itself). */ +class UnitVector2D(x: Double, y: Double) extends Vector2D(x, y) { + + def isValid = x * x + y * y == 1 + + override lazy val unit = this + + override lazy val length = 1.0 + + override lazy val lengthSquare = 1.0 + + override lazy val leftNormal = new UnitVector2D(-y, x) + + override lazy val rightNormal = new UnitVector2D(y, -x) + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/ClippingTest.scala b/src/test/scala/sims/test/ClippingTest.scala new file mode 100644 index 0000000..317abfe --- /dev/null +++ b/src/test/scala/sims/test/ClippingTest.scala @@ -0,0 +1,58 @@ +package sims.test + +import org.scalatest.FunSuite +import org.scalatest.matchers.ShouldMatchers + +import sims.math._ +import sims.collision._ + +class ClippingTest extends FunSuite with ShouldMatchers { + + /* + test("clipping segment to segment, outside clipping zone") { + val A = new Segment((1, 1), (3, 0)) + val B = new Segment((0,0), (0, 1)) + val C = new Segment((4,-1), (10, 5)) + + (B clipped A) should equal (None) + (C clipped A) should equal (None) + } + + test("clipping segment to segment, inside clipping zone") { + val segments = List( + new Segment((2, 3), (4.5, 8)), + new Segment((3, -4), (2, -5)), + new Segment((0, 0), (10, 0)), + new Segment((1.1, -1234), (9.999, 0.001)), + new Segment((5, 5), (5, -5)) + ) + + val clippers = List( + new Segment((0, 0), (10, 0)), + new Segment((10, 0), (0, 0)), + new Segment((120, -0.01), (-130, -0.01)), + new Segment((-2.3, 10000), (14, 10000)) + ) + + def translate(s: Segment, v: Vector2D) = { + new Segment(s.point1 + v, s.point2 + v) + } + + def rotate(s: Segment, r: Double) = { + new Segment(s.point1 rotate r, s.point2 rotate r) + } + + for (s <- segments; c <- clippers) {(s clipped c).get should equal (s)} + + for (i <- 0 until 1000) for(s <- segments; c <- clippers) { + val r = new scala.util.Random + def next = r.nextDouble * 1000 * (if (r.nextBoolean) -1 else 1) + val x = (next, next) + val y = next + (translate(rotate(s, y), x) clipped translate(rotate(c, y), x)).get should equal (translate(rotate(s, y), x)) + } + + } + */ + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/LinearOverlapTest.scala b/src/test/scala/sims/test/LinearOverlapTest.scala new file mode 100644 index 0000000..aa99f6f --- /dev/null +++ b/src/test/scala/sims/test/LinearOverlapTest.scala @@ -0,0 +1,92 @@ +package sims.test + +import org.scalatest.FlatSpec +import org.scalatest.matchers.ShouldMatchers + +import sims.math._ +import sims.collision._ + +class LinearOverlapTest extends FlatSpec with ShouldMatchers { + + "A segment" should "throw IllegalArgumentException if both of its " + + "vertices degenerate into a single one" in { + evaluating { val s1 = Segment(Vector2D(0,0), Vector2D(0,0)) } should produce [IllegalArgumentException] + } + + it should "not intersect with itself" in { + val s1 = Segment(Vector2D(2, 2), Vector2D(3, 5)) + s1 intersection s1 should equal (None) + } + + "Two segments" should "have an intersection point if they intersect" in { + val s1 = Segment(Vector2D(0, 0), Vector2D(3, 1)) + val s2 = Segment(Vector2D(0, 1), Vector2D(3, -2)) + s1 intersection s2 should not equal (None) + } + + it should "have an intersection point if they share a vertice" in { + val s1 = Segment(Vector2D(1, 2), Vector2D(3, 1)) + val s2 = Segment(Vector2D(3, 1), Vector2D(3, -2)) + s1 intersection s2 should not equal (None) + } + + it should "have an intersection point if one contains one of the other's vertices" in { + val s1 = Segment(Vector2D(2, 4), Vector2D(3, 100)) + val s2 = Segment(Vector2D(1, 3), Vector2D(3, 5)) + s1 intersection s2 should not equal (None) + } + + it should "not have an intersection point if they are parallel" in { + val s1 = Segment(Vector2D(0, 0), Vector2D(3, 1)) + val s2 = Segment(Vector2D(0, 1), Vector2D(3, 2)) + s1 intersection s2 should equal (None) + } + + it should "not have an intersection point if they are parallel and lie on each other" in { + val s1 = Segment(Vector2D(2, 2), Vector2D(6, 6)) + val s2 = Segment(Vector2D(3, 3), Vector2D(4, 4)) + s1 intersection s2 should equal (None) + } + + + "A ray and a segment" should "have an intersection point if they intersect" in { + val r1 = Ray(Vector2D(3, 5), Vector2D(3, -1)) + val s1 = Segment(Vector2D(6.32, math.sqrt(4.0)), Vector2D(10, 15.5)) + r1 intersection s1 should not equal (None) + } + + it should "have an intersection point if they share a vertice" in { + val r1 = Ray(Vector2D(3, 4), Vector2D(2, 1)) + val s1 = Segment(Vector2D(0, 10), Vector2D(3, 4)) + r1 intersection s1 should not equal (None) + } + + it should "have an intersection point if the ray contains one of the segment's vertices" in { + val r1 = Ray(Vector2D(0, 0), Vector2D(1, 2)) + val s1 = Segment(Vector2D(2, 4), Vector2D(5, 4)) + r1 intersection s1 should not equal (None) + } + + it should "have an intersection point if the segment contains the ray's vertice" in { + val r1 = Ray(Vector2D(0, math.Pi), Vector2D(1, 2)) + val s1 = Segment(Vector2D(0, 0), Vector2D(0, 4)) + r1 intersection s1 should not equal (None) + + val r2 = Ray(Vector2D(2, 3), Vector2D(-2, -1)) + val s2 = Segment(Vector2D(0, 4), Vector2D(4, 2)) + r2 intersection s2 should not equal (None) + } + + it should "not have an intersection point if they are parallel" in { + val r1 = Ray(Vector2D(2, 3), Vector2D(3, 4)) + val s1 = Segment(Vector2D(1, 4), Vector2D(4, 8)) + r1 intersection s1 should equal (None) + } + + it should "not have an intersection point if they lie on each other" in { + val r1 = Ray(Vector2D(0, 1), Vector2D(2, 3)) + val s1 = Segment(Vector2D(-1, 0), Vector2D(4, 4)) + r1 intersection s1 should equal (None) + } + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gjk/EPA.scala b/src/test/scala/sims/test/gjk/EPA.scala new file mode 100644 index 0000000..e64a73e --- /dev/null +++ b/src/test/scala/sims/test/gjk/EPA.scala @@ -0,0 +1,98 @@ +package sims.test.gjk + +import scala.collection.mutable.ListBuffer +import sims.math._ + +/** The implementation was adapted from dyn4j by William Bittle (see http://www.dyn4j.org). */ +object EPA { + + val MaxIterations = 100 + + val DistanceEpsilon = 0.0001 + + protected 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 getPenetration(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 + return new Penetration(edge.normal, point dot edge.normal) + } + + 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 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 + return edge; + } + +} + +class Penetration(val normal: Vector2D, val overlap: Double) \ No newline at end of file diff --git a/src/test/scala/sims/test/gjk/FeatureManifold.scala b/src/test/scala/sims/test/gjk/FeatureManifold.scala new file mode 100644 index 0000000..527d8ba --- /dev/null +++ b/src/test/scala/sims/test/gjk/FeatureManifold.scala @@ -0,0 +1,89 @@ +package sims.test.gjk + +import sims.collision._ +import sims.math._ + +object FeatureManifold { + + 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.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): Option[Manifold] = { + var points = new scala.collection.mutable.ArrayBuffer[Vector2D] + + val feature1 = farthestFeature(pair._1, normal) + + //is feature 1 a vertex? + if (feature1.isLeft) { + return Some(Manifold(Array(feature1.left.get), normal)) + } + + val feature2 = farthestFeature(pair._2, -normal) + + //is feature 2 a vertex? + if (feature2.isLeft) { + return Some(Manifold(Array(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 + + + //both features are sides, clip feature2 to feature1 + val clipped = incident clipped reference + + /*val n = if (!flipped) normal else -normal + clipped match { + case None => None + case Some(Segment(point1, point2)) => Some( + Manifold(Array(point1, point2) filter ((v: Vector2D) => ((v - reference.point1) dot n) <= 0), n) + ) + }*/ + error("") + } + +} + +case class Manifold(pts: Array[Vector2D], normal: Vector2D) \ No newline at end of file diff --git a/src/test/scala/sims/test/gjk/GJK.scala b/src/test/scala/sims/test/gjk/GJK.scala new file mode 100644 index 0000000..a8b1732 --- /dev/null +++ b/src/test/scala/sims/test/gjk/GJK.scala @@ -0,0 +1,115 @@ +package sims.test.gjk + +import scala.collection.mutable.ListBuffer +import sims.collision._ +import sims.collision.narrowphase.NarrowPhaseDetector +import sims.math._ + +/** A narrowphase detector using the Gilbert-Johnson-Keerthi algorithm. */ +class GJK[A <: Collidable: ClassManifest] extends NarrowPhaseDetector[A] { + + /** Checks whether the given simplex contains the origin. If it does, `None` is returned. + * Otherwise a new search direction is returned and the simplex is updated. */ + protected def checkSimplex(simplex: ListBuffer[Vector2D], direction: Vector2D): Option[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) { + Some(ab cross ao cross ab) + } else { + simplex.remove(1) + Some(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) + Some(ab cross ao cross ab) + } else if (ao directionOf ac) { + simplex.remove(1) + Some(ac cross ao cross ac) + } else { + simplex.remove(2) + simplex.remove(1) + Some(ao) + } + } else { + if (ao directionOf (winding cross ac)) { + if (ao directionOf ac) { + simplex.remove(1) + Some(ac cross ao cross ac) + } else { + simplex.remove(2) + simplex.remove(1) + Some(ao) + } + } else { + None + } + } + } //end simplex == 3 + + else throw new IllegalArgumentException("Invalid simplex size.") + + } + + + def getPenetration(pair: (A, A)): Option[Penetration] = { + implicit val pr = pair + val ms = new MinkowskiSum(pair) + import ms._ + val s = support(Vector2D.i) + val simplex = new ListBuffer[Vector2D] + simplex prepend s + var direction = -s + + var counter = 0 + while (counter < 100) { + + val a = support(direction) + + if ((a dot direction) < 0) return None + + simplex prepend a + + val newDirection = checkSimplex(simplex, direction) + + if (newDirection.isEmpty) return Some(EPA.getPenetration(simplex, ms)) + else direction = newDirection.get + + counter += 1 + } + throw new IllegalArgumentException("Something went wrong, should not reach here.") + } + + override def collision(pair: (A, A)): Option[Collision[A]] = { + val p = getPenetration(pair) + if (p == None) return None + val man = FeatureManifold.getCollisionPoints(pair, p.get.normal) + if (man == None) return None + + Some(new Collision[A] { + val item1 = pair._1 + val item2 = pair._2 + val normal = man.get.normal + val overlap = p.get.overlap + val points = man.get.pts.toList + }) + } + +} + + diff --git a/src/test/scala/sims/test/gjk/GJK2.scala b/src/test/scala/sims/test/gjk/GJK2.scala new file mode 100644 index 0000000..7fdaec3 --- /dev/null +++ b/src/test/scala/sims/test/gjk/GJK2.scala @@ -0,0 +1,168 @@ +package sims.test.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.1 + + 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/test/scala/sims/test/gjk/GJKTest.scala b/src/test/scala/sims/test/gjk/GJKTest.scala new file mode 100644 index 0000000..d2ba22f --- /dev/null +++ b/src/test/scala/sims/test/gjk/GJKTest.scala @@ -0,0 +1,115 @@ +package sims.test.gjk + +import processing.core.PApplet +import processing.core.PConstants._ + +class GJKTest extends PApplet { + implicit val top = this + + import sims.dynamics._ + import sims.math._ + import sims.test.gui._ + import sims.test.gui.RichShape._ + + var s1: GraphicalShape = _ + var s2: GraphicalShape = _ + + override def setup() = { + size(600, 600, P2D) + background(255,255,255) + frameRate(60) + + + s1 = (new Rectangle(1, 3) {position = Vector2D(5,5)}).toGraphical + s2 = (new Rectangle(1, 2) {position = Vector2D(8,8); rotation = 0.2}).toGraphical + + } + + val PPM = 39.37f * 96 + var viewScale: Float = 1.0f / 80 + + val GJK = new GJK2[Shape] + + var invert = false + def pair = if (!invert) (s1, s2) else (s2, s1) + + override def draw() = { + smooth() + background(255,255,255) + translate(0, height) + scale(viewScale * PPM, -viewScale * PPM) + + if (keyCode == 32) invert = true + else invert = false + + val collision = GJK.collision(pair._1.shape, pair._2.shape) + /*if (collision != None) { + pushMatrix() + rectMode(CORNER) + stroke(255, 0, 50) + strokeWeight(10) + fill(0, 0, 0, 0) + rect(0, 0, 600, 600) + strokeWeight(1) + popMatrix() + }*/ + //val separation = GJK.collision(pair._1.shape, pair._2.shape) + //if (!separation.isEmpty) + //List(separation.get.point1, separation.get.point2) foreach (p => ellipse(p.x.toFloat, p.y.toFloat, 0.1f, 0.1f)) + + + + label() + s2.shape.position = Vector2D(mouseX / viewScale / PPM, -(mouseY - height) / viewScale / PPM) + + s1.render() + s2.render() + + + + collision match { + case Some(c) => { + stroke(0, 255, 0) + for (p <- c.points) { + ellipse(p.x.toFloat, p.y.toFloat, 0.1f, 0.1f) + val s = p + val e = p + c.normal + line(s.x.toFloat, s.y.toFloat, e.x.toFloat, e.y.toFloat) + println(c.overlap) + } + } + case _ => () + } + + /*stroke(255, 0, 255) + val f = FeatureManifold.farthestFeature(pair._1.shape, Vector2D.j + Vector2D.i) + f match { + case Left(p) => ellipse(p.x.toFloat, p.y.toFloat, 0.1f, 0.1f) + case Right(s) => line(s.point1.x.toFloat, s.point1.y.toFloat, s.point2.x.toFloat, s.point2.y.toFloat) + }*/ + + } + + private val fontSize = 16 + private val f = createFont("Monospaced.plain", fontSize) + private def label() = { + val size = 16 + fill(0, 0, 0) + textMode(SCREEN) + textFont(f) + + val p1 = pair._1.shape + val p2 = pair._2.shape + text("1", (p1.position.x * PPM * viewScale).toFloat, (height - p1.position.y * PPM * viewScale).toFloat) + text("2", (p2.position.x * PPM * viewScale).toFloat, (height - p2.position.y * PPM * viewScale).toFloat) + } + +} + +object GJKTest { + + def main(args: Array[String]): Unit = { + PApplet.main(args ++ Array("sims.test.gjk.GJKTest")) + } + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gjk/MinkowskiSum.scala b/src/test/scala/sims/test/gjk/MinkowskiSum.scala new file mode 100644 index 0000000..7c574ce --- /dev/null +++ b/src/test/scala/sims/test/gjk/MinkowskiSum.scala @@ -0,0 +1,11 @@ +package sims.test.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/test/scala/sims/test/gui/DebugWorld.scala b/src/test/scala/sims/test/gui/DebugWorld.scala new file mode 100644 index 0000000..937bd77 --- /dev/null +++ b/src/test/scala/sims/test/gui/DebugWorld.scala @@ -0,0 +1,29 @@ +package sims.test.gui + +class DebugWorld extends sims.dynamics.World with Publisher { + + override def +=(b: sims.dynamics.Body) = { + super.+=(b) + publish(BodyAdded(this, b)) + } + + override def -=(b: sims.dynamics.Body) = { + super.-=(b) + publish(BodyRemoved(this, b)) + } + + override def +=(j: sims.dynamics.Joint) = { + super.+=(j) + publish(JointAdded(this, j)) + } + + override def -=(j: sims.dynamics.Joint) = { + super.-=(j) + publish(JointRemoved(this, j)) + } + + override def step() = { + super.step() + publish(Stepped(this)) + } +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/KeyManager.scala b/src/test/scala/sims/test/gui/KeyManager.scala new file mode 100644 index 0000000..f7cc199 --- /dev/null +++ b/src/test/scala/sims/test/gui/KeyManager.scala @@ -0,0 +1,53 @@ +package sims.test.gui + +import processing.core.PApplet + +class KeyManager(implicit top: Main) { + + def keyPressed(keyCode: Int) = keyCode match { + // ENTER + case 10 => top.SceneManager.currentScene.world.step() + + // SPACE + case 32 => top.paused = !top.paused + + // PAGE UP + case 33 => top.viewScale += top.viewScale * 0.02f + + // PAGE DOWN + case 34 => top.viewScale -= top.viewScale * 0.02f + + // 0 + case 36 => {top.offsetX = 0; top.offsetY = 0} + + // LEFT + case 37 => top.offsetX += 50 + + // UP + case 38 => top.offsetY -= 50 + + // RIGHT + case 39 => top.offsetX -= 50 + + // DOWN + case 40 => top.offsetY += 50 + + // , (<) + case 44 => top.SceneManager.previousScene() + + // . (>) + case 46 => top.SceneManager.nextScene() + + // b + case 66 => top.SceneManager.currentScene.world.errorReduction += 0.1 + + //v + case 86 => top.SceneManager.currentScene.world.errorReduction -= 0.1 + + case 45 => top.SceneManager.currentScene.world.iterations -= 1 + case 61 => top.SceneManager.currentScene.world.iterations += 1 + + case x: Any => println("unknown key: " + x) + } + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/Main.scala b/src/test/scala/sims/test/gui/Main.scala new file mode 100644 index 0000000..745d793 --- /dev/null +++ b/src/test/scala/sims/test/gui/Main.scala @@ -0,0 +1,143 @@ +package sims.test.gui + +import processing.core.PApplet +import processing.core.PConstants._ +import scala.collection.mutable.ArrayBuffer + +import sims.math._ +import sims.test.gui.RichShape._ +import sims.test.gui.scenes._ +import sims.dynamics.Shape + +class Main extends PApplet { + implicit val top = this + + val SceneManager = new SceneManager + import SceneManager._ + + val KeyManager = new KeyManager + import KeyManager._ + + var (offsetX, offsetY) = (200.0f, 100.0f) + val PPM = 39.37f * 96 + var viewScale: Float = 1.0f / 80 + + private val fontSize = 16 + private val f = createFont("Monospaced.plain", fontSize) + private def displayText(lines: String*) = { + val size = 16 + val indent = 10 + + fill(0, 0, 0) + textMode(SCREEN) + textFont(f) + + for (i <- 0 until lines.length) text(lines(i), indent, (i + 1) * size) + } + + + override def setup() = { + size(screenWidth * 2 / 3, screenHeight * 2 / 3, P2D) + background(255,255,255) + frameRate(60) + //frame.setResizable(true) + currentScene = scenes(0) + } + + var paused = true + override def draw() = { + smooth() + background(255,255,255) + + translate(offsetX, height - offsetY) + scale(viewScale * PPM, -viewScale * PPM) + + val t0 = System.nanoTime() + if (!paused) currentScene.world.step() + val collisions = if (currentScene.world.collisionDetection) SceneManager.currentScene.world.detector.collisions() else Seq() + val dStep = System.nanoTime() - t0 + + for (g <- graphicals) g.render() + fill(255, 0, 0) + stroke(20, 0, 0) + for (c <- collisions; p <- c.points) { + ellipse(p.x.toFloat, p.y.toFloat, 0.1f, 0.1f) + stroke(0, 255, 0) + val s = p + val e = p + c.normal + line(s.x.toFloat, s.y.toFloat, e.x.toFloat, e.y.toFloat) + } + + //_.points.foreach((v) => ellipse(v.x.toFloat, v.y.toFloat, 0.1f, 0.1f))) + + val dRender = System.nanoTime() - t0 - dStep + + displayText( + "status : " + (if (paused) "paused" else "running"), + "------------", + "fps [Hz]: " + frameRate, + "------------", + "step [ms]: " + (dStep / 1E6f), + " [%] : " + (dStep.toFloat / (dStep + dRender) * 100), + "render [ms]: " + (dRender / 1E6f), + " [%] : " + (dRender.toFloat / (dStep + dRender) * 100), + "------------", + "memory [MB]: " + java.lang.Runtime.getRuntime.totalMemory / 1E6, + "load : " + java.lang.management.ManagementFactory.getOperatingSystemMXBean.getSystemLoadAverage(), + "------------", + "bodies : " + currentScene.world.bodies.length, + "shapes : " + currentScene.world.shapes.length, + "joints : " + currentScene.world.joints.length, + "constraints: " + currentScene.world.joints.map(_.constraints.length).sum, + "collisions : " + collisions.length, + "it [1] : " + currentScene.world.iterations, + "dt [ms]: " + currentScene.world.h.toFloat, + "erp [ms]: " + currentScene.world.errorReduction.toFloat, + "------------", + "(" + scaledMouseX + ", " + scaledMouseY + ")" + ) + } + + def drawGrid() = { + + } + + override def keyPressed() = KeyManager.keyPressed(keyCode) + + def scaledMouseX = (mouseX - offsetX) / viewScale / PPM + def scaledMouseY = (height - mouseY - offsetY) / viewScale / PPM + + var mouseJoint: Option[MouseJoint] = None + override def mousePressed(): Unit = { + import Vector2D._ + val body = currentScene.world.bodies.find(_.contains((scaledMouseX, scaledMouseY))) + if (body.isEmpty) return () + val mj = new MouseJoint(body.get, (scaledMouseX, scaledMouseY)) + currentScene.world += mj + mouseJoint = Some(mj) + } + + override def mouseReleased(): Unit = { + if (mouseJoint.isEmpty) return () + currentScene.world -= mouseJoint.get + mouseJoint = None + } + + override def mouseDragged(): Unit = { + import Vector2D._ + + if (mouseJoint.isEmpty) return () + + mouseJoint.get.anchor = (scaledMouseX, scaledMouseY) + + } + + + +} + +object Main { + def main(args : Array[String]) : Unit = { + PApplet.main(args ++ Array("sims.test.gui.Main")) + } +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/MouseJoint.scala b/src/test/scala/sims/test/gui/MouseJoint.scala new file mode 100644 index 0000000..0d74dd5 --- /dev/null +++ b/src/test/scala/sims/test/gui/MouseJoint.scala @@ -0,0 +1,38 @@ +package sims.test.gui + +import sims.dynamics._ +import sims.dynamics.constraints._ +import sims.math._ + +class MouseJoint(val body1: Body, var anchor: Vector2D) extends Joint { + val body2 = new Body() {fixed = true} + + private val self = this + + private val local1 = anchor - body1.position + + private val rotation01 = body1.rotation + + def r1 = (local1 rotate (body1.rotation - rotation01)) + + def x1 = body1.position + r1 + def x2 = anchor + + def x = x2 - x1 + + val constraints = List( + new Constraint { + val body1 = self.body1 + val body2 = self.body2 + def value = x.x + def jacobian = new Jacobian(-Vector2D(1, 0), r1.y, Vector2D.i, 0) + }, + new Constraint { + val body1 = self.body1 + val body2 = self.body2 + def value = x.y + def jacobian = new Jacobian(-Vector2D(0, 1), -r1.x, Vector2D.j, 0) + } + ) + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/RichJoint.scala b/src/test/scala/sims/test/gui/RichJoint.scala new file mode 100644 index 0000000..2c3d5fd --- /dev/null +++ b/src/test/scala/sims/test/gui/RichJoint.scala @@ -0,0 +1,51 @@ +package sims.test.gui + +import processing.core.PApplet +import processing.core.PConstants._ +import sims.dynamics._ + +class RichJoint(joint: Joint) { +private implicit def double2Float(x: Double): Float = x.toFloat + + def toGraphical(implicit parent: PApplet) = new GraphicalJoint(joint) { + + val top = parent + + val render = joint match { + + case j: DistanceJoint => () => { + top.pushMatrix() + top.stroke(0, 0, 0) + top.fill(0, 0, 0) + top.line(j.x1.x, j.x1.y, j.x2.x, j.x2.y) + top.stroke(100, 100, 100) + top.line(j.body1.position.x, j.body1.position.y, j.x1.x, j.x1.y) + top.line(j.body2.position.x, j.body2.position.y, j.x2.x, j.x2.y) + top.popMatrix() + } + + case j: RevoluteJoint => () => { + top.pushMatrix() + top.stroke(0, 0, 0) + top.fill(0, 0, 0) + top.line(j.x1.x, j.x1.y, j.x2.x, j.x2.y) + top.stroke(100, 100, 100) + top.line(j.body1.position.x, j.body1.position.y, j.x1.x, j.x1.y) + top.line(j.body2.position.x, j.body2.position.y, j.x2.x, j.x2.y) + top.popMatrix() + } + + case j: MouseJoint => () => () + + case _ => throw new IllegalArgumentException("Cannot create graphical joint: unknown joint.") + } + + } + +} + +object RichJoint { + + implicit def jointToRichShape(j: Joint) = new RichJoint(j) + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/RichShape.scala b/src/test/scala/sims/test/gui/RichShape.scala new file mode 100644 index 0000000..6817ebe --- /dev/null +++ b/src/test/scala/sims/test/gui/RichShape.scala @@ -0,0 +1,49 @@ +package sims.test.gui + +import processing.core.PApplet +import processing.core.PConstants._ +import sims.dynamics._ + +class RichShape(shape: Shape) { + private implicit def double2Float(x: Double): Float = x.toFloat + + def toGraphical(implicit parent: PApplet) = new GraphicalShape(shape) { + + val top = parent + + val render = shape match { + + case c: Circle => () => { + top.pushMatrix() + top.stroke(0, 0, 0) + top.fill(0, 0, 255, 200) + top.translate(c.position.x, c.position.y) + top.rotate(-c.rotation) + top.ellipseMode(CENTER) + top.ellipse(0, 0, c.radius * 2, c.radius * 2) + top.line(0,0, c.radius, 0) + top.popMatrix() + } + + case r: Rectangle => () => { + top.pushMatrix() + top.translate(r.position.x, r.position.y) + top.rotate(-r.rotation) + top.fill(255, 0, 0, 200) + top.rectMode(CENTER) + top.rect(0, 0, r.halfWidth * 2, r.halfHeight * 2) + top.popMatrix() + } + + case _ => throw new IllegalArgumentException("Cannot create graphical shape: unknown shape.") + } + + } + +} + +object RichShape { + + implicit def shapeToRichShape(s: Shape) = new RichShape(s) + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/Scene.scala b/src/test/scala/sims/test/gui/Scene.scala new file mode 100644 index 0000000..6e1664e --- /dev/null +++ b/src/test/scala/sims/test/gui/Scene.scala @@ -0,0 +1,13 @@ +package sims.test.gui + +trait Scene extends Reactor { + def name: String = this.getClass().getName() + def description: String = "" + + val world = new DebugWorld + + def init(): Unit + + def exit(): Unit = {} + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/SceneManager.scala b/src/test/scala/sims/test/gui/SceneManager.scala new file mode 100644 index 0000000..39da308 --- /dev/null +++ b/src/test/scala/sims/test/gui/SceneManager.scala @@ -0,0 +1,95 @@ +package sims.test.gui + +import processing.core.PApplet +import scala.collection.mutable.ArrayBuffer +import sims.math._ +import sims.test.gui.scenes._ +import sims.test.gui.RichShape._ +import sims.test.gui.RichJoint._ + +class SceneManager(implicit top: PApplet) { + + /* Contains objects that will be rendered on `draw()`. */ + private var _graphicals = new ArrayBuffer[Graphical[_]] + def graphicals: Seq[Graphical[_]] = _graphicals + + /* Current scene. */ + private var _currentScene: Scene = EmptyScene + + /* Get current scene. */ + def currentScene = _currentScene + + /* Set current scene. */ + def currentScene_=(newScene: Scene) = { + + // remove reactions + currentScene.deafTo(currentScene.world) + currentScene.reactions.clear() + + // empty world + currentScene.world.clear() + + // clear graphical objects + _graphicals.clear() + + // custom exit behavior + currentScene.exit() + + // add new reactions to create / remove graphical objects + newScene.listenTo(newScene.world) + newScene.reactions += { + case BodyAdded(newScene.world, body) => for (s <- body.shapes) _graphicals += s.toGraphical + case BodyRemoved(newScene.world, body) => for (s <- body.shapes) { + val index = _graphicals.findIndexOf((g: Graphical[_]) => g match { + case gs: GraphicalShape => gs.physical == s + case _ => false + }) + _graphicals.remove(index) + } + + case JointAdded(newScene.world, joint) => _graphicals += joint.toGraphical + case JointRemoved(newScene.world, joint) => { + val index = _graphicals.findIndexOf((g: Graphical[_]) => g match { + case gj: GraphicalJoint => gj.physical == joint + case _ => false + }) + _graphicals.remove(index) + } + + } + + // custom initialization + newScene.init() + + // set current scene + _currentScene = newScene + + println("set scene to '" + currentScene.name + "'") + } + + private var currentSceneIndex = 0 + val scenes = List( + BasicScene, + CollisionScene, + LongCollisionScene, + CloudScene, + PyramidScene, + ShiftedStackScene, + JointScene + ) + + def nextScene() = { + currentSceneIndex += 1 + currentScene = scenes(mod(currentSceneIndex, scenes.length)) + } + + def previousScene() = { + currentSceneIndex -= 1 + currentScene = scenes(mod(currentSceneIndex, scenes.length)) + } + + def restartScene() = { + currentScene = currentScene + } + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/events.scala b/src/test/scala/sims/test/gui/events.scala new file mode 100644 index 0000000..22015ce --- /dev/null +++ b/src/test/scala/sims/test/gui/events.scala @@ -0,0 +1,49 @@ +package sims.test.gui + +import scala.collection.mutable.ListBuffer +import sims.dynamics._ + +trait Event +case class BodyAdded(world: World, body: Body) extends Event +case class BodyRemoved(world: World, body: Body) extends Event +case class Stepped(wordl: World) extends Event +case class JointAdded(world: World, joint: Joint) extends Event +case class JointRemoved(world: World, joint: Joint) extends Event + +object Reactions { + class Impl extends Reactions { + private val parts = new ListBuffer[Reaction] + def isDefinedAt(e: Event) = parts exists (_ isDefinedAt e) + def +=(r: Reaction) = parts += r + def -=(r: Reaction) = parts -= r + def clear() = parts.clear() + def apply(e: Event) { + for (p <- parts; if p isDefinedAt e) p(e) + } + } + type Reaction = PartialFunction[Event, Unit] +} + +abstract class Reactions extends Reactions.Reaction { + def +=(r: Reactions.Reaction): Unit + def -=(r: Reactions.Reaction): Unit + def clear(): Unit +} + +trait Reactor { + val reactions: Reactions = new Reactions.Impl + + def listenTo(ps: Publisher*) = for (p <- ps) p.subscribe(reactions) + def deafTo(ps: Publisher*) = for (p <- ps) p.unsubscribe(reactions) +} + +trait Publisher { + import Reactions._ + + private val listeners = new ListBuffer[Reaction] + + def subscribe(listener: Reaction) = listeners += listener + def unsubscribe(listener: Reaction) = listeners -= listener + + def publish(e: Event) { for (l <- listeners) l(e) } +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/graphicals.scala b/src/test/scala/sims/test/gui/graphicals.scala new file mode 100644 index 0000000..39b2ea0 --- /dev/null +++ b/src/test/scala/sims/test/gui/graphicals.scala @@ -0,0 +1,13 @@ +package sims.test.gui + +import processing.core.PApplet +import sims.dynamics.Shape +import sims.dynamics.Joint + +abstract class Graphical[+A](val physical: A) { + val top: PApplet + val render: () => Unit +} + +abstract class GraphicalShape(val shape: Shape) extends Graphical[Shape](shape) +abstract class GraphicalJoint(val joint: Joint) extends Graphical[Joint](joint) \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/scenes/BasicScene.scala b/src/test/scala/sims/test/gui/scenes/BasicScene.scala new file mode 100644 index 0000000..9598ab1 --- /dev/null +++ b/src/test/scala/sims/test/gui/scenes/BasicScene.scala @@ -0,0 +1,18 @@ +package sims.test.gui +package scenes + +import sims.math._ +import sims.dynamics._ +import sims.dynamics._ + +object BasicScene extends Scene { + + def init() = { + world.gravity = Vector2D.Null + val s = new Circle(1) + world += new Body(s) {linearVelocity = Vector2D(0.1, 0.01); angularVelocity = 1} + world += new Body(new Rectangle(2,1)) {linearVelocity = Vector2D(0.1, 0.01); angularVelocity = 1} + } + + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/scenes/CloudScene.scala b/src/test/scala/sims/test/gui/scenes/CloudScene.scala new file mode 100644 index 0000000..660e309 --- /dev/null +++ b/src/test/scala/sims/test/gui/scenes/CloudScene.scala @@ -0,0 +1,47 @@ +package sims.test.gui +package scenes + +import sims.math._ +import sims.dynamics._ + +object CloudScene extends Scene { + override def description = "A cloud of circles." + + val MaxItems = 1000 + val MaxItemSize = 0.2 + val Width = 10 + val Height = 10 + + val random = new scala.util.Random(1234567890) + def randomCircles(): Seq[Body] = for (i <- 0 until MaxItems) yield { + val rX = random.nextDouble * Width + val rY = random.nextDouble * Height + val c = new Circle(random.nextDouble * MaxItemSize) { + position = Vector2D(rX, rY) + } + new Body(c) { + linearVelocity = Vector2D(random.nextDouble * (if (random.nextBoolean) 1 else -1), random.nextDouble * (if (random.nextBoolean) 1 else -1)) + angularVelocity = random.nextDouble * (if (random.nextBoolean) 1 else -1) * 10 + } + } + + def frame() = { + val points = List(Vector2D(-1, -1), Vector2D(11, -1), Vector2D(11, 11), Vector2D(-1, 11)) + for (i <- 0 until points.length) yield { + val sp = points(i) + val ep = points((i + 1) % points.length) + val center = (sp + ep) / 2 + val r = new Rectangle((center - ep).length, 0.2) { + position = center + rotation = math.Pi / 2 * i + } + new Body(r) {fixed = true} + } + } + + override def init() = { + world.gravity = Vector2D.Null + for (r <- randomCircles()) world += r + //for (r <- frame()) world += r + } +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/scenes/CollisionScene.scala b/src/test/scala/sims/test/gui/scenes/CollisionScene.scala new file mode 100644 index 0000000..cf2ea61 --- /dev/null +++ b/src/test/scala/sims/test/gui/scenes/CollisionScene.scala @@ -0,0 +1,41 @@ +package sims.test.gui +package scenes + +import sims.dynamics._ +import sims.math._ +import sims.dynamics._ + +object CollisionScene extends Scene { + override def description = "A basic collision detection test." + + var c1 = (new Circle(1) {restitution = 1.0}).asBody + var c2 = (new Circle(1) {position = Vector2D(3, 0)}).asBody + var r1 = (new Rectangle(0.5, 0.5) {position = Vector2D(6,0)}).asBody + + def init() = { + c1 = (new Circle(1) {restitution = 1.0}).asBody + c2 = (new Circle(1) {position = Vector2D(3, 0); restitution = 1.0}).asBody + r1 = (new Rectangle(0.5, 0.5) {position = Vector2D(6,0)}).asBody + + c1.linearVelocity = Vector2D(1, 0) + + world.gravity = Vector2D(0, 0) + world += c1 + world += c2 + world += r1 + + reactions += { + case Stepped(`world`) => + println( + "p: " + (c1.linearMomentum.length + c2.linearMomentum.length + r1.linearMomentum.length).toFloat + + "\tE: " + (E(c1) + E(c2) + E(r1)).toFloat + ) + } + } + + + def E(b: Body) = { + (b.linearVelocity dot b.linearVelocity) * b.mass / 2 + } + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/scenes/EmptyScene.scala b/src/test/scala/sims/test/gui/scenes/EmptyScene.scala new file mode 100644 index 0000000..a88679a --- /dev/null +++ b/src/test/scala/sims/test/gui/scenes/EmptyScene.scala @@ -0,0 +1,6 @@ +package sims.test.gui +package scenes + +object EmptyScene extends Scene { + def init() = () +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/scenes/JointScene.scala b/src/test/scala/sims/test/gui/scenes/JointScene.scala new file mode 100644 index 0000000..4a0726f --- /dev/null +++ b/src/test/scala/sims/test/gui/scenes/JointScene.scala @@ -0,0 +1,107 @@ +package sims.test.gui +package scenes + +import sims.dynamics._ +import sims.math._ +import sims.dynamics._ + +object JointScene extends Scene { + + override def init() = { + val b1 = new Body(new Circle(0.1)) {fixed = true} + val b2 = new Body(new Rectangle(0.1, 0.5)) {position = Vector2D(2,2)} + val j = new DistanceJoint(b1, b1.position, b2, b2.position + Vector2D(0, -0.5)) + world += b1 + world += b2 + world += j + + val chainBodies = for (i <- 0 until 10) yield new Body(new Rectangle(0.5, 0.1)){fixed = i == 0 || i == 9; position = Vector2D(i, 5)} + val chainHinges = for (i <- 0 until chainBodies.length - 1) yield + new RevoluteJoint(chainBodies(i), chainBodies(i + 1), Vector2D(i + 0.5, 5)) + for (b <- chainBodies) world += b + for (j <- chainHinges) world += j + + import sims.dsl._ + val c = new Body(new Circle(0.1)) {position = Vector2D(4, 0)} + world += c + world += c distance chainBodies(4) + + val r = new Body(new Rectangle(2, 0.1)) {position = Vector2D(4, 0)} + world += r + world += c revolute r + + val c2 = new Body(new Circle(0.2)) {position = Vector2D(2, 2)} + world += c2 + world += r :@@ (-2, 0) distance c2 + + val r2 = new Body(new Rectangle(0.1, 0.2)) {position = Vector2D(6, 2)} + world += r2 + world += r :@@ (2, 0) distance (0, -0.2) @@: r2 + + val r3 = new Body(new Rectangle(0.3, 0.1)) {position = Vector2D(6.3, 2.2)} + world += r3 + world += r2 :@ (6, 2.2) revolute r3 + + // chaos pendulum + { + val c1 = new Body(new Circle(0.1)) {fixed = true; position = Vector2D(12, 2)} + world += c1 + val r1 = new Body(new Rectangle(1, 0.1)) {position = Vector2D(13, 2)} + world += r1 + val r2 = new Body(new Rectangle(1, 0.1)) {position = Vector2D(15, 2)} + world += r2 + + world += c1 revolute r1 + world += r1 :@@ (1, 0) revolute r2 + + } + + // net + { + val w = 10 + val h = 10 + val d = 0.2 + + val nodes = + for (i <- (0 until w).toArray) yield + for (j <- (0 until h).toArray) yield + new Body(new Circle(0.05)) {fixed = i == 0 && j == h - 1 ; position = Vector2D(i * d, j * d) + Vector2D(-3, 2)} + + for (n <- nodes.flatten) world += n + + val joints = { + var r: List[DistanceJoint] = Nil + for(i <- 0 to nodes.length - 1; j <- 0 to nodes(i).length - 1) { + if (i > 0) + r = (nodes(i-1)(j) distance nodes(i)(j)) :: r + if (j > 0) + r = (nodes(i)(j-1) distance nodes(i)(j)) :: r + } + r + } + + for (j <- joints) world += j + + } + + + world.collisionDetection = false + world.iterations = 10 + world.errorReduction = 1 + + + /* + val r1 = new Body(new Rectangle(0.5, 0.1)) {fixed = true; position = Vector2D(5, 5)} + val r2 = new Body(new Rectangle(0.5, 0.1)) {position = Vector2D(6, 5)} + val r3 = new Body(new Rectangle(0.5, 0.1)) {position = Vector2D(7, 5)} + val j12 = new RevoluteJoint(r1, r2, Vector2D(5.5, 5)) + val j23 = new RevoluteJoint(r2, r3, Vector2D(6.5, 5)) + world += r1 + world += r2 + world += r3 + world += j12 + world += j23 + */ + } + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/scenes/LongCollisionScene.scala b/src/test/scala/sims/test/gui/scenes/LongCollisionScene.scala new file mode 100644 index 0000000..d153f1b --- /dev/null +++ b/src/test/scala/sims/test/gui/scenes/LongCollisionScene.scala @@ -0,0 +1,54 @@ +package sims.test.gui +package scenes + +import sims.dynamics._ +import sims.math._ +import sims.dynamics._ + +object LongCollisionScene extends Scene { + override def description = "A test to verify conservation in a collision." + + def makeBodies() = (for (i <- 0 until 10) yield { + new Circle(0.2) { + position = Vector2D(0 + 2.01 * radius * i, 0) + restitution = 0.8 + } + }.asBody) ++ (for (i <- 0 until 10) yield { + new Circle(0.2) { + position = Vector2D(6 + 2.01 * radius * i, 0) + restitution = 0.8 + } + }.asBody) + + override def init() = { + val bodies = makeBodies() + bodies(0).fixed = true + bodies(19).fixed = true + for (b <- bodies) world += b + world.gravity = Vector2D.Null + + val bullet = new Body(new Circle(0.2) { + position = Vector2D(5, 0) + restitution = 0.8}) { + linearVelocity = Vector2D(3, 0) + } + world += bullet + + world.iterations = 10 + + registerListeners() + } + + def registerListeners() = { + reactions += { + case Stepped(`world`) => println( + "P: " + world.bodies.map(P(_)).sum.toFloat + + "\tE: " + world.bodies.map(E(_)).sum.toFloat + ) + } + } + + def P(b: Body) = if (b.fixed) 0 else b.linearMomentum.length + def E(b: Body) = if (b.fixed) 0 else (b.linearVelocity dot b.linearVelocity) * b.mass * 0.5 + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/scenes/PyramidScene.scala b/src/test/scala/sims/test/gui/scenes/PyramidScene.scala new file mode 100644 index 0000000..19438de --- /dev/null +++ b/src/test/scala/sims/test/gui/scenes/PyramidScene.scala @@ -0,0 +1,40 @@ +package sims.test.gui +package scenes + +import sims.math._ +import sims.dynamics._ +import sims.dynamics._ + +object PyramidScene extends Scene { + override def description = "A pyramid made out of circles." + + val Base = 40 + var Radius = 0.2 + + val s = math.sqrt(3) + + def pyramid: Seq[Body] = (for (i <- 0 until Base) yield { + for (j <- 0 until Base - i) yield new Body( + new Circle(Radius) { + position = Vector2D(2 * j * Radius + i * Radius, s * i * Radius) + restitution = 0.5 + } + ) {fixed = (i == 0)} + }).flatten + + override def init() = { + //world.gravity = Vector2D.Null + for (b <- pyramid) world += b + + val b = new Circle(0.3) { + override val density = 10.0 + position = Vector2D(4,15) + } + val bd = new Body(b){ + linearVelocity = Vector2D(0, -2.5) + } + + world += bd + } + +} \ No newline at end of file diff --git a/src/test/scala/sims/test/gui/scenes/ShiftedStackScene.scala b/src/test/scala/sims/test/gui/scenes/ShiftedStackScene.scala new file mode 100644 index 0000000..c8a7af6 --- /dev/null +++ b/src/test/scala/sims/test/gui/scenes/ShiftedStackScene.scala @@ -0,0 +1,30 @@ +package sims.test.gui +package scenes + +import sims.math._ +import sims.dynamics._ + +object ShiftedStackScene extends Scene { + override def description = "A stack of shifted rectangles." + /*override val world = new DebugWorld{ + import sims.collision._ + import sims.collision.narrowphase._ + import sims.collision.broadphase._ + override val detector = SAP[Shape] narrowedBy new sims.test.gjk.GJK[Shape] + }*/ + + val width = 1.0 + val height = 0.2 + + def stack() = for (i <- 0 until 2) yield + new Body(new Rectangle(width / 2, height / 2) { + position = Vector2D(0.25 * (i % 2) , i * height) + restitution = 0.0 + }) {fixed = i == 0} + + override def init() = { + for (s <- stack()) world += s + world.iterations = 100 + } + +} \ No newline at end of file -- cgit v1.2.3