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