sbylib.collision.narrow.polygonsphere source code

1 module sbylib.collision.narrow.polygonsphere;
2 
3 import sbylib.collision.shape.polygon : CollisionPolygon;
4 import sbylib.collision.shape.sphere : CollisionSphere;
5 import sbylib.math;
6 import std.typecons : Nullable, nullable;
7 
8 enum Type { Face, Edge, Vertex }
9 
10 struct PolygonSphereResult {
11     vec3 _pushVector;
12     Type type;
13 
14     vec3 pushVector(CollisionPolygon) {
15         return _pushVector;
16     }
17 
18     vec3 pushVector(CollisionSphere) {
19         return -_pushVector;
20     }
21 }
22 
23 auto detect(Polygon : CollisionPolygon, Sphere : CollisionSphere) (Sphere sphere, Polygon polygon) {
24     return detect(polygon, sphere);
25 }
26 
27 auto detect(Polygon : CollisionPolygon, Sphere : CollisionSphere) (Polygon polygon, Sphere sphere) {
28     auto pos = inverseSignPos(polygon.vertices, sphere.center);
29     if (pos[0] == -1)
30         return toFace(polygon.vertices, sphere);
31     if (pos[1] == -1)
32         return toEdge([polygon.vertices[pos[0]], polygon.vertices[(pos[0]+1)%$]], sphere);
33     if (pos[0] + 1 == pos[1])
34         return toVertex(polygon.vertices[pos[1]], sphere);
35     if ((pos[1] + 1) % polygon.vertices.length == pos[0])
36         return toVertex(polygon.vertices[pos[0]], sphere);
37     assert(false);
38 }
39 
40 private int[2] inverseSignPos(vec3[] v, vec3 p) {
41     auto base = getNormal(v[0], v[1], v[2]);
42     int pos = 0;
43     int[2] res = [-1,-1];
44     foreach (i; 0..v.length) {
45         if (dot(base, getNormal(v[i], v[(i+1)%v.length], p)) < 0) {
46             assert(pos < 2);
47             res[pos++] = cast(int)i;
48         }
49     }
50     return res;
51 }
52 
53 private auto toFace(Sphere)(vec3[] v, Sphere s) {
54     auto n = getNormal(v[0], v[1], v[2]).normalize;
55     auto d = dot(n, s.center - v[0]);
56     return wrapResult(n * d, s, Type.Face);
57 }
58 
59 private auto toEdge(Sphere)(vec3[] v, Sphere s) {
60     const vec = v[1] - v[0];
61     const l2 = lengthSq(vec);
62     const t = dot(vec, s.center - v[0]) / l2;
63     if (t < 0) return wrapResult(v[0] - s.center, s, Type.Edge);
64     if (t > 1) return wrapResult(v[1] - s.center, s, Type.Edge);
65     return wrapResult(v[0] + vec * t - s.center, s, Type.Edge);
66 }
67 
68 private auto toVertex(Sphere)(vec3 v, Sphere s) {
69     return wrapResult(v - s.center, s, Type.Vertex);
70 }
71 
72 private Nullable!PolygonSphereResult wrapResult(Sphere)(vec3 d, Sphere s, Type t) {
73     auto l = length(d);
74     if (l > s.radius) return typeof(return).init;
75     return nullable(PolygonSphereResult(safeNormalize(d) * (l - s.radius), t));
76 }
77 
78 private vec3 getNormal(vec3 a, vec3 b, vec3 c) {
79     return cross(a-b, b-c);
80 }