git.maemo.org Git - blok/blob - Box2D/Source/Collision/b2CollideCircle.cpp

   1 /*
   2 * Copyright (c) 2007 Erin Catto http://www.gphysics.com
   3 *
   4 * This software is provided 'as-is', without any express or implied
   5 * warranty.  In no event will the authors be held liable for any damages
   6 * arising from the use of this software.
   7 * Permission is granted to anyone to use this software for any purpose,
   8 * including commercial applications, and to alter it and redistribute it
   9 * freely, subject to the following restrictions:
  10 * 1. The origin of this software must not be misrepresented; you must not
  11 * claim that you wrote the original software. If you use this software
  12 * in a product, an acknowledgment in the product documentation would be
  13 * appreciated but is not required.
  14 * 2. Altered source versions must be plainly marked as such, and must not be
  15 * misrepresented as being the original software.
  16 * 3. This notice may not be removed or altered from any source distribution.
  17 */
  18
  19 #include "b2Collision.h"
  20 #include "Shapes/b2CircleShape.h"
  21 #include "Shapes/b2PolygonShape.h"
  22
  23 void b2CollideCircles(
  24         b2Manifold* manifold,
  25         const b2CircleShape* circle1, const b2XForm& xf1,
  26         const b2CircleShape* circle2, const b2XForm& xf2)
  27 {
  28         manifold->pointCount = 0;
  29
  30         b2Vec2 p1 = b2Mul(xf1, circle1->GetLocalPosition());
  31         b2Vec2 p2 = b2Mul(xf2, circle2->GetLocalPosition());
  32
  33         b2Vec2 d = p2 - p1;
  34         float32 distSqr = b2Dot(d, d);
  35         float32 r1 = circle1->GetRadius();
  36         float32 r2 = circle2->GetRadius();
  37         float32 radiusSum = r1 + r2;
  38         if (distSqr > radiusSum * radiusSum)
  39         {
  40                 return;
  41         }
  42
  43         float32 separation;
  44         if (distSqr < B2_FLT_EPSILON)
  45         {
  46                 separation = -radiusSum;
  47                 manifold->normal.Set(0.0f, 1.0f);
  48         }
  49         else
  50         {
  51                 float32 dist = b2Sqrt(distSqr);
  52                 separation = dist - radiusSum;
  53                 float32 a = 1.0f / dist;
  54                 manifold->normal.x = a * d.x;
  55                 manifold->normal.y = a * d.y;
  56         }
  57
  58         manifold->pointCount = 1;
  59         manifold->points[0].id.key = 0;
  60         manifold->points[0].separation = separation;
  61
  62         p1 += r1 * manifold->normal;
  63         p2 -= r2 * manifold->normal;
  64
  65         b2Vec2 p = 0.5f * (p1 + p2);
  66
  67         manifold->points[0].localPoint1 = b2MulT(xf1, p);
  68         manifold->points[0].localPoint2 = b2MulT(xf2, p);
  69 }
  70
  71 void b2CollidePolygonAndCircle(
  72         b2Manifold* manifold,
  73         const b2PolygonShape* polygon, const b2XForm& xf1,
  74         const b2CircleShape* circle, const b2XForm& xf2)
  75 {
  76         manifold->pointCount = 0;
  77
  78         // Compute circle position in the frame of the polygon.
  79         b2Vec2 c = b2Mul(xf2, circle->GetLocalPosition());
  80         b2Vec2 cLocal = b2MulT(xf1, c);
  81
  82         // Find the min separating edge.
  83         int32 normalIndex = 0;
  84         float32 separation = -B2_FLT_MAX;
  85         float32 radius = circle->GetRadius();
  86         int32 vertexCount = polygon->GetVertexCount();
  87         const b2Vec2* vertices = polygon->GetVertices();
  88         const b2Vec2* normals = polygon->GetNormals();
  89
  90         for (int32 i = 0; i < vertexCount; ++i)
  91         {
  92                 float32 s = b2Dot(normals[i], cLocal - vertices[i]);
  93
  94                 if (s > radius)
  95                 {
  96                         // Early out.
  97                         return;
  98                 }
  99
 100                 if (s > separation)
 101                 {
 102                         separation = s;
 103                         normalIndex = i;
 104                 }
 105         }
 106
 107         // If the center is inside the polygon ...
 108         if (separation < B2_FLT_EPSILON)
 109         {
 110                 manifold->pointCount = 1;
 111                 manifold->normal = b2Mul(xf1.R, normals[normalIndex]);
 112                 manifold->points[0].id.features.incidentEdge = (uint8)normalIndex;
 113                 manifold->points[0].id.features.incidentVertex = b2_nullFeature;
 114                 manifold->points[0].id.features.referenceEdge = 0;
 115                 manifold->points[0].id.features.flip = 0;
 116                 b2Vec2 position = c - radius * manifold->normal;
 117                 manifold->points[0].localPoint1 = b2MulT(xf1, position);
 118                 manifold->points[0].localPoint2 = b2MulT(xf2, position);
 119                 manifold->points[0].separation = separation - radius;
 120                 return;
 121         }
 122
 123         // Project the circle center onto the edge segment.
 124         int32 vertIndex1 = normalIndex;
 125         int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
 126         b2Vec2 e = vertices[vertIndex2] - vertices[vertIndex1];
 127
 128         float32 length = e.Normalize();
 129         b2Assert(length > B2_FLT_EPSILON);
 130
 131         // Project the center onto the edge.
 132         float32 u = b2Dot(cLocal - vertices[vertIndex1], e);
 133         b2Vec2 p;
 134         if (u <= 0.0f)
 135         {
 136                 p = vertices[vertIndex1];
 137                 manifold->points[0].id.features.incidentEdge = b2_nullFeature;
 138                 manifold->points[0].id.features.incidentVertex = (uint8)vertIndex1;
 139         }
 140         else if (u >= length)
 141         {
 142                 p = vertices[vertIndex2];
 143                 manifold->points[0].id.features.incidentEdge = b2_nullFeature;
 144                 manifold->points[0].id.features.incidentVertex = (uint8)vertIndex2;
 145         }
 146         else
 147         {
 148                 p = vertices[vertIndex1] + u * e;
 149                 manifold->points[0].id.features.incidentEdge = (uint8)normalIndex;
 150                 manifold->points[0].id.features.incidentVertex = 0;
 151         }
 152
 153         b2Vec2 d = cLocal - p;
 154         float32 dist = d.Normalize();
 155         if (dist > radius)
 156         {
 157                 return;
 158         }
 159
 160         manifold->pointCount = 1;
 161         manifold->normal = b2Mul(xf1.R, d);
 162         b2Vec2 position = c - radius * manifold->normal;
 163         manifold->points[0].localPoint1 = b2MulT(xf1, position);
 164         manifold->points[0].localPoint2 = b2MulT(xf2, position);
 165         manifold->points[0].separation = dist - radius;
 166         manifold->points[0].id.features.referenceEdge = 0;
 167         manifold->points[0].id.features.flip = 0;
 168 }