首页 模拟笔记

# GJK 碰撞检测

闵可夫斯基和是两个欧几里得空间的点集的和，也称为这两个空间的膨胀集，

例如，平面上有两个三角形，其坐标分别为 A={(1,0),(0,1),(0,-1)} 及 B = {(0, 0), (1, 1), (1, −1)}，则其闵可夫斯基和为 A + B= {(1, 0), (2, 1), (2, −1), (0, 1), (1, 2), (1, 0), (0, −1), (1, 0), (1, −2)}。

若推广至流形的连续集，闵可夫斯基和从几何上的直观体现即是 A 集合沿 B 的边际连续运动一周扫过的区域与 B 集合本身的并集，也可以是 B 沿着 A 的边界连续运动扫过区域与 A 自身的并集。

Untitled

Untitled

可以判断两个多边形是否相交，要是并集过零的话，说明相交

闵可夫斯基差

就是被减数 b 取反然后顺着 a 去描边

Untitled

Untitled

两个形体最顶端的点相一定会是闵可夫斯基的其中一个顶点，找到三个点形成三角形，判断是不是包含原点。每次迭代输入一个迭代方向可以得到不同的点。

迭代方式是首先找到闵可夫斯基集的两个点连线以垂直为向量为方向，找到第三个点，判断原点是不是在里面，要是没在就把远离原点的点移除，剩下连线的找垂直向量为方向找新的点

Untitled

Untitled
碰撞检测算法之 GJK 算法
(转) GJK 算法详细介绍

GJKhlsl
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
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
	// GJK support function for a given direction
Vector3 GJKSupportFunction(Vector3 shape1Vertices[], int shape1VerticesCount, Vector3 shape2Vertices[], int shape2VerticesCount, Vector3 direction)
{
    Vector3 supportPoint;

    float maxDot = -3.402823466e+38; // Min float value
    for (int i = 0; i < shape1VerticesCount; i++)
    {
        Vector3 vertex = shape1Vertices[i];
        float dotProduct = dot(vertex, direction);
        if (dotProduct > maxDot)
        {
            maxDot = dotProduct;
            supportPoint = vertex;
        }
    }
		maxDot = -3.402823466e+38;
		vector3 supportPoint1;

    for (int i = 0; i < shape2VerticesCount; i++)
    {
        Vector3 vertex = shape2Vertices[i];
        float dotProduct = dot(vertex, -direction);
        if (dotProduct > maxDot)
        {
            maxDot = dotProduct;
            supportPoint1 = vertex;
        }
    }

    return supportPoint - supportPoint1;
}//////////


bool GJKIntersect(Vector3 shape1Vertices[], int shape1VerticesCount, Vector3 shape2Vertices[], int shape2VerticesCount)
{
    Vector3 simplex[4];
    int simplexCount = 0;

    Vector3 direction = normalize(shape2Vertices[0] - shape1Vertices[0]);

    simplex[simplexCount++] = GJKSupportFunction(shape1Vertices, shape1VerticesCount, shape2Vertices, shape2VerticesCount, direction);

    direction = -simplex[0];

    while (true)
    {
        Vector3 a = GJKSupportFunction(shape1Vertices, shape1VerticesCount, shape2Vertices, shape2VerticesCount, direction);

        if (dot(a, direction) < 0)
            return false;

        simplex[simplexCount++] = a;

        if (simplexCount == 4)
            break;

        if (DoSimplex(simplex, simplexCount, direction))
            return true;
    }

    return false;
}

// Helper function to do the simplex part of GJK algorithm


bool DoSimplex2(Vector3 simplex[], int& simplexCount, inout Vector3 direction)
{
    Vector3f A = s[1];
    Vector3f B = s[0];
    Vector3f AB = B - A;
    Vector3f AO = -A;
    Vector3f ABOB = AB.cross(AO).cross(AB); // norm to AB toward origin

    if (ABOB.norm() == 0.f) // origin is on AB
    {
        return true;
    }
    else // origin is on voronoi region of AB
    {
        d = ABOB;
        return false;
    }

}

bool DoSimplex3(Vector3 simplex[], int& simplexCount, inout Vector3 direction)
{
   Vector3f A = s[2];
    Vector3f B = s[1];
    Vector3f C = s[0];
    Vector3f AB = B - A;
    Vector3f AC = C - A;
    Vector3f AO = -A;
    Vector3f ABC = AB.cross(AC);
    Vector3f ACBB = -ABC.cross(AB); // norm to AB toward far from C
    Vector3f ABCC = ABC.cross(AC); // norm to AC toward far from B

    if (ABCC.dot(AO) > 0.f) // origin is on AC voronoi
    {
        s = {C, A};
        d = ABCC;
    }
    else if (ACBB.dot(AO) > 0.f) // origin is on AB voronoi
    {
        s = {B, A};
        d = ACBB;
    }
    else // origin is on ABC voronoi
    {
        float ABCO = ABC.dot(AO);
        if (ABCO > 0.f) // above
        {
            d = ABC;
        }
        else if (ABCO < 0.f) // below
        {
            s = {B, C, A};
            d = -ABC;
        }
        else // on
        {
            return true;
        }
    }
    return false;
}

