← Back to branch summary

~ubuntu-branches/debian/jessie/libccd/jessie

« back to all changes in this revision

Viewing changes to src/testsuites/mpr_boxbox.c

  • Committer: Package Import Robot
  • Author(s): Jose Luis Rivero
  • Date: 2014-03-24 16:51:48 UTC
  • Revision ID: package-import@ubuntu.com-20140324165148-mfno979f58rv322z
Tags: upstream-2.0
Import upstream version 2.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/testsuites/mpr_boxbox.c
 
1
#include <stdio.h>
 
2
#include <cu/cu.h>
 
3
 
 
4
#include <ccd/ccd.h>
 
5
#include <ccd/vec3.h>
 
6
#include "../dbg.h"
 
7
#include "support.h"
 
8
#include "common.h"
 
9
 
 
10
 
 
11
TEST(mprBoxboxAlignedX)
 
12
{
 
13
    size_t i;
 
14
    ccd_t ccd;
 
15
    CCD_BOX(box1);
 
16
    CCD_BOX(box2);
 
17
    int res;
 
18
 
 
19
    CCD_INIT(&ccd);
 
20
    ccd.support1 = ccdSupport;
 
21
    ccd.support2 = ccdSupport;
 
22
    ccd.center1  = ccdObjCenter;
 
23
    ccd.center2  = ccdObjCenter;
 
24
 
 
25
    box1.x = 1;
 
26
    box1.y = 2;
 
27
    box1.z = 1;
 
28
    box2.x = 2;
 
29
    box2.y = 1;
 
30
    box2.z = 2;
 
31
 
 
32
    ccdVec3Set(&box1.pos, -5., 0., 0.);
 
33
    ccdVec3Set(&box2.pos, 0., 0., 0.);
 
34
    ccdQuatSet(&box1.quat, 0., 0., 0., 1.);
 
35
    ccdQuatSet(&box2.quat, 0., 0., 0., 1.);
 
36
    for (i = 0; i < 100; i++){
 
37
        res = ccdMPRIntersect(&box1, &box2, &ccd);
 
38
        if (i < 35 || i > 65){
 
39
            assertFalse(res);
 
40
        }else if (i != 35 && i != 65){
 
41
            assertTrue(res);
 
42
        }
 
43
 
 
44
        box1.pos.v[0] += 0.1;
 
45
    }
 
46
 
 
47
    box1.x = 0.1;
 
48
    box1.y = 0.2;
 
49
    box1.z = 0.1;
 
50
    box2.x = 0.2;
 
51
    box2.y = 0.1;
 
52
    box2.z = 0.2;
 
53
 
 
54
    ccdVec3Set(&box1.pos, -0.5, 0., 0.);
 
55
    ccdVec3Set(&box2.pos, 0., 0., 0.);
 
56
    ccdQuatSet(&box1.quat, 0., 0., 0., 1.);
 
57
    ccdQuatSet(&box2.quat, 0., 0., 0., 1.);
 
58
    for (i = 0; i < 100; i++){
 
59
        res = ccdMPRIntersect(&box1, &box2, &ccd);
 
60
 
 
61
        if (i < 35 || i > 65){
 
62
            assertFalse(res);
 
63
        }else if (i != 35 && i != 65){
 
64
            assertTrue(res);
 
65
        }
 
66
 
 
67
        box1.pos.v[0] += 0.01;
 
68
    }
 
69
 
 
70
 
 
71
    box1.x = 1;
 
72
    box1.y = 2;
 
73
    box1.z = 1;
 
74
    box2.x = 2;
 
75
    box2.y = 1;
 
76
    box2.z = 2;
 
77
 
 
78
    ccdVec3Set(&box1.pos, -5., -0.1, 0.);
 
79
    ccdVec3Set(&box2.pos, 0., 0., 0.);
 
80
    ccdQuatSet(&box1.quat, 0., 0., 0., 1.);
 
81
    ccdQuatSet(&box2.quat, 0., 0., 0., 1.);
 
82
    for (i = 0; i < 100; i++){
 
83
        res = ccdMPRIntersect(&box1, &box2, &ccd);
 
84
 
 
85
        if (i < 35 || i > 65){
 
86
            assertFalse(res);
 
87
        }else if (i != 35 && i != 65){
 
88
            assertTrue(res);
 
89
        }
 
90
 
 
91
        box1.pos.v[0] += 0.1;
 
92
    }
 
93
}
 
94
 
 
95
TEST(mprBoxboxAlignedY)
 
