b2CollidePolygon.cpp
Go to the documentation of this file.
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 
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 
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;
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;
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
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 }
float32 b2Dot(const b2Vec2 &a, const b2Vec2 &b)
Perform the dot product on two vectors.
Definition: b2Math.h:406
uint8 indexB
Feature index on shapeB.
Definition: b2Collision.h:47
uint8 typeA
The feature type on shapeA.
Definition: b2Collision.h:48
void b2CollidePolygons(b2Manifold *manifold, const b2PolygonShape *polyA, const b2Transform &xfA, const b2PolygonShape *polyB, const b2Transform &xfB)
Compute the collision manifold between two polygons.
b2Vec2 b2Mul(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:433
#define b2_linearSlop
Definition: b2Settings.h:68
Used for computing contact manifolds.
Definition: b2Collision.h:140
b2Vec2 localNormal
not use for Type::e_points
Definition: b2Collision.h:103
b2Rot q
Definition: b2Math.h:373
f
static void b2FindIncidentEdge(b2ClipVertex c[2], const b2PolygonShape *poly1, const b2Transform &xf1, int32 edge1, const b2PolygonShape *poly2, const b2Transform &xf2)
b2ContactID id
uniquely identifies a contact point between two shapes
Definition: b2Collision.h:74
uint8 typeB
The feature type on shapeB.
Definition: b2Collision.h:49
b2ContactFeature cf
Definition: b2Collision.h:55
#define b2_maxManifoldPoints
Definition: b2Settings.h:50
float32 m_radius
Definition: b2Shape.h:93
A 2D column vector.
Definition: b2Math.h:53
b2ContactID id
Definition: b2Collision.h:143
signed int int32
Definition: b2Settings.h:31
float32 b2Cross(const b2Vec2 &a, const b2Vec2 &b)
Perform the cross product on two vectors. In 2D this produces a scalar.
Definition: b2Math.h:412
b2Vec2 m_vertices[b2_maxPolygonVertices]
int32 pointCount
the number of manifold points
Definition: b2Collision.h:106
b2Vec2 localPoint
usage depends on manifold type
Definition: b2Collision.h:71
uint8 indexA
Feature index on shapeA.
Definition: b2Collision.h:46
b2Vec2 b2MulT(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:440
unsigned char uint8
Definition: b2Settings.h:32
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
b2Vec2 localPoint
usage depends on manifold type
Definition: b2Collision.h:104
b2ManifoldPoint points[b2_maxManifoldPoints]
the points of contact
Definition: b2Collision.h:102
#define b2Assert(A)
Definition: b2Settings.h:27
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], const b2Vec2 &normal, float32 offset, int32 vertexIndexA)
Clipping for contact manifolds.
#define b2_maxFloat
Definition: b2Settings.h:38
static float32 b2FindMaxSeparation(int32 *edgeIndex, const b2PolygonShape *poly1, const b2Transform &xf1, const b2PolygonShape *poly2, const b2Transform &xf2)
float32 Normalize()
Convert this vector into a unit vector. Returns the length.
Definition: b2Math.h:114
b2Vec2 m_normals[b2_maxPolygonVertices]
float float32
Definition: b2Settings.h:35


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 19:36:39