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
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
|
#include <stdio.h>
#include <cu/cu.h>
#include <ccd/vec3.h>
TEST(vec3SetUp)
{
}
TEST(vec3TearDown)
{
}
TEST(vec3PointSegmentDist)
{
ccd_vec3_t P, a, b, w, ew;
ccd_real_t dist;
ccdVec3Set(&a, 0., 0., 0.);
ccdVec3Set(&b, 1., 0., 0.);
// extereme w == a
ccdVec3Set(&P, -1., 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 1.));
assertTrue(ccdVec3Eq(&w, &a));
ccdVec3Set(&P, -0.5, 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 0.5 * 0.5));
assertTrue(ccdVec3Eq(&w, &a));
ccdVec3Set(&P, -0.1, 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, .1 * .1));
assertTrue(ccdVec3Eq(&w, &a));
ccdVec3Set(&P, 0., 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 0.));
assertTrue(ccdVec3Eq(&w, &a));
ccdVec3Set(&P, -1., 1., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 2.));
assertTrue(ccdVec3Eq(&w, &a));
ccdVec3Set(&P, -0.5, 0.5, 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 0.5));
assertTrue(ccdVec3Eq(&w, &a));
ccdVec3Set(&P, -0.1, -1., 2.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 5.01));
assertTrue(ccdVec3Eq(&w, &a));
// extereme w == b
ccdVec3Set(&P, 2., 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 1.));
assertTrue(ccdVec3Eq(&w, &b));
ccdVec3Set(&P, 1.5, 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 0.5 * 0.5));
assertTrue(ccdVec3Eq(&w, &b));
ccdVec3Set(&P, 1.1, 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, .1 * .1));
assertTrue(ccdVec3Eq(&w, &b));
ccdVec3Set(&P, 1., 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 0.));
assertTrue(ccdVec3Eq(&w, &b));
ccdVec3Set(&P, 2., 1., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 2.));
assertTrue(ccdVec3Eq(&w, &b));
ccdVec3Set(&P, 1.5, 0.5, 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 0.5));
assertTrue(ccdVec3Eq(&w, &b));
ccdVec3Set(&P, 1.1, -1., 2.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 5.01));
assertTrue(ccdVec3Eq(&w, &b));
// inside segment
ccdVec3Set(&P, .5, 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 0.));
assertTrue(ccdVec3Eq(&w, &P));
ccdVec3Set(&P, .9, 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 0.));
assertTrue(ccdVec3Eq(&w, &P));
ccdVec3Set(&P, .5, 1., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 1.));
ccdVec3Set(&ew, 0.5, 0., 0.);
assertTrue(ccdVec3Eq(&w, &ew));
ccdVec3Set(&P, .5, 1., 1.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 2.));
ccdVec3Set(&ew, 0.5, 0., 0.);
assertTrue(ccdVec3Eq(&w, &ew));
ccdVec3Set(&a, -.5, 2., 1.);
ccdVec3Set(&b, 1., 1.5, 0.5);
// extereme w == a
ccdVec3Set(&P, -10., 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 9.5 * 9.5 + 2. * 2. + 1.));
assertTrue(ccdVec3Eq(&w, &a));
ccdVec3Set(&P, -10., 9.2, 3.4);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 9.5 * 9.5 + 7.2 * 7.2 + 2.4 * 2.4));
assertTrue(ccdVec3Eq(&w, &a));
// extereme w == b
ccdVec3Set(&P, 10., 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 9. * 9. + 1.5 * 1.5 + 0.5 * 0.5));
assertTrue(ccdVec3Eq(&w, &b));
ccdVec3Set(&P, 10., 9.2, 3.4);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 9. * 9. + 7.7 * 7.7 + 2.9 * 2.9));
assertTrue(ccdVec3Eq(&w, &b));
// inside ab
ccdVec3Set(&a, -.1, 1., 1.);
ccdVec3Set(&b, 1., 1., 1.);
ccdVec3Set(&P, 0., 0., 0.);
dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
assertTrue(ccdEq(dist, 2.));
ccdVec3Set(&ew, 0., 1., 1.);
assertTrue(ccdVec3Eq(&w, &ew));
}
TEST(vec3PointTriDist)
{
ccd_vec3_t P, a, b, c, w, P0;
ccd_real_t dist;
ccdVec3Set(&a, -1., 0., 0.);
ccdVec3Set(&b, 0., 1., 1.);
ccdVec3Set(&c, -1., 0., 1.);
ccdVec3Set(&P, -1., 0., 0.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 0.));
assertTrue(ccdVec3Eq(&w, &a));
ccdVec3Set(&P, 0., 1., 1.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 0.));
assertTrue(ccdVec3Eq(&w, &b));
ccdVec3Set(&P, -1., 0., 1.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 0.));
assertTrue(ccdVec3Eq(&w, &c));
ccdVec3Set(&P, 0., 0., 0.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, NULL);
assertTrue(ccdEq(dist, 2./3.));
// region 4
ccdVec3Set(&P, -2., 0., 0.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &a)));
assertTrue(ccdVec3Eq(&w, &a));
ccdVec3Set(&P, -2., 0.2, -1.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &a)));
assertTrue(ccdVec3Eq(&w, &a));
// region 2
ccdVec3Set(&P, -1.3, 0., 1.2);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &c)));
assertTrue(ccdVec3Eq(&w, &c));
ccdVec3Set(&P, -1.2, 0.2, 1.1);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &c)));
assertTrue(ccdVec3Eq(&w, &c));
// region 6
ccdVec3Set(&P, 0.3, 1., 1.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &b)));
assertTrue(ccdVec3Eq(&w, &b));
ccdVec3Set(&P, .1, 1., 1.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &b)));
assertTrue(ccdVec3Eq(&w, &b));
// region 1
ccdVec3Set(&P, 0., 1., 2.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 1.));
assertTrue(ccdVec3Eq(&w, &b));
ccdVec3Set(&P, -1., 0., 2.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 1.));
assertTrue(ccdVec3Eq(&w, &c));
ccdVec3Set(&P, -0.5, 0.5, 2.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 1.));
ccdVec3Set(&P0, -0.5, 0.5, 1.);
assertTrue(ccdVec3Eq(&w, &P0));
// region 3
ccdVec3Set(&P, -2., -1., 0.7);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 2.));
ccdVec3Set(&P0, -1., 0., 0.7);
assertTrue(ccdVec3Eq(&w, &P0));
// region 5
ccdVec3Set(&P, 0., 0., 0.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 2./3.));
ccdVec3Set(&P0, -2./3., 1./3., 1./3.);
assertTrue(ccdVec3Eq(&w, &P0));
// region 0
ccdVec3Set(&P, -0.5, 0.5, 0.5);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 0.));
assertTrue(ccdVec3Eq(&w, &P));
ccdVec3Set(&P, -0.5, 0.5, 0.7);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 0.));
assertTrue(ccdVec3Eq(&w, &P));
ccdVec3Set(&P, -0.5, 0.5, 0.9);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 0.));
assertTrue(ccdVec3Eq(&w, &P));
ccdVec3Set(&P, 0., 0., 0.5);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 0.5));
ccdVec3Set(&P0, -.5, .5, .5);
assertTrue(ccdVec3Eq(&w, &P0));
ccdVec3Set(&a, -1., 0., 0.);
ccdVec3Set(&b, 0., 1., -1.);
ccdVec3Set(&c, 0., 1., 1.);
ccdVec3Set(&P, 0., 0., 0.);
dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
assertTrue(ccdEq(dist, 0.5));
ccdVec3Set(&P0, -.5, .5, 0.);
assertTrue(ccdVec3Eq(&w, &P0));
//fprintf(stderr, "dist: %lf\n", dist);
}
|