96
{
 
97
    size_t i;
 
98
    ccd_t ccd;
 
99
    CCD_BOX(box1);
 
100
    CCD_BOX(box2);
 
101
    int res;
 
102
 
 
103
    CCD_INIT(&ccd);
 
104
    ccd.support1 = ccdSupport;
 
105
    ccd.support2 = ccdSupport;
 
106
    ccd.center1  = ccdObjCenter;
 
107
    ccd.center2  = ccdObjCenter;
 
108
 
 
109
    box1.x = 1;
 
110
    box1.y = 2;
 
111
    box1.z = 1;
 
112
    box2.x = 2;
 
113
    box2.y = 1;
 
114
    box2.z = 2;
 
115
 
 
116
    ccdVec3Set(&box1.pos, 0., -5., 0.);
 
117
    ccdVec3Set(&box2.pos, 0., 0., 0.);
 
118
    ccdQuatSet(&box1.quat, 0., 0., 0., 1.);
 
119
    ccdQuatSet(&box2.quat, 0., 0., 0., 1.);
 
120
    for (i = 0; i < 100; i++){
 
121
        res = ccdMPRIntersect(&box1, &box2, &ccd);
 
122
 
 
123
        if (i < 35 || i > 65){
 
124
            assertFalse(res);
 
125
        }else if (i != 35 && i != 65){
 
126
            assertTrue(res);
 
127
        }
 
128
 
 
129
        box1.pos.v[1] += 0.1;
 
130
    }
 
131
}
 
132
 
 
133
TEST(mprBoxboxAlignedZ)
 
134
{
 
135
    size_t i;
 
136
    ccd_t ccd;
 
137
    CCD_BOX(box1);
 
138
    CCD_BOX(box2);
 
139
    int res;
 
140
 
 
141
    CCD_INIT(&ccd);
 
142
    ccd.support1 = ccdSupport;
 
143
    ccd.support2 = ccdSupport;
 
144
    ccd.center1  = ccdObjCenter;
 
145
    ccd.center2  = ccdObjCenter;
 
146
 
 
147
    box1.x = 1;
 
148
    box1.y = 2;
 
149
    box1.z = 1;
 
150
    box2.x = 2;
 
151
    box2.y = 1;
 
152
    box2.z = 2;
 
153
 
 
154
    ccdVec3Set(&box1.pos, 0., 0., -5.);
 
155
    ccdVec3Set(&box2.pos, 0., 0., 0.);
 
156
    ccdQuatSet(&box1.quat, 0., 0., 0., 1.);
 
157
    ccdQuatSet(&box2.quat, 0., 0., 0., 1.);
 
158
    for (i = 0; i < 100; i++){
 
159
        res = ccdMPRIntersect(&box1, &box2, &ccd);
 
160
 
 
161
        if (i < 35 || i > 65){
 
162
            assertFalse(res);
 
163
        }else if (i != 35 && i != 65){
 
164
            assertTrue(res);
 
165
        }
 
166
 
 
167
        box1.pos.v[2] += 0.1;
 
168
    }
 
169
}
 
170
 
 
171
 
 
172
TEST(mprBoxboxRot)
 
173
{
 
174
    size_t i;
 
175
    ccd_t ccd;
 
176
    CCD_BOX(box1);
 
177
    CCD_BOX(box2);
 
178
    int res;
 
179
    ccd_vec3_t axis;
 
180
    ccd_real_t angle;
 
181
 
 
182
    CCD_INIT(&ccd);
 
183
    ccd.support1 = ccdSupport;
 
184
    ccd.support2 = ccdSupport;
 
185
    ccd.center1  = ccdObjCenter;
 
186
    ccd.center2  = ccdObjCenter;
 
187
 
 
188
    box1.x = 1;
 
189
    box1.y = 2;
 
190
    box1.z = 1;
 
191
    box2.x = 2;
 
192
    box2.y = 1;
 
193
    box2.z = 2;
 
194
 
 
195
    ccdVec3Set(&box1.pos, -5., 0.5, 0.);
 
196
    ccdVec3Set(&box2.pos, 0., 0., 0.);
 
197
    ccdQuatSet(&box2.quat, 0., 0., 0., 1.);
 
198
    ccdVec3Set(&axis, 0., 1., 0.);
 
199
    ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
 
200
 
 
201
    for (i = 0; i < 100; i++){
 
202
        res = ccdMPRIntersect(&box1, &box2, &ccd);
 
203
 
 
204
        if (i < 33 || i > 67){
 
205
            assertFalse(res);
 
206
        }else if (i != 33 && i != 67){
 
207
            assertTrue(res);
 
208
        }
 
209
 
 
210
        box1.pos.v[0] += 0.1;
 
211
    }
 
212
 
 
213
    box1.x = 1;
 
214
    box1.y = 1;
 
215
    box1.z = 1;
 
216
    box2.x = 1;
 
217
    box2.y = 1;
 
218
    box2.z = 1;
 
219
 
 
220
    ccdVec3Set(&box1.pos, -1.01, 0., 0.);
 
221
    ccdVec3Set(&box2.pos, 0., 0., 0.);
 
222
    ccdQuatSet(&box1.quat, 0., 0., 0., 1.);
 
223
    ccdQuatSet(&box2.quat, 0., 0., 0., 1.);
 
224
 
 
225
    ccdVec3Set(&axis, 0., 1., 0.);
 
226
    angle = 0.;
 
227
    for (i = 0; i < 30; i++){
 
228
        res = ccdMPRIntersect(&box1, &box2, &ccd);
 
229
 
 
230
        if (i != 0 && i != 10 && i != 20){
 
231
            assertTrue(res);
 
232
        }else{
 
233
            assertFalse(res);
 
234
        }
 
235
 
 
236
        angle += M_PI / 20.;
 
237
        ccdQuatSetAngleAxis(&box1.quat, angle, &axis);
 
238
    }
 
239
 
 
240
}
 
