Home | History | Annotate | Download | only in Collision 
      1 /*
      2 * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
      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 <Box2D/Collision/b2Collision.h>
     20 #include <Box2D/Collision/Shapes/b2CircleShape.h>
     21 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
     22 
     23 void b2CollideCircles(
     24 	b2Manifold* manifold,
     25 	const b2CircleShape* circleA, const b2Transform& xfA,
     26 	const b2CircleShape* circleB, const b2Transform& xfB)
     27 {
     28 	manifold->pointCount = 0;
     29 
     30 	b2Vec2 pA = b2Mul(xfA, circleA->m_p);
     31 	b2Vec2 pB = b2Mul(xfB, circleB->m_p);
     32 
     33 	b2Vec2 d = pB - pA;
     34 	float32 distSqr = b2Dot(d, d);
     35 	float32 rA = circleA->m_radius, rB = circleB->m_radius;
     36 	float32 radius = rA + rB;
     37 	if (distSqr > radius * radius)
     38 	{
     39 		return;
     40 	}
     41 
     42 	manifold->type = b2Manifold::e_circles;
     43 	manifold->localPoint = circleA->m_p;
     44 	manifold->localNormal.SetZero();
     45 	manifold->pointCount = 1;
     46 
     47 	manifold->points[0].localPoint = circleB->m_p;
     48 	manifold->points[0].id.key = 0;
     49 }
     50 
     51 void b2CollidePolygonAndCircle(
     52 	b2Manifold* manifold,
     53 	const b2PolygonShape* polygonA, const b2Transform& xfA,
     54 	const b2CircleShape* circleB, const b2Transform& xfB)
     55 {
     56 	manifold->pointCount = 0;
     57 
     58 	// Compute circle position in the frame of the polygon.
     59 	b2Vec2 c = b2Mul(xfB, circleB->m_p);
     60 	b2Vec2 cLocal = b2MulT(xfA, c);
     61 
     62 	// Find the min separating edge.
     63 	int32 normalIndex = 0;
     64 	float32 separation = -b2_maxFloat;
     65 	float32 radius = polygonA->m_radius + circleB->m_radius;
     66 	int32 vertexCount = polygonA->m_count;
     67 	const b2Vec2* vertices = polygonA->m_vertices;
     68 	const b2Vec2* normals = polygonA->m_normals;
     69 
     70 	for (int32 i = 0; i < vertexCount; ++i)
     71 	{
     72 		float32 s = b2Dot(normals[i], cLocal - vertices[i]);
     73 
     74 		if (s > radius)
     75 		{
     76 			// Early out.
     77 			return;
     78 		}
     79 
     80 		if (s > separation)
     81 		{
     82 			separation = s;
     83 			normalIndex = i;
     84 		}
     85 	}
     86 
     87 	// Vertices that subtend the incident face.
     88 	int32 vertIndex1 = normalIndex;
     89 	int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
     90 	b2Vec2 v1 = vertices[vertIndex1];
     91 	b2Vec2 v2 = vertices[vertIndex2];
     92 
     93 	// If the center is inside the polygon ...
     94 	if (separation < b2_epsilon)
     95 	{
     96 		manifold->pointCount = 1;
     97 		manifold->type = b2Manifold::e_faceA;
     98 		manifold->localNormal = normals[normalIndex];
     99 		manifold->localPoint = 0.5f * (v1 + v2);
    100 		manifold->points[0].localPoint = circleB->m_p;
    101 		manifold->points[0].id.key = 0;
    102 		return;
    103 	}
    104 
    105 	// Compute barycentric coordinates
    106 	float32 u1 = b2Dot(cLocal - v1, v2 - v1);
    107 	float32 u2 = b2Dot(cLocal - v2, v1 - v2);
    108 	if (u1 <= 0.0f)
    109 	{
    110 		if (b2DistanceSquared(cLocal, v1) > radius * radius)
    111 		{
    112 			return;
    113 		}
    114 
    115 		manifold->pointCount = 1;
    116 		manifold->type = b2Manifold::e_faceA;
    117 		manifold->localNormal = cLocal - v1;
    118 		manifold->localNormal.Normalize();
    119 		manifold->localPoint = v1;
    120 		manifold->points[0].localPoint = circleB->m_p;
    121 		manifold->points[0].id.key = 0;
    122 	}
    123 	else if (u2 <= 0.0f)
    124 	{
    125 		if (b2DistanceSquared(cLocal, v2) > radius * radius)
    126 		{
    127 			return;
    128 		}
    129 
    130 		manifold->pointCount = 1;
    131 		manifold->type = b2Manifold::e_faceA;
    132 		manifold->localNormal = cLocal - v2;
    133 		manifold->localNormal.Normalize();
    134 		manifold->localPoint = v2;
    135 		manifold->points[0].localPoint = circleB->m_p;
    136 		manifold->points[0].id.key = 0;
    137 	}
    138 	else
    139 	{
    140 		b2Vec2 faceCenter = 0.5f * (v1 + v2);
    141 		float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
    142 		if (separation > radius)
    143 		{
    144 			return;
    145 		}
    146 
    147 		manifold->pointCount = 1;
    148 		manifold->type = b2Manifold::e_faceA;
    149 		manifold->localNormal = normals[vertIndex1];
    150 		manifold->localPoint = faceCenter;
    151 		manifold->points[0].localPoint = circleB->m_p;
    152 		manifold->points[0].id.key = 0;
    153 	}
    154 }
    155