b2CollideCircle.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
00003 *
00004 * This software is provided 'as-is', without any express or implied
00005 * warranty.  In no event will the authors be held liable for any damages
00006 * arising from the use of this software.
00007 * Permission is granted to anyone to use this software for any purpose,
00008 * including commercial applications, and to alter it and redistribute it
00009 * freely, subject to the following restrictions:
00010 * 1. The origin of this software must not be misrepresented; you must not
00011 * claim that you wrote the original software. If you use this software
00012 * in a product, an acknowledgment in the product documentation would be
00013 * appreciated but is not required.
00014 * 2. Altered source versions must be plainly marked as such, and must not be
00015 * misrepresented as being the original software.
00016 * 3. This notice may not be removed or altered from any source distribution.
00017 */
00018 
00019 #include <Box2D/Collision/b2Collision.h>
00020 #include <Box2D/Collision/Shapes/b2CircleShape.h>
00021 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
00022 
00023 void b2CollideCircles(
00024         b2Manifold* manifold,
00025         const b2CircleShape* circleA, const b2Transform& xfA,
00026         const b2CircleShape* circleB, const b2Transform& xfB)
00027 {
00028         manifold->pointCount = 0;
00029 
00030         b2Vec2 pA = b2Mul(xfA, circleA->m_p);
00031         b2Vec2 pB = b2Mul(xfB, circleB->m_p);
00032 
00033         b2Vec2 d = pB - pA;
00034         float32 distSqr = b2Dot(d, d);
00035         float32 rA = circleA->m_radius, rB = circleB->m_radius;
00036         float32 radius = rA + rB;
00037         if (distSqr > radius * radius)
00038         {
00039                 return;
00040         }
00041 
00042         manifold->type = b2Manifold::e_circles;
00043         manifold->localPoint = circleA->m_p;
00044         manifold->localNormal.SetZero();
00045         manifold->pointCount = 1;
00046 
00047         manifold->points[0].localPoint = circleB->m_p;
00048         manifold->points[0].id.key = 0;
00049 }
00050 
00051 void b2CollidePolygonAndCircle(
00052         b2Manifold* manifold,
00053         const b2PolygonShape* polygonA, const b2Transform& xfA,
00054         const b2CircleShape* circleB, const b2Transform& xfB)
00055 {
00056         manifold->pointCount = 0;
00057 
00058         // Compute circle position in the frame of the polygon.
00059         b2Vec2 c = b2Mul(xfB, circleB->m_p);
00060         b2Vec2 cLocal = b2MulT(xfA, c);
00061 
00062         // Find the min separating edge.
00063         int32 normalIndex = 0;
00064         float32 separation = -b2_maxFloat;
00065         float32 radius = polygonA->m_radius + circleB->m_radius;
00066         int32 vertexCount = polygonA->m_count;
00067         const b2Vec2* vertices = polygonA->m_vertices;
00068         const b2Vec2* normals = polygonA->m_normals;
00069 
00070         for (int32 i = 0; i < vertexCount; ++i)
00071         {
00072                 float32 s = b2Dot(normals[i], cLocal - vertices[i]);
00073 
00074                 if (s > radius)
00075                 {
00076                         // Early out.
00077                         return;
00078                 }
00079 
00080                 if (s > separation)
00081                 {
00082                         separation = s;
00083                         normalIndex = i;
00084                 }
00085         }
00086 
00087         // Vertices that subtend the incident face.
00088         int32 vertIndex1 = normalIndex;
00089         int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
00090         b2Vec2 v1 = vertices[vertIndex1];
00091         b2Vec2 v2 = vertices[vertIndex2];
00092 
00093         // If the center is inside the polygon ...
00094         if (separation < b2_epsilon)
00095         {
00096                 manifold->pointCount = 1;
00097                 manifold->type = b2Manifold::e_faceA;
00098                 manifold->localNormal = normals[normalIndex];
00099                 manifold->localPoint = 0.5f * (v1 + v2);
00100                 manifold->points[0].localPoint = circleB->m_p;
00101                 manifold->points[0].id.key = 0;
00102                 return;
00103         }
00104 
00105         // Compute barycentric coordinates
00106         float32 u1 = b2Dot(cLocal - v1, v2 - v1);
00107         float32 u2 = b2Dot(cLocal - v2, v1 - v2);
00108         if (u1 <= 0.0f)
00109         {
00110                 if (b2DistanceSquared(cLocal, v1) > radius * radius)
00111                 {
00112                         return;
00113                 }
00114 
00115                 manifold->pointCount = 1;
00116                 manifold->type = b2Manifold::e_faceA;
00117                 manifold->localNormal = cLocal - v1;
00118                 manifold->localNormal.Normalize();
00119                 manifold->localPoint = v1;
00120                 manifold->points[0].localPoint = circleB->m_p;
00121                 manifold->points[0].id.key = 0;
00122         }
00123         else if (u2 <= 0.0f)
00124         {
00125                 if (b2DistanceSquared(cLocal, v2) > radius * radius)
00126                 {
00127                         return;
00128                 }
00129 
00130                 manifold->pointCount = 1;
00131                 manifold->type = b2Manifold::e_faceA;
00132                 manifold->localNormal = cLocal - v2;
00133                 manifold->localNormal.Normalize();
00134                 manifold->localPoint = v2;
00135                 manifold->points[0].localPoint = circleB->m_p;
00136                 manifold->points[0].id.key = 0;
00137         }
00138         else
00139         {
00140                 b2Vec2 faceCenter = 0.5f * (v1 + v2);
00141                 float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
00142                 if (separation > radius)
00143                 {
00144                         return;
00145                 }
00146 
00147                 manifold->pointCount = 1;
00148                 manifold->type = b2Manifold::e_faceA;
00149                 manifold->localNormal = normals[vertIndex1];
00150                 manifold->localPoint = faceCenter;
00151                 manifold->points[0].localPoint = circleB->m_p;
00152                 manifold->points[0].id.key = 0;
00153         }
00154 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:34