241
 
 
242
 
 
243
 
 
244
static void pConf(ccd_box_t *box1, ccd_box_t *box2, const ccd_vec3_t *v)
 
245
{
 
246
    fprintf(stdout, "# box1.pos: [%lf %lf %lf]\n",
 
247
            ccdVec3X(&box1->pos), ccdVec3Y(&box1->pos), ccdVec3Z(&box1->pos));
 
248
    fprintf(stdout, "# box1->quat: [%lf %lf %lf %lf]\n",
 
249
            box1->quat.q[0], box1->quat.q[1], box1->quat.q[2], box1->quat.q[3]);
 
250
    fprintf(stdout, "# box2->pos: [%lf %lf %lf]\n",
 
251
            ccdVec3X(&box2->pos), ccdVec3Y(&box2->pos), ccdVec3Z(&box2->pos));
 
252
    fprintf(stdout, "# box2->quat: [%lf %lf %lf %lf]\n",
 
253
            box2->quat.q[0], box2->quat.q[1], box2->quat.q[2], box2->quat.q[3]);
 
254
    fprintf(stdout, "# sep: [%lf %lf %lf]\n",
 
255
            ccdVec3X(v), ccdVec3Y(v), ccdVec3Z(v));
 
256
    fprintf(stdout, "#\n");
 
257
}
 
258
 
 
259
TEST(mprBoxboxSeparate)
 
260
{
 
261
    ccd_t ccd;
 
262
    CCD_BOX(box1);
 
263
    CCD_BOX(box2);
 
264
    int res;
 
265
    ccd_vec3_t sep, expsep, expsep2, axis;
 
266
 
 
267
    fprintf(stderr, "\n\n\n---- boxboxSeparate ----\n\n\n");
 
268
 
 
269
    box1.x = box1.y = box1.z = 1.;
 
270
    box2.x = 0.5;
 
271
    box2.y = 1.;
 
272
    box2.z = 1.5;
 
273
 
 
274
 
 
275
    CCD_INIT(&ccd);
 
276
    ccd.support1 = ccdSupport;
 
277
    ccd.support2 = ccdSupport;
 
278
    ccd.center1  = ccdObjCenter;
 
279
    ccd.center2  = ccdObjCenter;
 
280
 
 
281
    ccdVec3Set(&box1.pos, -0.5, 0.5, 0.2);
 
282
    res = ccdMPRIntersect(&box1, &box2, &ccd);
 
283
    assertTrue(res);
 
284
 
 
285
    res = ccdGJKSeparate(&box1, &box2, &ccd, &sep);
 
286
    assertTrue(res == 0);
 
287
    ccdVec3Set(&expsep, 0.25, 0., 0.);
 
288
    assertTrue(ccdVec3Eq(&sep, &expsep));
 
289
 
 
290
    ccdVec3Scale(&sep, -1.);
 
291
    ccdVec3Add(&box1.pos, &sep);
 
292
    res = ccdGJKSeparate(&box1, &box2, &ccd, &sep);
 
293
    assertTrue(res == 0);
 
294
    ccdVec3Set(&expsep, 0., 0., 0.);
 
295
    assertTrue(ccdVec3Eq(&sep, &expsep));
 
296
 
 
297
 
 
298
    ccdVec3Set(&box1.pos, -0.3, 0.5, 1.);
 
299
    res = ccdGJKSeparate(&box1, &box2, &ccd, &sep);
 
300
    assertTrue(res == 0);
 
301
    ccdVec3Set(&expsep, 0., 0., -0.25);
 
302
    assertTrue(ccdVec3Eq(&sep, &expsep));
 
303
 
 
304
 
 
305
 
 
306
    box1.x = box1.y = box1.z = 1.;
 
307
    box2.x = box2.y = box2.z = 1.;
 
308
    ccdVec3Set(&axis, 0., 0., 1.);
 
309
    ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
 
310
    ccdVec3Set(&box1.pos, 0., 0., 0.);
 
311
 
 
312
    res = ccdGJKSeparate(&box1, &box2, &ccd, &sep);
 
313
    assertTrue(res == 0);
 
314
    ccdVec3Set(&expsep, 0., 0., 1.);
 
315
    ccdVec3Set(&expsep2, 0., 0., -1.);
 
316
    assertTrue(ccdVec3Eq(&sep, &expsep) || ccdVec3Eq(&sep, &expsep2));
 
317
 
 
318
 
 
319
 
 
320
    box1.x = box1.y = box1.z = 1.;
 
321
    ccdVec3Set(&axis, 0., 0., 1.);
 
322
    ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
 
323
    ccdVec3Set(&box1.pos, -0.5, 0., 0.);
 
324
 
 
325
    res = ccdGJKSeparate(&box1, &box2, &ccd, &sep);
 
326
    assertTrue(res == 0);
 
327
    pConf(&box1, &box2, &sep);
 
328
 
 
329
 
 
330
 
 
331
    box1.x = box1.y = box1.z = 1.;
 
332
    ccdVec3Set(&axis, 0., 1., 1.);
 
333
    ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
 
334
    ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4);
 
