diff options
Diffstat (limited to 'Client/ThirdParty/Box2D/src/collision/b2_collide_circle.cpp')
| -rw-r--r-- | Client/ThirdParty/Box2D/src/collision/b2_collide_circle.cpp | 158 |
1 files changed, 158 insertions, 0 deletions
diff --git a/Client/ThirdParty/Box2D/src/collision/b2_collide_circle.cpp b/Client/ThirdParty/Box2D/src/collision/b2_collide_circle.cpp new file mode 100644 index 0000000..469da55 --- /dev/null +++ b/Client/ThirdParty/Box2D/src/collision/b2_collide_circle.cpp @@ -0,0 +1,158 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_collision.h" +#include "box2d/b2_circle_shape.h" +#include "box2d/b2_polygon_shape.h" + +void b2CollideCircles( + b2Manifold* manifold, + const b2CircleShape* circleA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + + b2Vec2 pA = b2Mul(xfA, circleA->m_p); + b2Vec2 pB = b2Mul(xfB, circleB->m_p); + + b2Vec2 d = pB - pA; + float distSqr = b2Dot(d, d); + float rA = circleA->m_radius, rB = circleB->m_radius; + float radius = rA + rB; + if (distSqr > radius * radius) + { + return; + } + + manifold->type = b2Manifold::e_circles; + manifold->localPoint = circleA->m_p; + manifold->localNormal.SetZero(); + manifold->pointCount = 1; + + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; +} + +void b2CollidePolygonAndCircle( + b2Manifold* manifold, + const b2PolygonShape* polygonA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + + // Compute circle position in the frame of the polygon. + b2Vec2 c = b2Mul(xfB, circleB->m_p); + b2Vec2 cLocal = b2MulT(xfA, c); + + // Find the min separating edge. + int32 normalIndex = 0; + float separation = -b2_maxFloat; + float radius = polygonA->m_radius + circleB->m_radius; + int32 vertexCount = polygonA->m_count; + const b2Vec2* vertices = polygonA->m_vertices; + const b2Vec2* normals = polygonA->m_normals; + + for (int32 i = 0; i < vertexCount; ++i) + { + float s = b2Dot(normals[i], cLocal - vertices[i]); + + if (s > radius) + { + // Early out. + return; + } + + if (s > separation) + { + separation = s; + normalIndex = i; + } + } + + // Vertices that subtend the incident face. + int32 vertIndex1 = normalIndex; + int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; + b2Vec2 v1 = vertices[vertIndex1]; + b2Vec2 v2 = vertices[vertIndex2]; + + // If the center is inside the polygon ... + if (separation < b2_epsilon) + { + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = normals[normalIndex]; + manifold->localPoint = 0.5f * (v1 + v2); + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + return; + } + + // Compute barycentric coordinates + float u1 = b2Dot(cLocal - v1, v2 - v1); + float u2 = b2Dot(cLocal - v2, v1 - v2); + if (u1 <= 0.0f) + { + if (b2DistanceSquared(cLocal, v1) > radius * radius) + { + return; + } + + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = cLocal - v1; + manifold->localNormal.Normalize(); + manifold->localPoint = v1; + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + } + else if (u2 <= 0.0f) + { + if (b2DistanceSquared(cLocal, v2) > radius * radius) + { + return; + } + + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = cLocal - v2; + manifold->localNormal.Normalize(); + manifold->localPoint = v2; + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + } + else + { + b2Vec2 faceCenter = 0.5f * (v1 + v2); + float s = b2Dot(cLocal - faceCenter, normals[vertIndex1]); + if (s > radius) + { + return; + } + + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = normals[vertIndex1]; + manifold->localPoint = faceCenter; + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + } +} |