2d-solid-collision/geo_util.cpp at main · jpadgett314/2d-solid-collision · 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
139
140
141
142
143
144
145
146
147
#include "geo_util.h"

using std::abs;

bool rectOverlap(const Rect& a, const Rect& b)
{
    Vec d1 = b.min - a.max;
    Vec d2 = a.min - b.max;

    if (d1.x > 0.0f || d1.y > 0.0f)
        return false;

    if (d2.x > 0.0f || d2.y > 0.0f)
        return false;

    return true;
}

bool segOverlap(const Segment& s1, const Segment& s2)
{
    if (dot(s1.p2 - s1.p1, s2.p1 - s1.p1) <= 0 &&
        dot(s1.p2 - s1.p1, s2.p2 - s1.p1) <= 0)
        return false;

    if (dot(s1.p1 - s1.p2, s2.p1 - s1.p2) <= 0 &&
        dot(s1.p1 - s1.p2, s2.p2 - s1.p2) <= 0)
        return false;

    return true;
}

bool segOverlap(const Segment& s1, const Segment& s2, Vec& overlap)
{
    if (!segOverlap(s1, s2))
        return false;

    Vec u, v;
    if ( dot(s2.p2 - s2.p1, s1.p2 - s1.p1) > 0 )
    {
        u = vectorProject(s2.p2 - s1.p1, s1.p2 - s1.p1);
        v = vectorProject(s2.p1 - s1.p2, s1.p2 - s1.p1);
    }
    else
    {
        u = vectorProject(s2.p1 - s1.p1, s1.p2 - s1.p1);
        v = vectorProject(s2.p2 - s1.p2, s1.p2 - s1.p1);
    }

    if (mag(v) < mag(u))
         overlap = v;
    else
         overlap = u;

    return true;
}

Vec segmentNormal(const Vec& e1, const Vec& e2)
{
    return Vec{-((e2 - e1).y), (e2 - e1).x};
}

Vec segmentNormal(const Vec& e1, const Vec& e2, const Vec& towards)
{
    Vec target{-((e2 - e1).y), (e2 - e1).x};
    Vec v = towards - e1;

    if(dot(target, v) < 0)
        target = target * -1;

    return target;
}

Vec projectPoint(const Vec& pt, const Vec& axis)
{
    /** Derivation

    y = m*x; // equation of axis. m is known.
    v = xi + yj  // vector along axis. x and y are unknowns.
    u = ai + bj  // vector to some arbitrary point; a and b are known.
    d = v - u = (x-a)i + (y-b)j  // vector from u to v

    // find x and y which causes the dot of v and d to be 0
    v . d = 0   ==>    x*(x-a) + y*(y-b) = 0
                ==>    y*(y-b) = -x*(x-a)
                ==>    m*(y-b) = -(x-a)
                ==>    m*(m*x-b) = -(x-a)
                ==>    m^2 * x - m*b = -x + a
                ==>    m^2 * x + x = a + m*b
                ==>    x * ( m^2 + 1 ) = a + m*b
                ==>    x = ( a + m*b ) / ( m^2 + 1 )
                ==>    y = m * x = m * ( a + m*b ) / ( m^2 + 1 )
    */

    if( abs(axis.y) > abs(axis.x) )
    {
        float m = axis.x / axis.y;
        float y = ( pt.y + m * pt.x ) / ( m*m + 1 );
        return {m*y, y};
    }
    else
    {
        float m = axis.y / axis.x;
        float x = ( pt.x + m * pt.y ) / ( m*m + 1 );
        return {x, m*x};
    }
}

bool projectPoint(const Vec& pt, const Segment& onto, Vec& result)
{
    Vec m = {onto.p2.x - onto.p1.x, onto.p2.y - onto.p1.y};
    result = projectPoint(pt, m);
    Segment seg = { projectPoint(onto.p1, m),
                    projectPoint(onto.p2, m) };

    return segContainsPt(seg, result);
}

bool segContainsPt(const Segment& seg, const Vec& pt)
{
    /** Derivation

        // endpoints of segment
        p1 = <u1, u2>
        p2 = <v1, v2>

        // parametric equation for segment, given above endpoints
        { x = (v1 - u1) * t1 + u1
          y = (v2 - u2) * t2 + u2 }
        (t from 0 to 1)

        // those equations can be rewritten as:
        t1 = (x - u1) / (v1 - u1)
        t2 = (y - u2) / (v2 - u2)

        // a point is on the segment if 0 <= t1 <= 1,
        // and if t2 approximately equals t1.
    */

    float t1 = (pt.x - seg.p1.x) / (seg.p2.x - seg.p1.x);
    float t2 = (pt.y - seg.p1.y) / (seg.p2.y - seg.p1.y);
    float epsilon  = 0.001f;

    if(abs(t1 - t2) < epsilon && t1 >= 0 && t1 <= 1)
        return true;
    else
        return false;
}