b2CollidePolygon.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2006-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/b2PolygonShape.h>
00021 
00022 // Find the max separation between poly1 and poly2 using edge normals from poly1.
00023 static float32 b2FindMaxSeparation(int32* edgeIndex,
00024                                                                  const b2PolygonShape* poly1, const b2Transform& xf1,
00025                                                                  const b2PolygonShape* poly2, const b2Transform& xf2)
00026 {
00027         int32 count1 = poly1->m_count;
00028         int32 count2 = poly2->m_count;
00029         const b2Vec2* n1s = poly1->m_normals;
00030         const b2Vec2* v1s = poly1->m_vertices;
00031         const b2Vec2* v2s = poly2->m_vertices;
00032         b2Transform xf = b2MulT(xf2, xf1);
00033 
00034         int32 bestIndex = 0;
00035         float32 maxSeparation = -b2_maxFloat;
00036         for (int32 i = 0; i < count1; ++i)
00037         {
00038                 // Get poly1 normal in frame2.
00039                 b2Vec2 n = b2Mul(xf.q, n1s[i]);
00040                 b2Vec2 v1 = b2Mul(xf, v1s[i]);
00041 
00042                 // Find deepest point for normal i.
00043                 float32 si = b2_maxFloat;
00044                 for (int32 j = 0; j < count2; ++j)
00045                 {
00046                         float32 sij = b2Dot(n, v2s[j] - v1);
00047                         if (sij < si)
00048                         {
00049                                 si = sij;
00050                         }
00051                 }
00052 
00053                 if (si > maxSeparation)
00054                 {
00055                         maxSeparation = si;
00056                         bestIndex = i;
00057                 }
00058         }
00059 
00060         *edgeIndex = bestIndex;
00061         return maxSeparation;
00062 }
00063 
00064 static void b2FindIncidentEdge(b2ClipVertex c[2],
00065                                                          const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
00066                                                          const b2PolygonShape* poly2, const b2Transform& xf2)
00067 {
00068         const b2Vec2* normals1 = poly1->m_normals;
00069 
00070         int32 count2 = poly2->m_count;
00071         const b2Vec2* vertices2 = poly2->m_vertices;
00072         const b2Vec2* normals2 = poly2->m_normals;
00073 
00074         b2Assert(0 <= edge1 && edge1 < poly1->m_count);
00075 
00076         // Get the normal of the reference edge in poly2's frame.
00077         b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));
00078 
00079         // Find the incident edge on poly2.
00080         int32 index = 0;
00081         float32 minDot = b2_maxFloat;
00082         for (int32 i = 0; i < count2; ++i)
00083         {
00084                 float32 dot = b2Dot(normal1, normals2[i]);
00085                 if (dot < minDot)
00086                 {
00087                         minDot = dot;
00088                         index = i;
00089                 }
00090         }
00091 
00092         // Build the clip vertices for the incident edge.
00093         int32 i1 = index;
00094         int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
00095 
00096         c[0].v = b2Mul(xf2, vertices2[i1]);
00097         c[0].id.cf.indexA = (uint8)edge1;
00098         c[0].id.cf.indexB = (uint8)i1;
00099         c[0].id.cf.typeA = b2ContactFeature::e_face;
00100         c[0].id.cf.typeB = b2ContactFeature::e_vertex;
00101 
00102         c[1].v = b2Mul(xf2, vertices2[i2]);
00103         c[1].id.cf.indexA = (uint8)edge1;
00104         c[1].id.cf.indexB = (uint8)i2;
00105         c[1].id.cf.typeA = b2ContactFeature::e_face;
00106         c[1].id.cf.typeB = b2ContactFeature::e_vertex;
00107 }
00108 
00109 // Find edge normal of max separation on A - return if separating axis is found
00110 // Find edge normal of max separation on B - return if separation axis is found
00111 // Choose reference edge as min(minA, minB)
00112 // Find incident edge
00113 // Clip
00114 
00115 // The normal points from 1 to 2
00116 void b2CollidePolygons(b2Manifold* manifold,
00117                                           const b2PolygonShape* polyA, const b2Transform& xfA,
00118                                           const b2PolygonShape* polyB, const b2Transform& xfB)
00119 {
00120         manifold->pointCount = 0;
00121         float32 totalRadius = polyA->m_radius + polyB->m_radius;
00122 
00123         int32 edgeA = 0;
00124         float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
00125         if (separationA > totalRadius)
00126                 return;
00127 
00128         int32 edgeB = 0;
00129         float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
00130         if (separationB > totalRadius)
00131                 return;
00132 
00133         const b2PolygonShape* poly1;    // reference polygon
00134         const b2PolygonShape* poly2;    // incident polygon
00135         b2Transform xf1, xf2;
00136         int32 edge1;                                    // reference edge
00137         uint8 flip;
00138         const float32 k_tol = 0.1f * b2_linearSlop;
00139 
00140         if (separationB > separationA + k_tol)
00141         {
00142                 poly1 = polyB;
00143                 poly2 = polyA;
00144                 xf1 = xfB;
00145                 xf2 = xfA;
00146                 edge1 = edgeB;
00147                 manifold->type = b2Manifold::e_faceB;
00148                 flip = 1;
00149         }
00150         else
00151         {
00152                 poly1 = polyA;
00153                 poly2 = polyB;
00154                 xf1 = xfA;
00155                 xf2 = xfB;
00156                 edge1 = edgeA;
00157                 manifold->type = b2Manifold::e_faceA;
00158                 flip = 0;
00159         }
00160 
00161         b2ClipVertex incidentEdge[2];
00162         b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
00163 
00164         int32 count1 = poly1->m_count;
00165         const b2Vec2* vertices1 = poly1->m_vertices;
00166 
00167         int32 iv1 = edge1;
00168         int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
00169 
00170         b2Vec2 v11 = vertices1[iv1];
00171         b2Vec2 v12 = vertices1[iv2];
00172 
00173         b2Vec2 localTangent = v12 - v11;
00174         localTangent.Normalize();
00175         
00176         b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
00177         b2Vec2 planePoint = 0.5f * (v11 + v12);
00178 
00179         b2Vec2 tangent = b2Mul(xf1.q, localTangent);
00180         b2Vec2 normal = b2Cross(tangent, 1.0f);
00181         
00182         v11 = b2Mul(xf1, v11);
00183         v12 = b2Mul(xf1, v12);
00184 
00185         // Face offset.
00186         float32 frontOffset = b2Dot(normal, v11);
00187 
00188         // Side offsets, extended by polytope skin thickness.
00189         float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
00190         float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
00191 
00192         // Clip incident edge against extruded edge1 side edges.
00193         b2ClipVertex clipPoints1[2];
00194         b2ClipVertex clipPoints2[2];
00195         int np;
00196 
00197         // Clip to box side 1
00198         np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
00199 
00200         if (np < 2)
00201                 return;
00202 
00203         // Clip to negative box side 1
00204         np = b2ClipSegmentToLine(clipPoints2, clipPoints1,  tangent, sideOffset2, iv2);
00205 
00206         if (np < 2)
00207         {
00208                 return;
00209         }
00210 
00211         // Now clipPoints2 contains the clipped points.
00212         manifold->localNormal = localNormal;
00213         manifold->localPoint = planePoint;
00214 
00215         int32 pointCount = 0;
00216         for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
00217         {
00218                 float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
00219 
00220                 if (separation <= totalRadius)
00221                 {
00222                         b2ManifoldPoint* cp = manifold->points + pointCount;
00223                         cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
00224                         cp->id = clipPoints2[i].id;
00225                         if (flip)
00226                         {
00227                                 // Swap features
00228                                 b2ContactFeature cf = cp->id.cf;
00229                                 cp->id.cf.indexA = cf.indexB;
00230                                 cp->id.cf.indexB = cf.indexA;
00231                                 cp->id.cf.typeA = cf.typeB;
00232                                 cp->id.cf.typeB = cf.typeA;
00233                         }
00234                         ++pointCount;
00235                 }
00236         }
00237 
00238         manifold->pointCount = pointCount;
00239 }


mvsim
Author(s):
autogenerated on Thu Sep 7 2017 09:27:47