Warning, file /education/gcompris/external/qml-box2d/Box2D/Collision/b2CollidePolygon.cpp was not indexed or was modified since last indexation (in which case cross-reference links may be missing, inaccurate or erroneous).
0001 /* 0002 * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org 0003 * 0004 * This software is provided 'as-is', without any express or implied 0005 * warranty. In no event will the authors be held liable for any damages 0006 * arising from the use of this software. 0007 * Permission is granted to anyone to use this software for any purpose, 0008 * including commercial applications, and to alter it and redistribute it 0009 * freely, subject to the following restrictions: 0010 * 1. The origin of this software must not be misrepresented; you must not 0011 * claim that you wrote the original software. If you use this software 0012 * in a product, an acknowledgment in the product documentation would be 0013 * appreciated but is not required. 0014 * 2. Altered source versions must be plainly marked as such, and must not be 0015 * misrepresented as being the original software. 0016 * 3. This notice may not be removed or altered from any source distribution. 0017 */ 0018 0019 #include <Box2D/Collision/b2Collision.h> 0020 #include <Box2D/Collision/Shapes/b2PolygonShape.h> 0021 0022 // Find the max separation between poly1 and poly2 using edge normals from poly1. 0023 static float32 b2FindMaxSeparation(int32* edgeIndex, 0024 const b2PolygonShape* poly1, const b2Transform& xf1, 0025 const b2PolygonShape* poly2, const b2Transform& xf2) 0026 { 0027 int32 count1 = poly1->m_count; 0028 int32 count2 = poly2->m_count; 0029 const b2Vec2* n1s = poly1->m_normals; 0030 const b2Vec2* v1s = poly1->m_vertices; 0031 const b2Vec2* v2s = poly2->m_vertices; 0032 b2Transform xf = b2MulT(xf2, xf1); 0033 0034 int32 bestIndex = 0; 0035 float32 maxSeparation = -b2_maxFloat; 0036 for (int32 i = 0; i < count1; ++i) 0037 { 0038 // Get poly1 normal in frame2. 0039 b2Vec2 n = b2Mul(xf.q, n1s[i]); 0040 b2Vec2 v1 = b2Mul(xf, v1s[i]); 0041 0042 // Find deepest point for normal i. 0043 float32 si = b2_maxFloat; 0044 for (int32 j = 0; j < count2; ++j) 0045 { 0046 float32 sij = b2Dot(n, v2s[j] - v1); 0047 if (sij < si) 0048 { 0049 si = sij; 0050 } 0051 } 0052 0053 if (si > maxSeparation) 0054 { 0055 maxSeparation = si; 0056 bestIndex = i; 0057 } 0058 } 0059 0060 *edgeIndex = bestIndex; 0061 return maxSeparation; 0062 } 0063 0064 static void b2FindIncidentEdge(b2ClipVertex c[2], 0065 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1, 0066 const b2PolygonShape* poly2, const b2Transform& xf2) 0067 { 0068 const b2Vec2* normals1 = poly1->m_normals; 0069 0070 int32 count2 = poly2->m_count; 0071 const b2Vec2* vertices2 = poly2->m_vertices; 0072 const b2Vec2* normals2 = poly2->m_normals; 0073 0074 b2Assert(0 <= edge1 && edge1 < poly1->m_count); 0075 0076 // Get the normal of the reference edge in poly2's frame. 0077 b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])); 0078 0079 // Find the incident edge on poly2. 0080 int32 index = 0; 0081 float32 minDot = b2_maxFloat; 0082 for (int32 i = 0; i < count2; ++i) 0083 { 0084 float32 dot = b2Dot(normal1, normals2[i]); 0085 if (dot < minDot) 0086 { 0087 minDot = dot; 0088 index = i; 0089 } 0090 } 0091 0092 // Build the clip vertices for the incident edge. 0093 int32 i1 = index; 0094 int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; 0095 0096 c[0].v = b2Mul(xf2, vertices2[i1]); 0097 c[0].id.cf.indexA = (uint8)edge1; 0098 c[0].id.cf.indexB = (uint8)i1; 0099 c[0].id.cf.typeA = b2ContactFeature::e_face; 0100 c[0].id.cf.typeB = b2ContactFeature::e_vertex; 0101 0102 c[1].v = b2Mul(xf2, vertices2[i2]); 0103 c[1].id.cf.indexA = (uint8)edge1; 0104 c[1].id.cf.indexB = (uint8)i2; 0105 c[1].id.cf.typeA = b2ContactFeature::e_face; 0106 c[1].id.cf.typeB = b2ContactFeature::e_vertex; 0107 } 0108 0109 // Find edge normal of max separation on A - return if separating axis is found 0110 // Find edge normal of max separation on B - return if separation axis is found 0111 // Choose reference edge as min(minA, minB) 0112 // Find incident edge 0113 // Clip 0114 0115 // The normal points from 1 to 2 0116 void b2CollidePolygons(b2Manifold* manifold, 0117 const b2PolygonShape* polyA, const b2Transform& xfA, 0118 const b2PolygonShape* polyB, const b2Transform& xfB) 0119 { 0120 manifold->pointCount = 0; 0121 float32 totalRadius = polyA->m_radius + polyB->m_radius; 0122 0123 int32 edgeA = 0; 0124 float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); 0125 if (separationA > totalRadius) 0126 return; 0127 0128 int32 edgeB = 0; 0129 float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); 0130 if (separationB > totalRadius) 0131 return; 0132 0133 const b2PolygonShape* poly1; // reference polygon 0134 const b2PolygonShape* poly2; // incident polygon 0135 b2Transform xf1, xf2; 0136 int32 edge1; // reference edge 0137 uint8 flip; 0138 const float32 k_tol = 0.1f * b2_linearSlop; 0139 0140 if (separationB > separationA + k_tol) 0141 { 0142 poly1 = polyB; 0143 poly2 = polyA; 0144 xf1 = xfB; 0145 xf2 = xfA; 0146 edge1 = edgeB; 0147 manifold->type = b2Manifold::e_faceB; 0148 flip = 1; 0149 } 0150 else 0151 { 0152 poly1 = polyA; 0153 poly2 = polyB; 0154 xf1 = xfA; 0155 xf2 = xfB; 0156 edge1 = edgeA; 0157 manifold->type = b2Manifold::e_faceA; 0158 flip = 0; 0159 } 0160 0161 b2ClipVertex incidentEdge[2]; 0162 b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); 0163 0164 int32 count1 = poly1->m_count; 0165 const b2Vec2* vertices1 = poly1->m_vertices; 0166 0167 int32 iv1 = edge1; 0168 int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; 0169 0170 b2Vec2 v11 = vertices1[iv1]; 0171 b2Vec2 v12 = vertices1[iv2]; 0172 0173 b2Vec2 localTangent = v12 - v11; 0174 localTangent.Normalize(); 0175 0176 b2Vec2 localNormal = b2Cross(localTangent, 1.0f); 0177 b2Vec2 planePoint = 0.5f * (v11 + v12); 0178 0179 b2Vec2 tangent = b2Mul(xf1.q, localTangent); 0180 b2Vec2 normal = b2Cross(tangent, 1.0f); 0181 0182 v11 = b2Mul(xf1, v11); 0183 v12 = b2Mul(xf1, v12); 0184 0185 // Face offset. 0186 float32 frontOffset = b2Dot(normal, v11); 0187 0188 // Side offsets, extended by polytope skin thickness. 0189 float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius; 0190 float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius; 0191 0192 // Clip incident edge against extruded edge1 side edges. 0193 b2ClipVertex clipPoints1[2]; 0194 b2ClipVertex clipPoints2[2]; 0195 int np; 0196 0197 // Clip to box side 1 0198 np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); 0199 0200 if (np < 2) 0201 return; 0202 0203 // Clip to negative box side 1 0204 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); 0205 0206 if (np < 2) 0207 { 0208 return; 0209 } 0210 0211 // Now clipPoints2 contains the clipped points. 0212 manifold->localNormal = localNormal; 0213 manifold->localPoint = planePoint; 0214 0215 int32 pointCount = 0; 0216 for (int32 i = 0; i < b2_maxManifoldPoints; ++i) 0217 { 0218 float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; 0219 0220 if (separation <= totalRadius) 0221 { 0222 b2ManifoldPoint* cp = manifold->points + pointCount; 0223 cp->localPoint = b2MulT(xf2, clipPoints2[i].v); 0224 cp->id = clipPoints2[i].id; 0225 if (flip) 0226 { 0227 // Swap features 0228 b2ContactFeature cf = cp->id.cf; 0229 cp->id.cf.indexA = cf.indexB; 0230 cp->id.cf.indexB = cf.indexA; 0231 cp->id.cf.typeA = cf.typeB; 0232 cp->id.cf.typeB = cf.typeA; 0233 } 0234 ++pointCount; 0235 } 0236 } 0237 0238 manifold->pointCount = pointCount; 0239 }