2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
4 * Permission to use, copy, modify, distribute and sell this software
5 * and its documentation for any purpose is hereby granted without fee,
6 * provided that the above copyright notice appear in all copies.
7 * Erin Catto makes no representations about the suitability
8 * of this software for any purpose.
9 * It is provided "as is" without express or implied warranty.
11 module b2dlite
.collide
;
14 import b2dlite
.arbiter
;
16 import b2dlite
.mathutils
;
19 // Box vertex and edge numbering:
55 private void Flip (ref FeaturePair fp
) {
56 Swap(fp
.e
.inEdge1
, fp
.e
.inEdge2
);
57 Swap(fp
.e
.outEdge1
, fp
.e
.outEdge2
);
61 private int ClipSegmentToLine() (ClipVertex
[/*2*/] vOut
, ClipVertex
[/*2*/] vIn
, in auto ref Vec2 normal
, VFloat offset
, char clipEdge
) {
62 // start with no output points
64 // calculate the distance of end points to the line
65 VFloat distance0
= /*Dot*/(normal
*vIn
[0].v
)-offset
;
66 VFloat distance1
= /*Dot*/(normal
*vIn
[1].v
)-offset
;
67 // if the points are behind the plane
68 if (distance0
<= VFloatNum
!(0.0)) vOut
[numOut
++] = vIn
[0];
69 if (distance1
<= VFloatNum
!(0.0)) vOut
[numOut
++] = vIn
[1];
70 // if the points are on different sides of the plane
71 if (distance0
*distance1
< VFloatNum
!(0.0)) {
72 // find intersection point of edge and plane
73 VFloat interp
= distance0
/(distance0
-distance1
);
74 vOut
[numOut
].v
= vIn
[0].v
+interp
*(vIn
[1].v
-vIn
[0].v
);
75 if (distance0
> VFloatNum
!(0.0)) {
76 vOut
[numOut
].fp
= vIn
[0].fp
;
77 vOut
[numOut
].fp
.e
.inEdge1
= clipEdge
;
78 vOut
[numOut
].fp
.e
.inEdge2
= EdgeNumbers
.NO_EDGE
;
80 vOut
[numOut
].fp
= vIn
[1].fp
;
81 vOut
[numOut
].fp
.e
.outEdge1
= clipEdge
;
82 vOut
[numOut
].fp
.e
.outEdge2
= EdgeNumbers
.NO_EDGE
;
90 private void ComputeIncidentEdge() (ClipVertex
[/*2*/] c
, in auto ref Vec2 h
, in auto ref Vec2 pos
, in auto ref Mat22 Rot
, in auto ref Vec2 normal
) {
91 // the normal is from the reference box; convert it to the incident boxe's frame and flip sign
92 Mat22 RotT
= Rot
.transpose();
93 Vec2 n
= -(RotT
*normal
);
95 if (nAbs
.x
> nAbs
.y
) {
96 if (Sign(n
.x
) > VFloatNum
!(0.0)) {
97 c
[0].v
.set(h
.x
, -h
.y
);
98 c
[0].fp
.e
.inEdge2
= EdgeNumbers
.EDGE3
;
99 c
[0].fp
.e
.outEdge2
= EdgeNumbers
.EDGE4
;
101 c
[1].v
.set(h
.x
, h
.y
);
102 c
[1].fp
.e
.inEdge2
= EdgeNumbers
.EDGE4
;
103 c
[1].fp
.e
.outEdge2
= EdgeNumbers
.EDGE1
;
105 c
[0].v
.set(-h
.x
, h
.y
);
106 c
[0].fp
.e
.inEdge2
= EdgeNumbers
.EDGE1
;
107 c
[0].fp
.e
.outEdge2
= EdgeNumbers
.EDGE2
;
109 c
[1].v
.set(-h
.x
, -h
.y
);
110 c
[1].fp
.e
.inEdge2
= EdgeNumbers
.EDGE2
;
111 c
[1].fp
.e
.outEdge2
= EdgeNumbers
.EDGE3
;
114 if (Sign(n
.y
) > VFloatNum
!(0.0)) {
115 c
[0].v
.set(h
.x
, h
.y
);
116 c
[0].fp
.e
.inEdge2
= EdgeNumbers
.EDGE4
;
117 c
[0].fp
.e
.outEdge2
= EdgeNumbers
.EDGE1
;
119 c
[1].v
.set(-h
.x
, h
.y
);
120 c
[1].fp
.e
.inEdge2
= EdgeNumbers
.EDGE1
;
121 c
[1].fp
.e
.outEdge2
= EdgeNumbers
.EDGE2
;
123 c
[0].v
.set(-h
.x
, -h
.y
);
124 c
[0].fp
.e
.inEdge2
= EdgeNumbers
.EDGE2
;
125 c
[0].fp
.e
.outEdge2
= EdgeNumbers
.EDGE3
;
127 c
[1].v
.set(h
.x
, -h
.y
);
128 c
[1].fp
.e
.inEdge2
= EdgeNumbers
.EDGE3
;
129 c
[1].fp
.e
.outEdge2
= EdgeNumbers
.EDGE4
;
133 c
[0].v
= pos
+Rot
*c
[0].v
;
134 c
[1].v
= pos
+Rot
*c
[1].v
;
138 // the normal points from A to B
139 int Collide (Contact
* contacts
, Body bodyA
, Body bodyB
) {
141 Vec2 hA
= VFloatNum
!(0.5)*bodyA
.width
;
142 Vec2 hB
= VFloatNum
!(0.5)*bodyB
.width
;
144 Vec2 posA
= bodyA
.position
;
145 Vec2 posB
= bodyB
.position
;
147 auto RotA
= Mat22 (bodyA
.rotation
);
148 auto RotB
= Mat22 (bodyB
.rotation
);
150 Mat22 RotAT
= RotA
.transpose();
151 Mat22 RotBT
= RotB
.transpose();
153 //k8:Vec2 a1 = RotA.col1, a2 = RotA.col2;
154 //k8:Vec2 b1 = RotB.col1, b2 = RotB.col2;
160 Mat22 C
= RotAT
*RotB
;
162 Mat22 absCT
= absC
.transpose();
165 Vec2 faceA
= dA
.abs
-hA
-absC
*hB
;
166 if (faceA
.x
> VFloatNum
!(0.0) || faceA
.y
> VFloatNum
!(0.0)) return 0;
169 Vec2 faceB
= dB
.abs
-absCT
*hA
-hB
;
170 if (faceB
.x
> VFloatNum
!(0.0) || faceB
.y
> VFloatNum
!(0.0)) return 0;
178 axis
= Axis
.FACE_A_X
;
179 separation
= faceA
.x
;
180 normal
= dA
.x
> VFloatNum
!(0.0) ? RotA
.col1
: -RotA
.col1
;
182 const VFloat relativeTol
= VFloatNum
!(0.95);
183 const VFloat absoluteTol
= VFloatNum
!(0.01);
185 if (faceA
.y
> relativeTol
*separation
+absoluteTol
*hA
.y
) {
186 axis
= Axis
.FACE_A_Y
;
187 separation
= faceA
.y
;
188 normal
= dA
.y
> VFloatNum
!(0.0) ? RotA
.col2
: -RotA
.col2
;
192 if (faceB
.x
> relativeTol
*separation
+absoluteTol
*hB
.x
) {
193 axis
= Axis
.FACE_B_X
;
194 separation
= faceB
.x
;
195 normal
= dB
.x
> VFloatNum
!(0.0) ? RotB
.col1
: -RotB
.col1
;
198 if (faceB
.y
> relativeTol
*separation
+absoluteTol
*hB
.y
) {
199 axis
= Axis
.FACE_B_Y
;
200 separation
= faceB
.y
;
201 normal
= dB
.y
> VFloatNum
!(0.0) ? RotB
.col2
: -RotB
.col2
;
204 // Setup clipping plane data based on the separating axis
205 Vec2 frontNormal
, sideNormal
;
206 ClipVertex
[2] incidentEdge
;
207 VFloat front
, negSide
, posSide
;
208 char negEdge
, posEdge
;
210 // Compute the clipping lines and the line segment to be clipped.
213 frontNormal
= normal
;
214 front
= /*Dot*/(posA
*frontNormal
)+hA
.x
;
215 sideNormal
= RotA
.col2
;
216 VFloat side
= /*Dot*/(posA
*sideNormal
);
217 negSide
= -side
+hA
.y
;
219 negEdge
= EdgeNumbers
.EDGE3
;
220 posEdge
= EdgeNumbers
.EDGE1
;
221 ComputeIncidentEdge(incidentEdge
, hB
, posB
, RotB
, frontNormal
);
224 frontNormal
= normal
;
225 front
= /*Dot*/(posA
*frontNormal
)+hA
.y
;
226 sideNormal
= RotA
.col1
;
227 VFloat side
= /*Dot*/(posA
*sideNormal
);
228 negSide
= -side
+hA
.x
;
230 negEdge
= EdgeNumbers
.EDGE2
;
231 posEdge
= EdgeNumbers
.EDGE4
;
232 ComputeIncidentEdge(incidentEdge
, hB
, posB
, RotB
, frontNormal
);
235 frontNormal
= -normal
;
236 front
= /*Dot*/(posB
*frontNormal
)+hB
.x
;
237 sideNormal
= RotB
.col2
;
238 VFloat side
= /*Dot*/(posB
*sideNormal
);
239 negSide
= -side
+hB
.y
;
241 negEdge
= EdgeNumbers
.EDGE3
;
242 posEdge
= EdgeNumbers
.EDGE1
;
243 ComputeIncidentEdge(incidentEdge
, hA
, posA
, RotA
, frontNormal
);
246 frontNormal
= -normal
;
247 front
= /*Dot*/(posB
*frontNormal
)+hB
.y
;
248 sideNormal
= RotB
.col1
;
249 VFloat side
= /*Dot*/(posB
*sideNormal
);
250 negSide
= -side
+hB
.x
;
252 negEdge
= EdgeNumbers
.EDGE2
;
253 posEdge
= EdgeNumbers
.EDGE4
;
254 ComputeIncidentEdge(incidentEdge
, hA
, posA
, RotA
, frontNormal
);
259 // clip other face with 5 box planes (1 face plane, 4 edge planes)
260 ClipVertex
[2] clipPoints1
;
261 ClipVertex
[2] clipPoints2
;
264 // clip to box side 1
265 np
= ClipSegmentToLine(clipPoints1
, incidentEdge
, -sideNormal
, negSide
, negEdge
);
266 if (np
< 2) return 0;
268 // clip to negative box side 1
269 np
= ClipSegmentToLine(clipPoints2
, clipPoints1
, sideNormal
, posSide
, posEdge
);
270 if (np
< 2) return 0;
272 // Now clipPoints2 contains the clipping points.
273 // Due to roundoff, it is possible that clipping removes all points.
276 for (int i
= 0; i
< 2; ++i
) {
277 separation
= /*Dot*/(frontNormal
*clipPoints2
[i
].v
)-front
;
278 if (separation
<= 0) {
279 contacts
[numContacts
].separation
= separation
;
280 contacts
[numContacts
].normal
= normal
;
281 // slide contact point onto reference face (easy to cull)
282 contacts
[numContacts
].position
= clipPoints2
[i
].v
-separation
*frontNormal
;
283 contacts
[numContacts
].feature
= clipPoints2
[i
].fp
;
284 if (axis
== Axis
.FACE_B_X || axis
== Axis
.FACE_B_Y
) Flip(contacts
[numContacts
].feature
);