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
23void 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
51void 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