From bf409987ed5d6c9c32af8a574c402e69973f27ed Mon Sep 17 00:00:00 2001 From: Hanno Braun Date: Thu, 5 Mar 2009 09:08:17 +0100 Subject: [PATCH] Physics: Implemented continious collision detection between circles in SimpleNarrowPhase. --- src/net/habraun/kong/physics/NarrowPhase.scala | 4 +- .../habraun/kong/physics/SimpleNarrowPhase.scala | 53 ++++++++- src/net/habraun/kong/physics/World.scala | 4 +- .../kong/physics/SimpleNarrowPhaseTest.scala | 132 +++++++++++++++++---- test/net/habraun/kong/physics/WorldTest.scala | 8 +- 5 files changed, 168 insertions(+), 33 deletions(-) diff --git a/src/net/habraun/kong/physics/NarrowPhase.scala b/src/net/habraun/kong/physics/NarrowPhase.scala index f729a2d..f0a5ac1 100644 --- a/src/net/habraun/kong/physics/NarrowPhase.scala +++ b/src/net/habraun/kong/physics/NarrowPhase.scala @@ -30,9 +30,9 @@ package net.habraun.kong.physics trait NarrowPhase { /** - * Determines if the two bodies collides, and if they do, returns a Collision object that describes the + * Determines if the two bodies collide, and if they do, returns a Collision object that describes the * collision. If the bodies do not collide, this method returns None. */ - def inspectCollision(b1: Body, b2: Body): Option[Collision] + def inspectCollision(delta: Double, b1: Body, b2: Body): Option[Collision] } diff --git a/src/net/habraun/kong/physics/SimpleNarrowPhase.scala b/src/net/habraun/kong/physics/SimpleNarrowPhase.scala index 4d07cce..f046f21 100644 --- a/src/net/habraun/kong/physics/SimpleNarrowPhase.scala +++ b/src/net/habraun/kong/physics/SimpleNarrowPhase.scala @@ -22,20 +22,61 @@ package net.habraun.kong.physics class SimpleNarrowPhase extends NarrowPhase { - def inspectCollision(b1: Body, b2: Body) = { + def inspectCollision(delta: Double, b1: Body, b2: Body) = { if (b1.shape.isInstanceOf[Circle] && b2.shape.isInstanceOf[Circle]) { + // This algorithms does continious collision detection between two moving circles. I got this + // from "Real-Time Collision Detection" by Christer Ericson, page 223/224. + + // The two (possibly) colliding circles. val circle1 = b1.shape.asInstanceOf[Circle] val circle2 = b2.shape.asInstanceOf[Circle] - val dSquared = (b1.position - b2.position).squaredLength - val radii = circle1.radius + circle2.radius - if (dSquared <= radii * radii) { + + val s = b2.position - b1.position // vector between sphere centers + val v = (b2.velocity - b1.velocity) * delta // relative motion between the circles + val r = circle1.radius + circle2.radius // the sum of both radii + + // The time of impact is given by the smaller solution of the quadratic equation + // at^2 + 2bt + c = 0. + val a = v * v //a, b and c from the equation in the comment above + val b = v * s + val c = (s * s) - (r * r) + val d = (b * b) - (a * c) // the discriminant of the solution + + // Check for several corner cases. If none of these occurs, we can compute t after the general + // formula. + if (c < 0.0) { + // Spheres are initially overlapping. val normal1 = (b2.position - b1.position).normalize val normal2 = (b1.position - b2.position).normalize - Some(Collision(1.0, Contact(b1, b2, normal1, normal2, Vec2D(0, 0)))) + val point = Vec2D(0, 0) // This doesn't really make sense. The point should be the real point + // of impact and t should be negative. + Some(Collision(0.0, Contact(b1, b2, normal1, normal2, point))) } - else { + else if (a == 0) { + // Spheres are not moving relative to each other. None } + else if (b >= 0.0) { + // Spheres are not moving towards each other. + None + } + else if (d < 0.0) { + // Discriminant is negative, no real solution. + None + } + else { + // None of the edge cases has occured, so we need to compute the time of contact. + val t = (-b - Math.sqrt(d)) / a + if (t <= 1.0) { + val normal1 = (b2.position - b1.position).normalize + val normal2 = (b1.position - b2.position).normalize + val point = b1.position + (b1.velocity * delta * t) + (normal1 * circle1.radius) + Some(Collision(t, Contact(b1, b2, normal1, normal2, point))) + } + else { + None + } + } } else if (b1.shape.isInstanceOf[Circle] && b2.shape.isInstanceOf[LineSegment]) { circleLineSegment(b1, b2, b1.shape.asInstanceOf[Circle], b2.shape.asInstanceOf[LineSegment]) diff --git a/src/net/habraun/kong/physics/World.scala b/src/net/habraun/kong/physics/World.scala index 8561b52..3e5f3bc 100644 --- a/src/net/habraun/kong/physics/World.scala +++ b/src/net/habraun/kong/physics/World.scala @@ -116,7 +116,9 @@ class World { // Collision detection. val possibleCollisionPairs = broadPhase.detectPossibleCollisions(bodies.toList) - val possibleCollisions = possibleCollisionPairs.map((pair) => narrowPhase.inspectCollision(pair._1, pair._2)) + val possibleCollisions = possibleCollisionPairs.map((pair) => { + narrowPhase.inspectCollision(delta, pair._1, pair._2) + }) // Compute collision effects. // This is a tricky construction. The "possibleCollision <- collision" part is like an outer for loop diff --git a/test/net/habraun/kong/physics/SimpleNarrowPhaseTest.scala b/test/net/habraun/kong/physics/SimpleNarrowPhaseTest.scala index c33e2be..f08a9cd 100644 --- a/test/net/habraun/kong/physics/SimpleNarrowPhaseTest.scala +++ b/test/net/habraun/kong/physics/SimpleNarrowPhaseTest.scala @@ -44,13 +44,13 @@ class SimpleNarrowPhaseTest { val b2 = new Body b2.shape = NoShape - assertEquals(None, narrowPhase.inspectCollision(b1, b2)) + assertEquals(None, narrowPhase.inspectCollision(0.0, b1, b2)) } @Test - def inspectTwoCirclesExpectNoCollision { + def inspectTwoStationaryCirclesExpectNoCollision { val narrowPhase = new SimpleNarrowPhase val b1 = new Body @@ -60,43 +60,135 @@ class SimpleNarrowPhaseTest { b2.position = Vec2D(3, 0) b2.shape = Circle(1) - assertEquals(None, narrowPhase.inspectCollision(b1, b2)) + assertEquals(None, narrowPhase.inspectCollision(2.0, b1, b2)) } @Test - def inspectTwoCirclesExpectCollision { + def inspectTwoStationaryCirclesExpectCollision { val narrowPhase = new SimpleNarrowPhase val b1 = new Body b1.position = Vec2D(0, 0) + b1.shape = Circle(2) + val b2 = new Body + b2.position = Vec2D(3, 0) + b2.shape = Circle(2) + + val expectedCollision = Collision(0.0, Contact(b1, b2, Vec2D(1, 0), Vec2D(-1, 0), Vec2D(0, 0))) + + assertEquals(Some(expectedCollision), narrowPhase.inspectCollision(2.0, b1, b2)) + } + + + + @Test + def inspectTwoCirclesOneMovingExpectCollision { + val narrowPhase = new SimpleNarrowPhase + + val b1 = new Body + b1.position = Vec2D(-1, 0) + b1.velocity = Vec2D(1, 0) b1.shape = Circle(1) val b2 = new Body - b2.position = Vec2D(0.5, 0) + b2.position = Vec2D(2, 0) b2.shape = Circle(1) - val expectedCollision = Collision(1.0, Contact(b1, b2, Vec2D(1, 0), Vec2D(-1, 0), Vec2D(0, 0))) + val expectedCollision = Collision(0.5, Contact(b1, b2, Vec2D(1, 0), Vec2D(-1, 0), Vec2D(1, 0))) - assertEquals(Some(expectedCollision), narrowPhase.inspectCollision(b1, b2)) + assertEquals(Some(expectedCollision), narrowPhase.inspectCollision(2.0, b1, b2)) } @Test - def inspectTwoCirclesExpectCollision2 { + def inspectTwoCirclesOneMovingExpectNoCollision { val narrowPhase = new SimpleNarrowPhase val b1 = new Body - b1.position = Vec2D(0, 0) - b1.shape = Circle(2) + b1.position = Vec2D(-3, 0) + b1.velocity = Vec2D(1, 0) + b1.shape = Circle(1) val b2 = new Body - b2.position = Vec2D(3, 0) - b2.shape = Circle(2) + b2.position = Vec2D(2, 0) + b2.shape = Circle(1) + + assertEquals(None, narrowPhase.inspectCollision(2.0, b1, b2)) + } + + + + @Test + def inspectTwoMovingCirclesExpectCollision { + val narrowPhase = new SimpleNarrowPhase + + val b1 = new Body + b1.shape = Circle(1) + b1.position = Vec2D(-3, 0) + b1.velocity = Vec2D(1, 0) + val b2 = new Body + b2.shape = Circle(1) + b2.position = Vec2D(1, 0) + b2.velocity = Vec2D(-1, 0) + + val expectedCollision = Collision(0.5, Contact(b1, b2, Vec2D(1, 0), Vec2D(-1, 0), Vec2D(-1, 0))) + + assertEquals(Some(expectedCollision), narrowPhase.inspectCollision(2.0, b1, b2)) + } - val expectedCollision = Collision(1.0, Contact(b1, b2, Vec2D(1, 0), Vec2D(-1, 0), Vec2D(0, 0))) - assertEquals(Some(expectedCollision), narrowPhase.inspectCollision(b1, b2)) + + @Test + def inspectTwoMovingCirclesExpectNoCollision { + val narrowPhase = new SimpleNarrowPhase + + val b1 = new Body + b1.shape = Circle(1) + b1.position = Vec2D(-3, 0) + b1.velocity = Vec2D(1, 0) + val b2 = new Body + b2.shape = Circle(1) + b2.position = Vec2D(4, 0) + b2.velocity = Vec2D(-1, 0) + + assertEquals(None, narrowPhase.inspectCollision(2.0, b1, b2)) + } + + + + @Test + def inspectTwoMovingCirclesExpectNoCollision2 { + val narrowPhase = new SimpleNarrowPhase + + val b1 = new Body + b1.shape = Circle(1) + b1.position = Vec2D(-2, 0) + b1.velocity = Vec2D(-1, 0) + val b2 = new Body + b2.shape = Circle(1) + b2.position = Vec2D(2, 0) + b2.velocity = Vec2D(1, 0) + + assertEquals(None, narrowPhase.inspectCollision(2.0, b1, b2)) + } + + + + @Test + def inspectTwoMovingCirclesWithIntersectingCoursesExpectNoCollision { + val narrowPhase = new SimpleNarrowPhase + + val b1 = new Body + b1.shape = Circle(1) + b1.position = Vec2D(0, 0) + b1.velocity = Vec2D(0, 5) + val b2 = new Body + b2.shape = Circle(1) + b2.position = Vec2D(5, 0) + b2.velocity = Vec2D(-5, 0) + + assertEquals(None, narrowPhase.inspectCollision(2.0, b1, b2)) } @@ -112,7 +204,7 @@ class SimpleNarrowPhaseTest { b2.position = Vec2D(0, 0.5) b2.shape = NoShape - assertEquals(None, narrowPhase.inspectCollision(b1, b2)) + assertEquals(None, narrowPhase.inspectCollision(0.0, b1, b2)) } @@ -128,7 +220,7 @@ class SimpleNarrowPhaseTest { b2.position = Vec2D(0, 2) b2.shape = LineSegment(Vec2D(-1, 0), Vec2D(1, 0)) - assertEquals(None, narrowPhase.inspectCollision(b1, b2)) + assertEquals(None, narrowPhase.inspectCollision(0.0, b1, b2)) } @@ -144,7 +236,7 @@ class SimpleNarrowPhaseTest { b2.position = Vec2D(0, 2) b2.shape = LineSegment(Vec2D(-1, 0), Vec2D(1, 0)) - assertEquals(None, narrowPhase.inspectCollision(b2, b1)) + assertEquals(None, narrowPhase.inspectCollision(0.0, b2, b1)) } @@ -162,7 +254,7 @@ class SimpleNarrowPhaseTest { val expectedCollision = Collision(1.0, Contact(b1, b2, Vec2D(0, 1), Vec2D(0, -1), Vec2D(0, 0))) - assertEquals(Some(expectedCollision), narrowPhase.inspectCollision(b1, b2)) + assertEquals(Some(expectedCollision), narrowPhase.inspectCollision(0.0, b1, b2)) } @@ -180,7 +272,7 @@ class SimpleNarrowPhaseTest { val expectedCollision = Collision(1.0, Contact(b2, b1, Vec2D(0, -1), Vec2D(0, 1), Vec2D(0, 0))) - assertEquals(Some(expectedCollision), narrowPhase.inspectCollision(b2, b1)) + assertEquals(Some(expectedCollision), narrowPhase.inspectCollision(0.0, b2, b1)) } @@ -196,6 +288,6 @@ class SimpleNarrowPhaseTest { b2.position = Vec2D(0, -1) b2.shape = LineSegment(Vec2D(0, 0), Vec2D(2, 2)) - assertEquals(None, narrowPhase.inspectCollision(b1, b2)) + assertEquals(None, narrowPhase.inspectCollision(0.0, b1, b2)) } } diff --git a/test/net/habraun/kong/physics/WorldTest.scala b/test/net/habraun/kong/physics/WorldTest.scala index c144f16..00e2c46 100644 --- a/test/net/habraun/kong/physics/WorldTest.scala +++ b/test/net/habraun/kong/physics/WorldTest.scala @@ -224,7 +224,7 @@ class WorldTest { val narrowPhase = new NarrowPhase { var passedPairs: List[(Body, Body)] = Nil - def inspectCollision(b1: Body, b2: Body) = { passedPairs = passedPairs:::List((b1, b2)); None } + def inspectCollision(delta: Double, b1: Body, b2: Body) = { passedPairs = passedPairs:::List((b1, b2)); None } } world.narrowPhase = narrowPhase @@ -249,7 +249,7 @@ class WorldTest { b2.velocity = Vec2D(5, 5) world.narrowPhase = new NarrowPhase { - def inspectCollision(b1: Body, b2: Body) = { + def inspectCollision(delta: Double, b1: Body, b2: Body) = { Some(Collision(1.0, Contact(b1, b2, Vec2D(0, -1), Vec2D(0, 1), Vec2D(0, 0)))) } } @@ -275,7 +275,7 @@ class WorldTest { b2.velocity = Vec2D(1, 1) world.narrowPhase = new NarrowPhase { - def inspectCollision(b1: Body, b2: Body) = { + def inspectCollision(delta: Double, b1: Body, b2: Body) = { Some(Collision(1.0, Contact(b1, b2, Vec2D(0, -1), Vec2D(0, 1), Vec2D(0, 0)))) } } @@ -301,7 +301,7 @@ class WorldTest { b2.mass = Double.PositiveInfinity world.narrowPhase = new NarrowPhase { - def inspectCollision(b1: Body, b2: Body) = { + def inspectCollision(delta: Double, b1: Body, b2: Body) = { Some(Collision(1.0, Contact(b1, b2, Vec2D(0, 1), Vec2D(0, -1), Vec2D(0, 0)))) } } -- 2.11.4.GIT