bool DoSimplex4(Vector3 simplex[], int& simplexCount, inout Vector3 direction)
{
    Vector3f A = s[3];
    Vector3f B = s[2];
    Vector3f C = s[1];
    Vector3f D = s[0];

    Vector3f AO = -A;
    Vector3f AB = B - A;
    Vector3f AC = C - A;
    Vector3f AD = D - A;

    Vector3f ABC = AB.cross(AC); // normal to ABC
    Vector3f ACD = AC.cross(AD); // normal to ACD
    Vector3f ADB = AD.cross(AB); // normal to ADB

    if (ABC.dot(AO) > 0)
    {
        return Simplex3(s = {C, B, A}, d);
    }
    if (ACD.dot(AO) > 0)
    {
        return Simplex3(s = {D, C, A}, d);
    }
    if (ADB.dot(AO) > 0)
    {
        return Simplex3(s = {B, D, A}, d);
    }
    return true;
}
bool DoSimplex(Vector3 simplex[], int& simplexCount, inout Vector3 direction)
{
    switch (simplexCount)
    {
        case 2:
            return DoSimplex2(simplex, simplexCount, direction);
        case 3:
            return DoSimplex3(simplex, simplexCount, direction);
        case 4:
            return DoSimplex4(simplex, simplexCount, direction);
        default:
            return false;
    }
}


EPA
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
float3 closestPointOnLine(float3 p, float3 a, float3 b)
{
    float3 ap = p - a;
    float3 ab = b - a;
    float t = dot(ap, ab) / dot(ab, ab);
    return a + ab * t;
}

float3 support(float3 n, float3 p)
{
    float3 d = n > 0? float3(1, 1, 1) : float3(-1, -1, -1);
    float3 v = p + d * max(0, dot(n, p));
    return v;
}

bool intersects(float3 p1, float3 q1, float3 p2, float3 q2)
{
    float3 d1 = q1 - p1;
    float3 d2 = q2 - p2;
    float3 n = cross(d1, d2);
    if (dot(n, n) < 0.0001f)
    {
        return false;
    }
    float3 v1 = support(-n, p1);
    float3 v2 = support(n, p2);
    float3 w1 = v1 - p1;
    float3 w2 = v2 - p2;
    if (dot(cross(w1, d2), cross(w2, d1)) < 0)
    {
        return false;
    }
    float3 c1 = closestPointOnLine(v1, p1, q1);
    float3 c2 = closestPointOnLine(v2, p2, q2);
    return distance(c1, c2) < 0.0001f;
}

bool GJK(float3 p1, float3 d1, float3 p2, float3 d2, out float3 hit)
{
    float3 ab = p2 - p1;
    float3 ad = d2 - d1;
    float h = dot(ab, ad);
    if (h <= 0)
    {
        hit = p1;
        return false;
    }
    float3 ac = p1 - p2;
    float3 bc = d1 - d2;
    float k = dot(ac, bc);
    if (k <= 0)
    {
        hit = p2;
        return false;
    }
    float3 n = cross(ab, ac);
    if (dot(n, d1) < 0)
    {
        n = -n;
    }
    hit = p1 + n * sqrt(h * h - k * k) / dot(n, d1);
    return true;
}

bool EPA(float3 p1, float3 d1, float3 p2, float3 d2, out float3 hit)
{
    const int maxIterations = 100;
    const float tolerance = 0.001f;

    float3 m = normalize(p2 - p1);
    float3 n = cross(d1, d2);
    float3 q = p2 + d2 * dot(p2 - p1, d2);
    float3 a = dot(d1, d1);
    float3 e = dot(d2, d2);
    float3 c = dot(d1, n);
    float3 r = dot(n, n);
    float3 b = dot(d1, q - p1);
    float3 d = dot(d2, q - p2);
    float3 f = dot(n, q - p1);

    float3 u = float3(0, 0, 0);
    float3 v = float3(0, 0, 0);
    float3 w = float3(0, 0, 0);

    float3 closestPoint1 = float3(0, 0, 0);
    float3 closestPoint2 = float3(0, 0, 0);

    for (int i = 0; i < maxIterations; i++)
    {
        float t = (-b + sqrt(b * b - a * c)) / (a + e);
        if (t < 0 || t > 1)
        {
            t = (-b - sqrt(b * b - a * c)) / (a + e);
            if (t < 0 || t > 1)
            {
                break;
            }
        }

        float3 intersection = p1 + d1 * t;
        float3 distance = intersection - q;

        if (dot(distance, distance) < tolerance)
        {
            hit = intersection;
            return true;
        }

        if (dot(distance, n) > 0)
        {
            n = -n;
            r = -r;
            f = -f;
        }

        float3 tangent = normalize(q - intersection);
        float3 bitangent = cross(n, tangent);

        u = cross(tangent, bitangent);
        v = cross(bitangent, tangent);
        w = n;

        float3 offset = u * dot(distance, u) + v * dot(distance, v) + w * dot(distance, w);
        float3 newPoint1 = intersection + offset;
        float3 newPoint2 = intersection - offset;

        if (dot(newPoint1 - closestPoint1, newPoint1 - closestPoint1) < tolerance ||
            dot(newPoint2 - closestPoint2, newPoint2 - closestPoint2) < tolerance)
        {
            break;
        }

        closestPoint1 = newPoint1;
        closestPoint2 = newPoint2;
    }

    hit = float3(0, 0, 0);
    return false;
}

// 检测两个3D物体是否碰撞
bool CheckCollision(float3 p1, float3 d1, float3 p2, float3 d2, out float3 hit)
{
    float3 gjkHit = float3(0, 0, 0);
    if (!GJK(p1, d1, p2, d2, gjkHit)

更新于

请我喝[茶]~(￣▽￣)~*

Natsuneko 微信支付

微信支付

Natsuneko 支付宝

支付宝

Natsuneko 贝宝

贝宝