1 /* 2 * Copyright (c) 2006-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/b2PolygonShape.h> 21 22 // Find the max separation between poly1 and poly2 using edge normals from poly1. 23 static float32 b2FindMaxSeparation(int32* edgeIndex, 24 const b2PolygonShape* poly1, const b2Transform& xf1, 25 const b2PolygonShape* poly2, const b2Transform& xf2) 26 { 27 int32 count1 = poly1->m_count; 28 int32 count2 = poly2->m_count; 29 const b2Vec2* n1s = poly1->m_normals; 30 const b2Vec2* v1s = poly1->m_vertices; 31 const b2Vec2* v2s = poly2->m_vertices; 32 b2Transform xf = b2MulT(xf2, xf1); 33 34 int32 bestIndex = 0; 35 float32 maxSeparation = -b2_maxFloat; 36 for (int32 i = 0; i < count1; ++i) 37 { 38 // Get poly1 normal in frame2. 39 b2Vec2 n = b2Mul(xf.q, n1s[i]); 40 b2Vec2 v1 = b2Mul(xf, v1s[i]); 41 42 // Find deepest point for normal i. 43 float32 si = b2_maxFloat; 44 for (int32 j = 0; j < count2; ++j) 45 { 46 float32 sij = b2Dot(n, v2s[j] - v1); 47 if (sij < si) 48 { 49 si = sij; 50 } 51 } 52 53 if (si > maxSeparation) 54 { 55 maxSeparation = si; 56 bestIndex = i; 57 } 58 } 59 60 *edgeIndex = bestIndex; 61 return maxSeparation; 62 } 63 64 static void b2FindIncidentEdge(b2ClipVertex c[2], 65 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1, 66 const b2PolygonShape* poly2, const b2Transform& xf2) 67 { 68 const b2Vec2* normals1 = poly1->m_normals; 69 70 int32 count2 = poly2->m_count; 71 const b2Vec2* vertices2 = poly2->m_vertices; 72 const b2Vec2* normals2 = poly2->m_normals; 73 74 b2Assert(0 <= edge1 && edge1 < poly1->m_count); 75 76 // Get the normal of the reference edge in poly2's frame. 77 b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])); 78 79 // Find the incident edge on poly2. 80 int32 index = 0; 81 float32 minDot = b2_maxFloat; 82 for (int32 i = 0; i < count2; ++i) 83 { 84 float32 dot = b2Dot(normal1, normals2[i]); 85 if (dot < minDot) 86 { 87 minDot = dot; 88 index = i; 89 } 90 } 91 92 // Build the clip vertices for the incident edge. 93 int32 i1 = index; 94 int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; 95 96 c[0].v = b2Mul(xf2, vertices2[i1]); 97 c[0].id.cf.indexA = (uint8)edge1; 98 c[0].id.cf.indexB = (uint8)i1; 99 c[0].id.cf.typeA = b2ContactFeature::e_face; 100 c[0].id.cf.typeB = b2ContactFeature::e_vertex; 101 102 c[1].v = b2Mul(xf2, vertices2[i2]); 103 c[1].id.cf.indexA = (uint8)edge1; 104 c[1].id.cf.indexB = (uint8)i2; 105 c[1].id.cf.typeA = b2ContactFeature::e_face; 106 c[1].id.cf.typeB = b2ContactFeature::e_vertex; 107 } 108 109 // Find edge normal of max separation on A - return if separating axis is found 110 // Find edge normal of max separation on B - return if separation axis is found 111 // Choose reference edge as min(minA, minB) 112 // Find incident edge 113 // Clip 114 115 // The normal points from 1 to 2 116 void b2CollidePolygons(b2Manifold* manifold, 117 const b2PolygonShape* polyA, const b2Transform& xfA, 118 const b2PolygonShape* polyB, const b2Transform& xfB) 119 { 120 manifold->pointCount = 0; 121 float32 totalRadius = polyA->m_radius + polyB->m_radius; 122 123 int32 edgeA = 0; 124 float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); 125 if (separationA > totalRadius) 126 return; 127 128 int32 edgeB = 0; 129 float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); 130 if (separationB > totalRadius) 131 return; 132 133 const b2PolygonShape* poly1; // reference polygon 134 const b2PolygonShape* poly2; // incident polygon 135 b2Transform xf1, xf2; 136 int32 edge1; // reference edge 137 uint8 flip; 138 const float32 k_tol = 0.1f * b2_linearSlop; 139 140 if (separationB > separationA + k_tol) 141 { 142 poly1 = polyB; 143 poly2 = polyA; 144 xf1 = xfB; 145 xf2 = xfA; 146 edge1 = edgeB; 147 manifold->type = b2Manifold::e_faceB; 148 flip = 1; 149 } 150 else 151 { 152 poly1 = polyA; 153 poly2 = polyB; 154 xf1 = xfA; 155 xf2 = xfB; 156 edge1 = edgeA; 157 manifold->type = b2Manifold::e_faceA; 158 flip = 0; 159 } 160 161 b2ClipVertex incidentEdge[2]; 162 b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); 163 164 int32 count1 = poly1->m_count; 165 const b2Vec2* vertices1 = poly1->m_vertices; 166 167 int32 iv1 = edge1; 168 int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; 169 170 b2Vec2 v11 = vertices1[iv1]; 171 b2Vec2 v12 = vertices1[iv2]; 172 173 b2Vec2 localTangent = v12 - v11; 174 localTangent.Normalize(); 175 176 b2Vec2 localNormal = b2Cross(localTangent, 1.0f); 177 b2Vec2 planePoint = 0.5f * (v11 + v12); 178 179 b2Vec2 tangent = b2Mul(xf1.q, localTangent); 180 b2Vec2 normal = b2Cross(tangent, 1.0f); 181 182 v11 = b2Mul(xf1, v11); 183 v12 = b2Mul(xf1, v12); 184 185 // Face offset. 186 float32 frontOffset = b2Dot(normal, v11); 187 188 // Side offsets, extended by polytope skin thickness. 189 float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius; 190 float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius; 191 192 // Clip incident edge against extruded edge1 side edges. 193 b2ClipVertex clipPoints1[2]; 194 b2ClipVertex clipPoints2[2]; 195 int np; 196 197 // Clip to box side 1 198 np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); 199 200 if (np < 2) 201 return; 202 203 // Clip to negative box side 1 204 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); 205 206 if (np < 2) 207 { 208 return; 209 } 210 211 // Now clipPoints2 contains the clipped points. 212 manifold->localNormal = localNormal; 213 manifold->localPoint = planePoint; 214 215 int32 pointCount = 0; 216 for (int32 i = 0; i < b2_maxManifoldPoints; ++i) 217 { 218 float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; 219 220 if (separation <= totalRadius) 221 { 222 b2ManifoldPoint* cp = manifold->points + pointCount; 223 cp->localPoint = b2MulT(xf2, clipPoints2[i].v); 224 cp->id = clipPoints2[i].id; 225 if (flip) 226 { 227 // Swap features 228 b2ContactFeature cf = cp->id.cf; 229 cp->id.cf.indexA = cf.indexB; 230 cp->id.cf.indexB = cf.indexA; 231 cp->id.cf.typeA = cf.typeB; 232 cp->id.cf.typeB = cf.typeA; 233 } 234 ++pointCount; 235 } 236 } 237 238 manifold->pointCount = pointCount; 239 } 240
