src/net/habraun/sd/SimpleNarrowPhase.scala

   1 /*
   2         Copyright (c) 2009 Hanno Braun <hanno@habraun.net>
   3
   4         Licensed under the Apache License, Version 2.0 (the "License");
   5         you may not use this file except in compliance with the License.
   6         You may obtain a copy of the License at
   7
   8                 http://www.apache.org/licenses/LICENSE-2.0
   9
  10         Unless required by applicable law or agreed to in writing, software
  11         distributed under the License is distributed on an "AS IS" BASIS,
  12         WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13         See the License for the specific language governing permissions and
  14         limitations under the License.
  15 */
  16
  17
  18
  19 package net.habraun.sd
  20
  21
  22
  23 import math._
  24
  25
  26
  27 class SimpleNarrowPhase extends NarrowPhase {
  28
  29         def inspectCollision(delta: Double, b1: Body, b2: Body) = {
  30                 if (b1.shape.isInstanceOf[Circle] && b2.shape.isInstanceOf[Circle]) {
  31                         // This algorithms does continious collision detection between two moving circles. I got this
  32                         // from "Real-Time Collision Detection" by Christer Ericson, page 223/224.
  33
  34                         // The two (possibly) colliding circles.
  35                         val circle1 = b1.shape.asInstanceOf[Circle]
  36                         val circle2 = b2.shape.asInstanceOf[Circle]
  37
  38                         val s = b2.position - b1.position // vector between sphere centers
  39                         val v = (b2.velocity - b1.velocity) * delta // relative motion between the circles
  40                         val r = circle1.radius + circle2.radius // the sum of both radii
  41
  42                         // The time of impact is given by the smaller solution of the quadratic equation
  43                         // at^2 + 2bt + c = 0.
  44                         val a = v * v //a, b and c from the equation in the comment above
  45                         val b = v * s
  46                         val c = (s * s) - (r * r)
  47                         val d = (b * b) - (a * c) // the discriminant of the solution
  48
  49                         // Check for several corner cases. If none of these occurs, we can compute t after the general
  50                         // formula.
  51                         if (c < 0.0) {
  52                                 // Spheres are initially overlapping.
  53                                 val normal1 = (b2.position - b1.position).normalize
  54                                 val normal2 = (b1.position - b2.position).normalize
  55                                 val point = Vec2D(0, 0) // This doesn't really make sense. The point should be the real point
  56                                                         // of impact and t should be negative.
  57                                 Some(Collision(0.0, Contact(b1, b2, normal1, normal2, point)))
  58                         }
  59                         else if (a == 0) {
  60                                 // Spheres are not moving relative to each other.
  61                                 None
  62                         }
  63                         else if (b >= 0.0) {
  64                                 // Spheres are not moving towards each other.
  65                                 None
  66                         }
  67                         else if (d < 0.0) {
  68                                 // Discriminant is negative, no real solution.
  69                                 None
  70                         }
  71                         else {
  72                                 // None of the edge cases has occured, so we need to compute the time of contact.
  73                                 val t = (-b - Math.sqrt(d)) / a
  74                                 if (t <= 1.0) {
  75                                         val normal1 = (b2.position - b1.position).normalize
  76                                         val normal2 = (b1.position - b2.position).normalize
  77                                         val point = b1.position + (b1.velocity * delta * t) + (normal1 * circle1.radius)
  78                                         Some(Collision(t, Contact(b1, b2, normal1, normal2, point)))
  79                                 }
  80                                 else {
  81                                         None
  82                                 }
  83                         }
  84                 }
  85                 else if ((b1.shape.isInstanceOf[Circle] && b2.shape.isInstanceOf[LineSegment])
  86                                 || (b1.shape.isInstanceOf[LineSegment] && b2.shape.isInstanceOf[Circle])) {
  87                         // This algorithms does continious collision detection between a moving circle and a moving line
  88                         // segment. I got this from "Real-Time Collision Detection" by Christer Ericson, page 219-222.
  89
  90                         // The (possibly) colliding circle and line segment.
  91                         val circleBody = if (b1.shape.isInstanceOf[Circle]) b1 else b2
  92                         val circleShape = (if (b1.shape.isInstanceOf[Circle]) b1 else b2).shape.asInstanceOf[Circle]
  93                         val segmentBody = if (b1.shape.isInstanceOf[LineSegment]) b1 else b2
  94                         val segmentShape = (if (b1.shape.isInstanceOf[LineSegment]) b1 else b2)
  95                                         .shape.asInstanceOf[LineSegment]
  96
  97                         // For this algorithm, we need the normal and the distance from the origin, which together define
  98                         // the line on which the line segments lies.
  99                         // The normal is one of two possible line normals. The distance is the distance from the origin
 100                         // in units of the normal, which basically means that the distance is negative if the normal
 101                         // points towards the origin, positive if the normal points away from the origin.
 102                         val lineNormal = segmentShape.d.orthogonal.normalize
 103                         val lineDistance = (segmentBody.position + segmentShape.p) * lineNormal
 104
 105                         // Compute the distance between the line and the circle. The distance is positive if the line
 106                         // normal points towards the circle (the circle lies in front of the line), negative otherwise.
 107                         val distance = lineNormal * circleBody.position - lineDistance
 108
 109                         // Check if the circle and the line are already intersecting.
 110                         if (Math.abs(distance) <= circleShape.radius) {
 111                                 // Circle and line are already intersecting.
 112
 113                                 // The collision normals.
 114                                 val nLine = if (distance > 0) lineNormal else -lineNormal
 115                                 val nCircle = -nLine
 116
 117                                 // The point on the line that lies nearest to the circle center.
 118                                 val lambda = (circleBody.position - segmentBody.position - segmentShape.p) * segmentShape.d /
 119                                                 segmentShape.d.squaredLength
 120                                 if (lambda >= 0 && lambda <= segmentShape.d.length) {
 121                                         //val point = segmentBody.position + segmentShape.p + segmentShape.d * lambda
 122                                         Some(Collision(0.0, Contact(circleBody, segmentBody, nCircle, nLine, Vec2D(0, 0))))
 123                                 }
 124                                 else {
 125                                         None
 126                                 }
 127                         }
 128                         else {
 129                                 // Compute the relative velocity between the two bodies. No matter which of the two bodies
 130                                 // actually moves, we will model this as a moving sphere and a stationary line segment.
 131                                 val velocity = (circleBody.velocity - segmentBody.velocity) * delta
 132
 133                                 // Compute the direction of the circle's movement relative to the line normal. A positive
 134                                 // value denotes movment in the direction of the line normal, a negative value the opposite.
 135                                 // A value of zero means, that the sphere moves parallel to the line.
 136                                 val direction = lineNormal * velocity
 137
 138                                 // Check if the circle moves towards the line. This is the case if the direction multiplied
 139                                 // with the distance between the line and the circle is negative.
 140                                 // If both are positive, the circle lies in front of the circle (line normal points towards
 141                                 // it) and moves in the direction of the normal. If both are negative, it lies behind the
 142                                 // line (normal points away from the circle) and it moves against the direction of the
 143                                 // normal.
 144                                 // The value can only be zero if the circle moves parallel to the line. The direction can't
 145                                 // be zero because that has already been ruled out before.
 146                                 if (direction * distance < 0.0) {
 147                                         // The circle moves towards the line. What's left to do is compute the time of impact,
 148                                         // check if it lies within the current timeframe and check if the point of impact lies on
 149                                         // the line segment.
 150
 151                                         // For the following computation, we need the radius of the circle as a positive value if
 152                                         // it lies in front of the plane, as a negative value otherwise.
 153                                         val radius = if (distance > 0.0) circleShape.radius else -circleShape.radius
 154
 155                                         // Compute the time of impact.
 156                                         val t = (radius - distance) / direction
 157
 158                                         // Check if the time is within the movement interval.
 159                                         if (t >= 0.0 && t <= 1.0) {
 160                                                 // The time is within the movement interval, which  means the circle will hit the
 161                                                 // line. Compute the point of impact and the normals.
 162
 163                                                 // The collision normals.
 164                                                 val nLine = if (distance > 0) lineNormal else -lineNormal
 165                                                 val nCircle = -nLine
 166
 167                                                 // Point of impact.
 168                                                 val point = circleBody.position + (nCircle * circleShape.radius) +
 169                                                                 (circleBody.velocity * delta * t)
 170
 171                                                 // We now have the point of impact between the circle and the line. But does this
 172                                                 // point lie on the circle segment?
 173                                                 val pt = (point.x - (segmentBody.position.x + segmentShape.p.x)) / segmentShape.d.x
 174                                                 if (pt >= 0.0 && pt <= 1.0) {
 175                                                         // Yes it does.
 176                                                         Some(Collision(t, Contact(circleBody, segmentBody, nCircle, nLine, point)))
 177                                                 }
 178                                                 else {
 179                                                         // No, it doesn't. No collision.
 180                                                         None
 181                                                 }
 182                                         }
 183                                         else {
 184                                                 None
 185                                         }
 186                                 }
 187                                 else {
 188                                         None
 189                                 }
 190                         }
 191                 }
 192                 else {
 193                         None
 194                 }
 195         }
 196 }