335
 
 
336
    res = ccdGJKSeparate(&box1, &box2, &ccd, &sep);
 
337
    assertTrue(res == 0);
 
338
    pConf(&box1, &box2, &sep);
 
339
}
 
340
 
 
341
 
 
342
#define TOSVT() \
 
343
    svtObjPen(&box1, &box2, stdout, "Pen 1", depth, &dir, &pos); \
 
344
    ccdVec3Scale(&dir, depth); \
 
345
    ccdVec3Add(&box2.pos, &dir); \
 
346
    svtObjPen(&box1, &box2, stdout, "Pen 1", depth, &dir, &pos)
 
347
 
 
348
TEST(mprBoxboxPenetration)
 
349
{
 
350
    ccd_t ccd;
 
351
    CCD_BOX(box1);
 
352
    CCD_BOX(box2);
 
353
    int res;
 
354
    ccd_vec3_t axis;
 
355
    ccd_quat_t rot;
 
356
    ccd_real_t depth;
 
357
    ccd_vec3_t dir, pos;
 
358
 
 
359
    fprintf(stderr, "\n\n\n---- boxboxPenetration ----\n\n\n");
 
360
 
 
361
    box1.x = box1.y = box1.z = 1.;
 
362
    box2.x = 0.5;
 
363
    box2.y = 1.;
 
364
    box2.z = 1.5;
 
365
 
 
366
 
 
367
    CCD_INIT(&ccd);
 
368
    ccd.support1 = ccdSupport;
 
369
    ccd.support2 = ccdSupport;
 
370
    ccd.center1  = ccdObjCenter;
 
371
    ccd.center2  = ccdObjCenter;
 
372
 
 
373
    /*
 
374
    ccdVec3Set(&box2.pos, 0., 0., 0.);
 
375
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
376
    assertTrue(res == 0);
 
377
    recPen(depth, &dir, &pos, stdout, "Pen 1");
 
378
    TOSVT();
 
379
    */
 
380
 
 
381
    ccdVec3Set(&box2.pos, 0.1, 0., 0.);
 
382
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
383
    assertTrue(res == 0);
 
384
    recPen(depth, &dir, &pos, stdout, "Pen 1");
 
385
    //TOSVT();
 
386
 
 
387
 
 
388
    ccdVec3Set(&box1.pos, -0.3, 0.5, 1.);
 
389
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
390
    assertTrue(res == 0);
 
391
    recPen(depth, &dir, &pos, stdout, "Pen 2");
 
392
    //TOSVT();
 
393
 
 
394
 
 
395
    box1.x = box1.y = box1.z = 1.;
 
396
    box2.x = box2.y = box2.z = 1.;
 
397
    ccdVec3Set(&axis, 0., 0., 1.);
 
398
    ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
 
399
    ccdVec3Set(&box1.pos, 0.1, 0., 0.1);
 
400
 
 
401
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
402
    assertTrue(res == 0);
 
403
    recPen(depth, &dir, &pos, stdout, "Pen 3");
 
404
    //TOSVT();
 
405
 
 
406
 
 
407
    box1.x = box1.y = box1.z = 1.;
 
408
    box2.x = box2.y = box2.z = 1.;
 
409
    ccdVec3Set(&axis, 0., 0., 1.);
 
410
    ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
 
411
    ccdVec3Set(&box1.pos, -0.5, 0., 0.);
 
412
 
 
413
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
414
    assertTrue(res == 0);
 
415
    recPen(depth, &dir, &pos, stdout, "Pen 4");
 
416
    //TOSVT();
 
417
 
 
418
 
 
419
    box1.x = box1.y = box1.z = 1.;
 
420
    box2.x = box2.y = box2.z = 1.;
 
421
    ccdVec3Set(&axis, 0., 0., 1.);
 
422
    ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
 
423
    ccdVec3Set(&box1.pos, -0.5, 0.5, 0.);
 
424
 
 
425
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
426
    assertTrue(res == 0);
 
427
    recPen(depth, &dir, &pos, stdout, "Pen 5");
 
428
    //TOSVT();
 
429
 
 
430
 
 
431
    box1.x = box1.y = box1.z = 1.;
 
432
    box2.x = box2.y = box2.z = 1.;
 
433
    ccdVec3Set(&box2.pos, 0.1, 0., 0.);
 
434
 
 
435
    box1.x = box1.y = box1.z = 1.;
 
436
    ccdVec3Set(&axis, 0., 1., 1.);
 
437
    ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
 
438
    ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4);
 
