miniscript-physics/sat.ms at main · JoeStrout/miniscript-physics · GitHub

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
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
import "mathUtil"

import "vector"
import "debugDraw"
import "satClipping"

getRectangleVertices = function(body)
    hw = body.shape.width / 2
    hh = body.shape.height / 2

    pos = body.pos
    rot = body.rotationRad

    return [
        [-hw, hh].rotatedBy(rot).plus(pos),
        [hw , hh].rotatedBy(rot).plus(pos),
        [hw, -hh].rotatedBy(rot).plus(pos),
        [-hw, -hh].rotatedBy(rot).plus(pos),
    ]
end function

getRectangleAxes = function(body)
    return [
        [1, 0].rotatedBy(body.rotationRad),
        [0, 1].rotatedBy(body.rotationRad),
    ]
end function

projectPoints = function(axis, points)
    min = 0
    hasMin = false
    max = 0
    hasMax = false

    for p in points
        proj = p.dot(axis)

        if not hasMin or proj < min then
            hasMin = true
            min = proj
        end if
        if not hasMax or proj > max then
            hasMax = true
            max = proj
        end if
    end for

    return {
        "min": min,
        "max": max,
    }
end function

findOverlapPoints = function(axes, points1, points2)
    minDepth = 1 / 0
    result = false

    for axis in axes
        proj1 = projectPoints(axis, points1)
        proj2 = projectPoints(axis, points2)

        if proj1.min >= proj2.max or proj2.min >= proj1.max then
            // one axis doesn't have overlap, the objects don't collide
            return false
        end if

        depth1 = proj1.max - proj2.min
        depth2 = proj2.max - proj1.min

        if depth1 > depth2 then
            depth = depth2
            axis = axis.times(-1)
        else
            depth = depth1
        end if

        if depth < minDepth then
            minDepth = depth

            result = {
                "normal": axis,
                "depth": depth,
            }
        end if
    end for

    return result
end function

testRectangles = function(b1, b2)
    pos1 = b1.pos
    pos2 = b2.pos

    axes = getRectangleAxes(b1) +
           getRectangleAxes(b2)

    points1 = getRectangleVertices(b1)
    points2 = getRectangleVertices(b2)

    if debugDrawEnabled then
        debugDraw.points points1
        debugDraw.points points2

        debugDraw.drawDisplay.line b1.x, b1.y, b2.x, b2.y, color.gray

        debugDraw.axes b1.pos, getRectangleAxes(b1)
        debugDraw.axes b2.pos, getRectangleAxes(b2)
    end if

    overlap1 = findOverlapPoints(axes, points1, points2)
    if not overlap1 then
        return false
    end if

    overlap2 = findOverlapPoints(axes, points2, points1)
    if not overlap2 then
        return false
    end if

    if overlap2.depth > overlap1.depth then
        overlap = overlap1
    else
        overlap = overlap2

        // invert direction because overlap2 tests b2 against b1, instead of b1 against b2
        overlap.normal = overlap2.normal.times(-1)
    end if

    clipped = satClipping.clipPoints(points1, points2, overlap.normal)

    if debugDrawEnabled then
        dir = overlap.normal.times(-overlap.depth)
        debugDraw.drawDisplay.line b1.x, b1.y, b1.x + dir.x, b1.y + dir.y, color.red, 4
    end if

    overlap.points = clipped
    return overlap
end function