Updated to Box2D 2.3.0
[qml-box2d:qml-box2d-folibis.git] / Box2D / Collision / b2CollideCircle.cpp
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 }