1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
|
struct IntersectionResult {
bool intersect = false;
Vector2 collisionNormal;
Vector2 relativeVelocity;
Vector2 firstPointOfApplication;
Vector2 secondPointOfApplication;
};
struct Edge {
Vector2 normal;
Vector2 start;
Vector2 end;
};
struct SATResult {
Edge* minOverlapEdge = NULL; // Edge that caused the intersection (on the 'first' shape)
Vector2 overlapPoint; // Point that caused the intersection (on the 'second' shape)
float32 minOverlap = FLT_MAX; // Smallest projection overlap
};
struct ProjectionResult {
Vector2 minVertex;
Vector2 maxVertex;
Vector2 projection;
};
// Given the vertices in the shape, we find the min and max vertices that project
// onto the axis. We also save which vertices were which so that we can resolve
// the collision later.
ProjectionResult getProjection(Vector2* vertices, int numVertices, Vector2 axis) {
ProjectionResult pr;
pr.minVertex = vertices[0];
pr.maxVertex = vertices[0];
float32 min = axis.dot(vertices[0]);
float32 max = min;
for (int v = 1; v < numVertices; v++) {
float32 d = axis.dot(vertices[v]);
if (d < min) {
pr.minVertex = vertices[v];
min = d;
} else if (d > max) {
pr.maxVertex = vertices[v];
max = d;
}
}
pr.projection = Vector2 { min, max };
return pr;
}
bool projectionsOverlap(Vector2 first, Vector2 second) {
return first.x <= second.y && second.x <= first.y;
}
float32 getProjectionOverlap(Vector2 first, Vector2 second) {
float32 e = MIN(first.y, second.y);
float32 f = MAX(first.x, second.x);
return e - f;
}
bool runSatForShapesEdges(SATResult* result, ConvexPolygon* first, ConvexPolygon* second) {
for (int i = 0; i < first->numVertices; i++) {
Vector2 normal = first->edges[i].normal;
ProjectionResult firstProj = getProjection(first->transformedVertices, first->numVertices, normal);
ProjectionResult secondProj = getProjection(second->transformedVertices, second->numVertices, normal);
if (!projectionsOverlap(firstProj.projection, secondProj.projection)) {
return false;
}
float32 overlap = getProjectionOverlap(firstProj.projection, secondProj.projection);
if (overlap < result->minOverlap) {
result->minOverlap = overlap;
result->minOverlapEdge = &first->edges[i];
// The overlapPoint will be the point on the other shape that penetrated the edge.
// If we caught the intersection reasonably early, it should be the point on 'second'
// that is nearest to the points on 'first'.
float32 min1min2 = (firstProj.minVertex - secondProj.minVertex).length();
float32 min1max2 = (firstProj.minVertex - secondProj.maxVertex).length();
float32 max1max2 = (firstProj.maxVertex - secondProj.maxVertex).length();
float32 max1min2 = (firstProj.maxVertex - secondProj.minVertex).length();
float32 closest = MIN(min1min2, MIN(min1max2, MIN(max1max2, max1min2)));
if (closest == min1min2 || closest == max1min2) {
result->overlapPoint = secondProj.minVertex;
} else {
result->overlapPoint = secondProj.maxVertex;
}
}
}
return true;
}
const float32 EPSILON = 1.f;
IntersectionResult getIntersection(ConvexPolygon* first, ConvexPolygon* second) {
IntersectionResult ir;
SATResult sat;
if (!runSatForShapesEdges(&sat, first, second)) {
return ir;
}
if (!runSatForShapesEdges(&sat, second, first)) {
return ir;
}
ir.intersect = true;
ir.relativeVelocity = first->body.velocity - second->body.velocity;
ir.collisionNormal = sat.minOverlapEdge->normal;
ir.firstPointOfApplication = sat.overlapPoint - first->body.position;
ir.secondPointOfApplication = sat.overlapPoint - second->body.position;;
return ir;
}
|