439
 
 
440
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
441
    assertTrue(res == 0);
 
442
    recPen(depth, &dir, &pos, stdout, "Pen 6");
 
443
    //TOSVT();
 
444
 
 
445
 
 
446
    box1.x = box1.y = box1.z = 1.;
 
447
    ccdVec3Set(&axis, 0., 1., 1.);
 
448
    ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
 
449
    ccdVec3Set(&axis, 1., 1., 1.);
 
450
    ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis);
 
451
    ccdQuatMul(&box1.quat, &rot);
 
452
    ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4);
 
453
 
 
454
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
455
    assertTrue(res == 0);
 
456
    recPen(depth, &dir, &pos, stdout, "Pen 7");
 
457
    //TOSVT();
 
458
 
 
459
 
 
460
    box1.x = box1.y = box1.z = 1.;
 
461
    box2.x = 0.2; box2.y = 0.5; box2.z = 1.;
 
462
    box2.x = box2.y = box2.z = 1.;
 
463
 
 
464
    ccdVec3Set(&axis, 0., 0., 1.);
 
465
    ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
 
466
    ccdVec3Set(&axis, 1., 0., 0.);
 
467
    ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis);
 
468
    ccdQuatMul(&box1.quat, &rot);
 
469
    ccdVec3Set(&box1.pos, -1.3, 0., 0.);
 
470
 
 
471
    ccdVec3Set(&box2.pos, 0., 0., 0.);
 
472
 
 
473
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
474
    assertTrue(res == 0);
 
475
    recPen(depth, &dir, &pos, stdout, "Pen 8");
 
476
    //TOSVT();
 
477
 
 
478
 
 
479
    box1.x = box1.y = box1.z = 1.;
 
480
    box2.x = 0.5; box2.y = 0.5; box2.z = .5;
 
481
    ccdVec3Set(&box1.pos, 0., 0., 0.);
 
482
    ccdQuatSet(&box1.quat, 0., 0., 0., 1.);
 
483
    ccdVec3Set(&box2.pos, 0., 0.73, 0.);
 
484
    ccdQuatSet(&box2.quat, 0., 0., 0., 1.);
 
485
 
 
486
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
487
    assertTrue(res == 0);
 
488
    recPen(depth, &dir, &pos, stdout, "Pen 9");
 
489
    //TOSVT();
 
490
 
 
491
    box1.x = box1.y = box1.z = 1.;
 
492
    box2.x = 0.5; box2.y = 0.5; box2.z = .5;
 
493
    ccdVec3Set(&box1.pos, 0., 0., 0.);
 
494
    ccdQuatSet(&box1.quat, 0., 0., 0., 1.);
 
495
    ccdVec3Set(&box2.pos, 0.3, 0.738, 0.);
 
496
    ccdQuatSet(&box2.quat, 0., 0., 0., 1.);
 
497
 
 
498
    res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos);
 
499
    assertTrue(res == 0);
 
500
    recPen(depth, &dir, &pos, stdout, "Pen 10");
 
501
    //TOSVT();
 
502
}