1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Radu Serban
13 // =============================================================================
14 //
15 // Chrono unit test for narrow phase type PRIMS collision detection
16 // =============================================================================
17
18 #include "chrono/collision/chrono/ChNarrowphase.h"
19 #include "chrono/collision/chrono/ChCollisionUtils.h"
20
21 #include "gtest/gtest.h"
22
23 #include "unit_testing.h"
24
25 using namespace chrono;
26 using namespace chrono::collision;
27 using namespace chrono::collision::ch_utils;
28
29 using std::cout;
30 using std::endl;
31
32 #ifdef CHRONO_MULTICORE_USE_DOUBLE
33 const double precision = 1e-10;
34 #else
35 const float precision = 1e-6f;
36 #endif
37
38 // =============================================================================
39 // Tests for various utility functions
40 // =============================================================================
41
TEST(ChNarrowphasePRIMS,snap_to_box)42 TEST(ChNarrowphasePRIMS, snap_to_box) {
43 real3 hdims(1.0, 2.0, 3.0);
44
45 {
46 // interior point
47 real3 loc(0.5, -1.0, 1.5);
48 int code = snap_to_box(hdims, loc);
49 ASSERT_EQ(code, 0);
50 Assert_eq(loc, real3(0.5, -1.0, 1.5));
51 }
52
53 {
54 // face point
55 real3 loc(0.5, -1.0, -3.5);
56 int code = snap_to_box(hdims, loc);
57 ASSERT_EQ(code, 4);
58 Assert_eq(loc, real3(0.5, -1.0, -3.0));
59 }
60
61 {
62 // edge point
63 real3 loc(0.5, -2.5, -3.5);
64 int code = snap_to_box(hdims, loc);
65 ASSERT_EQ(code, 6);
66 Assert_eq(loc, real3(0.5, -2.0, -3.0));
67 }
68
69 {
70 // corner point
71 real3 loc(1.5, -2.5, -3.5);
72 int code = snap_to_box(hdims, loc);
73 ASSERT_EQ(code, 7);
74 Assert_eq(loc, real3(1.0, -2.0, -3.0));
75 }
76 }
77
TEST(ChNarrowphasePRIMS,snap_to_cylinder)78 TEST(ChNarrowphasePRIMS, snap_to_cylinder) {
79 real rad = 2;
80 real hlen = 1.5;
81
82 {
83 // interior point
84 real3 loc(0.5, -1.0, 1.5);
85 int code = snap_to_cylinder(rad, hlen, loc);
86 ASSERT_EQ(code, 0);
87 Assert_eq(loc, real3(0.5, -1.0, 1.5));
88 }
89
90 {
91 // cap point
92 real3 loc(0.5, 2.0, 1.5);
93 int code = snap_to_cylinder(rad, hlen, loc);
94 ASSERT_EQ(code, 1);
95 Assert_eq(loc, real3(0.5, 1.5, 1.5));
96 }
97
98 {
99 // side point
100 real3 loc(2.0, 0.5, 1.0);
101 int code = snap_to_cylinder(rad, hlen, loc);
102 ASSERT_EQ(code, 2);
103 Assert_near(loc, real3(4 / sqrt(5.0), 0.5, 2 / sqrt(5.0)), precision);
104 }
105
106 {
107 // edge point
108 real3 loc(2.0, 2.0, 1.0);
109 int code = snap_to_cylinder(rad, hlen, loc);
110 ASSERT_EQ(code, 3);
111 Assert_near(loc, real3(4 / sqrt(5.0), 1.5, 2 / sqrt(5.0)), precision);
112 }
113 }
114
TEST(ChNarrowphasePRIMS,snap_to_box_face)115 TEST(ChNarrowphasePRIMS, snap_to_box_face) {
116 {
117 {
118 // z dir / in range
119 real3 pt_on_box(1.0, 2.0, 3.0);
120 real3 hdims(1.0, 2.0, 3.0);
121 real3 pt_to_snap(0.1, 0.3, 2.0);
122 uint code = 1 << 2;
123 real3 result = snap_to_box_face(hdims, pt_on_box, code, pt_to_snap);
124 Assert_near(result, real3(0.1, 0.3, 3.0), precision);
125 }
126
127 {
128 // z dir / out of range
129 real3 pt_on_box(1.0, 2.0, 3.0);
130 real3 hdims(1.0, 2.0, 3.0);
131 real3 pt_to_snap(1.7, -2.5, 4.0);
132 uint code = 1 << 2;
133 real3 result = snap_to_box_face(hdims, pt_on_box, code, pt_to_snap);
134 Assert_near(result, real3(1.0, -2.0, 3.0), precision);
135 }
136 }
137 }
138
TEST(ChNarrowphasePRIMS,get_face_corners)139 TEST(ChNarrowphasePRIMS, get_face_corners) {
140 {
141 {
142 // dir x / inside
143 real3 pt_on_box(1.0, 2.0, 3.0);
144 real3 hdims(1.0, 2.0, 3.0);
145 uint code = 1 << 0;
146 real3 corners[4];
147
148 get_face_corners(pt_on_box, code, corners);
149 Assert_near(corners[0], real3(1.0, 2.0, 3.0), precision);
150 Assert_near(corners[1], real3(1.0, -2.0, 3.0), precision);
151 Assert_near(corners[2], real3(1.0, -2.0, -3.0), precision);
152 Assert_near(corners[3], real3(1.0, 2.0, -3.0), precision);
153 }
154
155 {
156 // dir y / inside
157 real3 pt_on_box(1.0, 2.0, 3.0);
158 real3 hdims(1.0, 2.0, 3.0);
159 uint code = 1 << 1;
160 real3 corners[4];
161
162 get_face_corners(pt_on_box, code, corners);
163 Assert_near(corners[0], real3(1.0, 2.0, 3.0), precision);
164 Assert_near(corners[1], real3(1.0, 2.0, -3.0), precision);
165 Assert_near(corners[2], real3(-1.0, 2.0, -3.0), precision);
166 Assert_near(corners[3], real3(-1.0, 2.0, 3.0), precision);
167 }
168
169 {
170 // dir z / inside
171 real3 pt_on_box(1.0, 2.0, 3.0);
172 real3 hdims(1.0, 2.0, 3.0);
173 uint code = 1 << 2;
174 real3 corners[4];
175
176 get_face_corners(pt_on_box, code, corners);
177 Assert_near(corners[0], real3(1, 2, 3), precision);
178 Assert_near(corners[1], real3(-1, 2, 3), precision);
179 Assert_near(corners[2], real3(-1, -2, 3), precision);
180 Assert_near(corners[3], real3(1, -2, 3), precision);
181 }
182 }
183 }
184
TEST(ChNarrowphasePRIMS,get_edge_corners)185 TEST(ChNarrowphasePRIMS, get_edge_corners) {
186 {
187 {
188 // dir x
189 real3 pt_on_box(1, 2, 3);
190 real3 hdims(1, 2, 3);
191 uint code = 6;
192 real3 corners[4];
193
194 get_edge_corners(pt_on_box, code, corners);
195 Assert_near(corners[0], real3(1, 2, 3), precision);
196 Assert_near(corners[1], real3(-1, 2, 3), precision);
197 }
198
199 {
200 // dir y
201 real3 pt_on_box(1, -2, -3);
202 real3 hdims(1, 2, 3);
203 uint code = 5;
204 real3 corners[4];
205
206 get_edge_corners(pt_on_box, code, corners);
207 Assert_near(corners[0], real3(1, -2, -3), precision);
208 Assert_near(corners[1], real3(1, 2, -3), precision);
209 }
210
211 {
212 // dir z
213 real3 pt_on_box(-1, 2, -3);
214 real3 hdims(1, 2, 3);
215 uint code = 3;
216 real3 corners[4];
217
218 get_edge_corners(pt_on_box, code, corners);
219 Assert_near(corners[0], real3(-1, 2, -3), precision);
220 Assert_near(corners[1], real3(-1, 2, 3), precision);
221 }
222 }
223 }
224
TEST(ChNarrowphasePRIMS,point_vs_face)225 TEST(ChNarrowphasePRIMS, point_vs_face) {
226 real3 result;
227 real3 normal;
228 real dist;
229
230 {
231 // dir x | valid contact | separation = 0
232 real3 pt_on_face(-0.9, 2, 3);
233 real3 hdims(1, 2, 3);
234 real3 pt_to_snap(-0.9, 1.4, 2);
235
236 ASSERT_TRUE(point_vs_face(hdims, pt_on_face, 1, pt_to_snap, 0.0, result, normal, dist));
237 Assert_near(result, real3(-1, 1.4, 2), precision);
238 Assert_near(normal, real3(-1, 0, 0), precision);
239 ASSERT_NEAR(dist, -0.1, precision);
240 }
241
242 {
243 // dir y | valid contact
244 real3 pt_on_face(1, 2, 3);
245 real3 hdims(1, 2, 3);
246 real3 pt_to_snap(0.5, 1.4, 2);
247
248 ASSERT_TRUE(point_vs_face(hdims, pt_on_face, 2, pt_to_snap, 0.0, result, normal, dist));
249 Assert_near(result, real3(0.5, 2, 2), precision);
250 Assert_near(normal, real3(0, 1, 0), precision);
251 ASSERT_NEAR(dist, -0.6, precision);
252 }
253
254 {
255 // dir z | valid contact
256 real3 pt_on_face(1, 2, 3);
257 real3 hdims(1, 2, 3);
258 real3 pt_to_snap(0.4, 0.8, 2.5);
259
260 ASSERT_TRUE(point_vs_face(hdims, pt_on_face, 4, pt_to_snap, 0.0, result, normal, dist));
261 Assert_near(result, real3(0.4, 0.8, 3), precision);
262 Assert_near(normal, real3(0, 0, 1), precision);
263 ASSERT_NEAR(dist, -0.5, precision);
264 }
265
266 {
267 // dir x | invalid contact
268 real3 pt_on_face(1, 2, 3);
269 real3 hdims(1, 2, 3);
270 real3 pt_to_snap(1.1, 1.4, 2);
271
272 ASSERT_FALSE(point_vs_face(hdims, pt_on_face, 1, pt_to_snap, 0.0, result, normal, dist));
273 }
274
275 {
276 // dir y | invalid contact
277 real3 pt_on_face(1, 2, 3);
278 real3 hdims(1, 2, 3);
279 real3 pt_to_snap(0.5, 2.4, 2);
280
281 ASSERT_FALSE(point_vs_face(hdims, pt_on_face, 2, pt_to_snap, 0.0, result, normal, dist));
282 }
283
284 {
285 // dir z | invalid contact
286 real3 pt_on_face(1, 2, 3);
287 real3 hdims(1, 2, 3);
288 real3 pt_to_snap(1.4, 0.8, 2.5);
289
290 ASSERT_FALSE(point_vs_face(hdims, pt_on_face, 4, pt_to_snap, 0.0, result, normal, dist));
291 }
292 }
293
TEST(ChNarrowphasePRIMS,segment_vs_edge)294 TEST(ChNarrowphasePRIMS, segment_vs_edge) {
295 real3 loc1;
296 real3 loc2;
297 {
298 real3 pt_on_edge(1, 2, 3);
299 real3 hdims(1, 2, 3);
300 real3 pt_to_snap(0.4, 0.8, 2.5);
301 real3 pt_1(0.5, 0.2, 2.5);
302 real3 pt_2(1.5, 0.2, 2.5);
303
304 ASSERT_TRUE(segment_vs_edge(hdims, pt_on_edge, 5, pt_1, pt_2, loc1, loc2));
305 Assert_near(loc1, real3(1, 0.2, 3), precision);
306 Assert_near(loc2, real3(1, 0.2, 2.5), precision);
307 }
308 }
309
310 // =============================================================================
311 // Utility wrappers
312 // =============================================================================
313
CheckValueList(real a[],const std::vector<real> & ref)314 void CheckValueList(real a[], const std::vector<real>& ref) {
315 Assert_near(std::vector<real>(a, a + ref.size()), ref, precision);
316 }
317
CheckValueList(real a[],int n,const real & ref)318 void CheckValueList(real a[], int n, const real& ref) {
319 CheckValueList(a, std::vector<real>(n, ref));
320 }
321
CheckPointList(real3 a[],const std::vector<real3> & ref)322 void CheckPointList(real3 a[], const std::vector<real3>& ref) {
323 Assert_near(std::vector<real3>(a, a + ref.size()), ref, precision);
324 }
325
CheckPointList(real3 a[],int n,const real3 & ref)326 void CheckPointList(real3 a[], int n, const real3& ref) {
327 CheckPointList(a, std::vector<real3>(n, ref));
328 }
329
330 // =============================================================================
331 // Tests for various primitive collision functions
332 // =============================================================================
333
334 class Collision : public ::testing::Test, public ::testing::WithParamInterface<bool> {
335 public:
Collision()336 Collision() : sep(GetParam()) {}
337
338 protected:
339 bool sep;
340 };
341
TEST_P(Collision,box_box)342 TEST_P(Collision, box_box) {
343 real separation = sep ? 0.1 : 0.0;
344 real penetration;
345 real3 norm[8];
346 real3 pt1[8];
347 real3 pt2[8];
348 real depth[8];
349 real eff_rad[8];
350 int nC = 0;
351
352 // face to face | stack
353 {
354 real3 hdims1(1.0, 2.0, 3.0);
355 real3 pos1(0.0, 0.0, 0.0);
356 quaternion rot1 = quaternion(1, 0, 0, 0);
357
358 real3 hdims2(1.0, 2.0, 3.0);
359 real3 pos2(0.0, 0.0, 6.0);
360 quaternion rot2 = quaternion(1, 0, 0, 0);
361
362 ConvexShapeCustom* shape1 = new ConvexShapeCustom();
363 shape1->type = ChCollisionShape::Type::BOX;
364 shape1->position = pos1;
365 shape1->dimensions = hdims1;
366 shape1->rotation = rot1;
367
368 ConvexShapeCustom* shape2 = new ConvexShapeCustom();
369 shape2->type = ChCollisionShape::Type::BOX;
370 shape2->position = pos2;
371 shape2->dimensions = hdims2;
372 shape2->rotation = rot2;
373
374 // penetrated
375 penetration = -0.05;
376 shape2->position = pos2 + real3(0, 0, penetration);
377 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
378 ASSERT_EQ(nC, 8);
379 CheckValueList(depth, nC, penetration);
380 CheckPointList(norm, nC, real3(0, 0, 1));
381
382 // penetrated, small penetration
383 penetration = -1e-5;
384 shape2->position = pos2 + real3(0, 0, penetration);
385 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
386 ASSERT_EQ(nC, 8);
387 CheckValueList(depth, nC, penetration);
388 CheckPointList(norm, nC, real3(0, 0, 1));
389
390 // separated by less than 'separation'
391 penetration = +0.05;
392 shape2->position = pos2 + real3(0, 0, penetration);
393 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
394 if (sep) {
395 ASSERT_EQ(nC, 8);
396 CheckValueList(depth, nC, penetration);
397 CheckPointList(norm, nC, real3(0, 0, 1));
398 } else {
399 ASSERT_EQ(nC, 0);
400 }
401
402 // separated by more than 'separation'
403 penetration = +0.15;
404 shape2->position = pos2 + real3(0, 0, penetration);
405 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
406 ASSERT_EQ(nC, 0);
407 }
408
409 // face to face | stack with rotation
410 {
411 real3 hdims1(1.0, 2.0, 3.0);
412 real3 pos1(0.0, 0.0, 0.0);
413 quaternion rot1 = quaternion(1, 0, 0, 0);
414
415 real3 hdims2(1.0, 2.0, 3.0);
416 real3 pos2(0.0, 0.0, 6.0);
417 quaternion rot2 = FromChQuaternion(Q_from_AngZ(CH_C_PI_4));
418
419 ConvexShapeCustom* shape1 = new ConvexShapeCustom();
420 shape1->type = ChCollisionShape::Type::BOX;
421 shape1->position = pos1;
422 shape1->dimensions = hdims1;
423 shape1->rotation = rot1;
424
425 ConvexShapeCustom* shape2 = new ConvexShapeCustom();
426 shape2->type = ChCollisionShape::Type::BOX;
427 shape2->position = pos2;
428 shape2->dimensions = hdims2;
429 shape2->rotation = rot2;
430
431 // penetrated
432 penetration = -0.05;
433 shape2->position = pos2 + real3(0, 0, penetration);
434 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
435 ASSERT_EQ(nC, 8);
436 CheckValueList(depth, nC, penetration);
437 CheckPointList(norm, nC, real3(0, 0, 1));
438
439 // penetrated, small penetration
440 penetration = -1e-5;
441 shape2->position = pos2 + real3(0, 0, penetration);
442 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
443 ASSERT_EQ(nC, 8);
444 CheckValueList(depth, nC, penetration);
445 CheckPointList(norm, nC, real3(0, 0, 1));
446
447 // separated by less than 'separation'
448 penetration = +0.05;
449 shape2->position = pos2 + real3(0, 0, penetration);
450 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
451 if (sep) {
452 ASSERT_EQ(nC, 8);
453 CheckValueList(depth, nC, penetration);
454 CheckPointList(norm, nC, real3(0, 0, 1));
455 } else {
456 ASSERT_EQ(nC, 0);
457 }
458
459 // separated by more than 'separation'
460 penetration = +0.15;
461 shape2->position = pos2 + real3(0, 0, penetration);
462 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
463 ASSERT_EQ(nC, 0);
464 }
465
466 // face to face | stack with rotation and offset
467 // 2 corners of top box on edges of bottom box
468 {
469 real3 hdims1(2.0, 2.0, 3.0);
470 real3 pos1(0.0, 0.0, 0.0);
471 quaternion rot1 = quaternion(1, 0, 0, 0);
472
473 real3 hdims2(2.0, 2.0, 3.0);
474 real3 pos2(2.0, 2.0, 6.0);
475 quaternion rot2 = FromChQuaternion(Q_from_AngZ(CH_C_PI_4));
476
477 ConvexShapeCustom* shape1 = new ConvexShapeCustom();
478 shape1->type = ChCollisionShape::Type::BOX;
479 shape1->position = pos1;
480 shape1->dimensions = hdims1;
481 shape1->rotation = rot1;
482
483 ConvexShapeCustom* shape2 = new ConvexShapeCustom();
484 shape2->type = ChCollisionShape::Type::BOX;
485 shape2->position = pos2;
486 shape2->dimensions = hdims2;
487 shape2->rotation = rot2;
488
489 // penetrated
490 penetration = -0.05;
491 shape2->position = pos2 + real3(0, 0, penetration);
492 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
493 ASSERT_EQ(nC, 3);
494 CheckValueList(depth, nC, penetration);
495 CheckPointList(norm, nC, real3(0, 0, 1));
496 CheckPointList(pt1, {real3(2, 2 - 2 * sqrt(2.0), 3), real3(2 - 2 * sqrt(2.0), 2, 3), real3(2, 2, 3)});
497 CheckPointList(pt2, {real3(2, 2 - 2 * sqrt(2.0), 3 + penetration), real3(2 - 2 * sqrt(2.0), 2, 3 + penetration),
498 real3(2, 2, 3 + penetration)});
499
500 // penetrated, small penetration
501 penetration = -1e-5;
502 shape2->position = pos2 + real3(0, 0, penetration);
503 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
504 ASSERT_EQ(nC, 3);
505 CheckValueList(depth, nC, penetration);
506 CheckPointList(norm, nC, real3(0, 0, 1));
507 CheckPointList(pt1, {real3(2, 2 - 2 * sqrt(2.0), 3), real3(2 - 2 * sqrt(2.0), 2, 3), real3(2, 2, 3)});
508 CheckPointList(pt2, {real3(2, 2 - 2 * sqrt(2.0), 3 + penetration), real3(2 - 2 * sqrt(2.0), 2, 3 + penetration),
509 real3(2, 2, 3 + penetration)});
510
511 // separated by less than 'separation'
512 penetration = +0.05;
513 shape2->position = pos2 + real3(0, 0, penetration);
514 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
515 if (sep) {
516 ASSERT_EQ(nC, 3);
517 CheckValueList(depth, nC, penetration);
518 CheckPointList(norm, nC, real3(0, 0, 1));
519 CheckPointList(pt1, {real3(2, 2 - 2 * sqrt(2.0), 3), real3(2 - 2 * sqrt(2.0), 2, 3), real3(2, 2, 3)});
520 CheckPointList(pt2, {real3(2, 2 - 2 * sqrt(2.0), 3 + penetration),
521 real3(2 - 2 * sqrt(2.0), 2, 3 + penetration), real3(2, 2, 3 + penetration)});
522 } else {
523 ASSERT_EQ(nC, 0);
524 }
525
526 // separated by more than 'separation'
527 penetration = +0.15;
528 shape2->position = pos2 + real3(0, 0, penetration);
529 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
530 ASSERT_EQ(nC, 0);
531 }
532
533 // face to face | stack with rotation and offset
534 // edge-edge interactions
535 {
536 real3 hdims1(2.0, 2.0, 3.0);
537 real3 pos1(0.0, 0.0, 0.0);
538 quaternion rot1 = quaternion(1, 0, 0, 0);
539
540 real3 hdims2(2.0, 2.0, 3.0);
541 real3 pos2(2 * sqrt(2.0), 2 * sqrt(2.0), 6.0);
542 quaternion rot2 = FromChQuaternion(Q_from_AngZ(CH_C_PI_4));
543
544 ConvexShapeCustom* shape1 = new ConvexShapeCustom();
545 shape1->type = ChCollisionShape::Type::BOX;
546 shape1->position = pos1;
547 shape1->dimensions = hdims1;
548 shape1->rotation = rot1;
549
550 ConvexShapeCustom* shape2 = new ConvexShapeCustom();
551 shape2->type = ChCollisionShape::Type::BOX;
552 shape2->position = pos2;
553 shape2->dimensions = hdims2;
554 shape2->rotation = rot2;
555
556 // penetrated
557 penetration = -0.05;
558 shape2->position = pos2 + real3(0, 0, penetration);
559 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
560 ASSERT_EQ(nC, 3);
561 CheckValueList(depth, nC, penetration);
562 CheckPointList(norm, nC, real3(0, 0, 1));
563 CheckPointList(pt1, {real3(2, 2 * sqrt(2.0) - 2, 3), real3(2 * sqrt(2.0) - 2, 2, 3), real3(2, 2, 3)});
564 CheckPointList(pt2, {real3(2, 2 * sqrt(2.0) - 2, 3 + penetration), real3(2 * sqrt(2.0) - 2, 2, 3 + penetration),
565 real3(2, 2, 3 + penetration)});
566
567 // penetrated, small penetration
568 penetration = -1e-5;
569 shape2->position = pos2 + real3(0, 0, penetration);
570 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
571 ASSERT_EQ(nC, 3);
572 CheckValueList(depth, nC, penetration);
573 CheckPointList(norm, nC, real3(0, 0, 1));
574 CheckPointList(pt1, {real3(2, 2 * sqrt(2.0) - 2, 3), real3(2 * sqrt(2.0) - 2, 2, 3), real3(2, 2, 3)});
575 CheckPointList(pt2, {real3(2, 2 * sqrt(2.0) - 2, 3 + penetration), real3(2 * sqrt(2.0) - 2, 2, 3 + penetration),
576 real3(2, 2, 3 + penetration)});
577
578 // separated by less than 'separation'
579 penetration = +0.05;
580 shape2->position = pos2 + real3(0, 0, penetration);
581 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
582 if (sep) {
583 ASSERT_EQ(nC, 3);
584 CheckValueList(depth, nC, penetration);
585 CheckPointList(norm, nC, real3(0, 0, 1));
586 CheckPointList(pt1, {real3(2, 2 * sqrt(2.0) - 2, 3), real3(2 * sqrt(2.0) - 2, 2, 3), real3(2, 2, 3)});
587 CheckPointList(pt2, {real3(2, 2 * sqrt(2.0) - 2, 3 + penetration),
588 real3(2 * sqrt(2.0) - 2, 2, 3 + penetration), real3(2, 2, 3 + penetration)});
589 } else {
590 ASSERT_EQ(nC, 0);
591 }
592
593 // separated by more than 'separation'
594 penetration = +0.15;
595 shape2->position = pos2 + real3(0, 0, penetration);
596 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
597 ASSERT_EQ(nC, 0);
598 }
599
600 // face to face | small on large, in range
601 {
602 real3 hdims1(1.0, 2.0, 3.0);
603 real3 pos1(0.0, 0.0, 0.0);
604 quaternion rot1 = quaternion(1, 0, 0, 0);
605
606 real3 hdims2(0.5, 1.0, 1.0);
607 real3 pos2(0.0, 0.0, 4.0);
608 quaternion rot2 = quaternion(1, 0, 0, 0);
609
610 ConvexShapeCustom* shape1 = new ConvexShapeCustom();
611 shape1->type = ChCollisionShape::Type::BOX;
612 shape1->position = pos1;
613 shape1->dimensions = hdims1;
614 shape1->rotation = rot1;
615
616 ConvexShapeCustom* shape2 = new ConvexShapeCustom();
617 shape2->type = ChCollisionShape::Type::BOX;
618 shape2->position = pos2;
619 shape2->dimensions = hdims2;
620 shape2->rotation = rot2;
621
622 // penetrated
623 penetration = -0.05;
624 shape2->position = pos2 + real3(0, 0, penetration);
625 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
626 ASSERT_EQ(nC, 4);
627 CheckValueList(depth, nC, penetration);
628 CheckPointList(norm, nC, real3(0, 0, 1));
629 CheckPointList(pt1, {real3(-0.5, -1, 3), real3(-0.5, 1, 3), real3(0.5, -1, 3), real3(0.5, 1, 3)});
630 CheckPointList(pt2, {real3(-0.5, -1, 3 + penetration), real3(-0.5, 1, 3 + penetration),
631 real3(0.5, -1, 3 + penetration), real3(0.5, 1, 3 + penetration)});
632
633 // penetrated, small penetration
634 penetration = -1e-5;
635 shape2->position = pos2 + real3(0, 0, penetration);
636 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
637 ASSERT_EQ(nC, 4);
638 CheckValueList(depth, nC, penetration);
639 CheckPointList(norm, nC, real3(0, 0, 1));
640 CheckPointList(pt1, {real3(-0.5, -1, 3), real3(-0.5, 1, 3), real3(0.5, -1, 3), real3(0.5, 1, 3)});
641 CheckPointList(pt2, {real3(-0.5, -1, 3 + penetration), real3(-0.5, 1, 3 + penetration),
642 real3(0.5, -1, 3 + penetration), real3(0.5, 1, 3 + penetration)});
643
644 // separated by less than 'separation'
645 penetration = +0.05;
646 shape2->position = pos2 + real3(0, 0, penetration);
647 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
648 if (sep) {
649 ASSERT_EQ(nC, 4);
650 CheckValueList(depth, nC, penetration);
651 CheckPointList(norm, nC, real3(0, 0, 1));
652 CheckPointList(pt1, {real3(-0.5, -1, 3), real3(-0.5, 1, 3), real3(0.5, -1, 3), real3(0.5, 1, 3)});
653 CheckPointList(pt2, {real3(-0.5, -1, 3 + penetration), real3(-0.5, 1, 3 + penetration),
654 real3(0.5, -1, 3 + penetration), real3(0.5, 1, 3 + penetration)});
655 } else {
656 ASSERT_EQ(nC, 0);
657 }
658
659 // separated by more than 'separation'
660 penetration = +0.15;
661 shape2->position = pos2 + real3(0, 0, penetration);
662 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
663 ASSERT_EQ(nC, 0);
664 }
665
666 // face to edge | edge ends interacting with face
667 // one large box at the bottom and a smaller one on top (rotated pi/4 about x)
668 {
669 real3 hdims1(1.0, 1.0, 1.0);
670 real3 pos1(0.0, 0.0, 2.0 + 1.0 * sqrt(2.0));
671 quaternion rot1 = FromChQuaternion(Q_from_AngX(CH_C_PI / 4));
672
673 real3 hdims2(2.0, 2.0, 2.0);
674 real3 pos2(0.0, 0.0, 0.0);
675 quaternion rot2 = quaternion(1, 0, 0, 0);
676
677 ConvexShapeCustom* shape1 = new ConvexShapeCustom();
678 shape1->type = ChCollisionShape::Type::BOX;
679 shape1->position = pos1;
680 shape1->dimensions = hdims1;
681 shape1->rotation = rot1;
682
683 ConvexShapeCustom* shape2 = new ConvexShapeCustom();
684 shape2->type = ChCollisionShape::Type::BOX;
685 shape2->position = pos2;
686 shape2->dimensions = hdims2;
687 shape2->rotation = rot2;
688
689 // penetrated
690 penetration = -0.05;
691 shape1->position = pos1 + real3(0, 0, penetration);
692 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
693 ASSERT_EQ(nC, 2);
694 CheckValueList(depth, nC, penetration);
695 CheckPointList(norm, nC, real3(0, 0, -1));
696 CheckPointList(pt1, {real3(-1, 0, 2 + penetration), real3(1, 0, 2 + penetration)});
697 CheckPointList(pt2, {real3(-1, 0, 2), real3(1, 0, 2)});
698
699 // penetrated, small penetration
700 penetration = -1e-5;
701 shape1->position = pos1 + real3(0, 0, penetration);
702 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
703 ASSERT_EQ(nC, 2);
704 CheckValueList(depth, nC, penetration);
705 CheckPointList(norm, nC, real3(0, 0, -1));
706 CheckPointList(pt1, {real3(-1, 0, 2 + penetration), real3(1, 0, 2 + penetration)});
707 CheckPointList(pt2, {real3(-1, 0, 2), real3(1, 0, 2)});
708
709 // separated by less than 'separation'
710 penetration = +0.05;
711 shape1->position = pos1 + real3(0, 0, penetration);
712 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
713 if (sep) {
714 ASSERT_EQ(nC, 2);
715 CheckValueList(depth, nC, penetration);
716 CheckPointList(norm, nC, real3(0, 0, -1));
717 CheckPointList(pt1, {real3(-1, 0, 2 + penetration), real3(1, 0, 2 + penetration)});
718 CheckPointList(pt2, {real3(-1, 0, 2), real3(1, 0, 2)});
719 } else {
720 ASSERT_EQ(nC, 0);
721 }
722
723 // separated by more than 'separation'
724 penetration = +0.15;
725 shape1->position = pos1 + real3(0, 0, penetration);
726 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
727 ASSERT_EQ(nC, 0);
728 }
729
730 // face to edge | edge middle interacting with face
731 // one large box at the bottom (rotated pi/4 about z) and a smaller one on top (rotated pi/4 about x)
732 // translate small box in x and y so that its bottom edge intersects two edges of bottom box
733 {
734 real3 hdims1(1.0, 1.0, 1.0);
735 real3 pos1(sqrt(2.0), sqrt(2.0), 2.0 + 1.0 * sqrt(2.0));
736 quaternion rot1 = FromChQuaternion(Q_from_AngX(CH_C_PI / 4));
737
738 real3 hdims2(2.0, 2.0, 2.0);
739 real3 pos2(0.0, 0.0, 0.0);
740 quaternion rot2 = FromChQuaternion(Q_from_AngZ(CH_C_PI / 4));
741
742 ConvexShapeCustom* shape1 = new ConvexShapeCustom();
743 shape1->type = ChCollisionShape::Type::BOX;
744 shape1->position = pos1;
745 shape1->dimensions = hdims1;
746 shape1->rotation = rot1;
747
748 ConvexShapeCustom* shape2 = new ConvexShapeCustom();
749 shape2->type = ChCollisionShape::Type::BOX;
750 shape2->position = pos2;
751 shape2->dimensions = hdims2;
752 shape2->rotation = rot2;
753
754 // penetrated
755 penetration = -0.05;
756 shape1->position = pos1 + real3(0, 0, penetration);
757 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
758 ASSERT_EQ(nC, 2);
759 CheckValueList(depth, nC, penetration);
760 CheckPointList(norm, nC, real3(0, 0, -1));
761 CheckPointList(
762 pt1, {real3(sqrt(2.0) - 1, sqrt(2.0), 2 + penetration), real3(sqrt(2.0), sqrt(2.0), 2 + penetration)});
763 CheckPointList(pt2, {real3(sqrt(2.0) - 1, sqrt(2.0), 2), real3(sqrt(2.0), sqrt(2.0), 2)});
764
765 // penetrated, small penetration
766 penetration = -1e-5;
767 shape1->position = pos1 + real3(0, 0, penetration);
768 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
769 ASSERT_EQ(nC, 2);
770 CheckValueList(depth, nC, penetration);
771 CheckPointList(norm, nC, real3(0, 0, -1));
772 CheckPointList(
773 pt1, {real3(sqrt(2.0) - 1, sqrt(2.0), 2 + penetration), real3(sqrt(2.0), sqrt(2.0), 2 + penetration)});
774 CheckPointList(pt2, {real3(sqrt(2.0) - 1, sqrt(2.0), 2), real3(sqrt(2.0), sqrt(2.0), 2)});
775
776 // separated by less than 'separation'
777 penetration = +0.05;
778 shape1->position = pos1 + real3(0, 0, penetration);
779 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
780 if (sep) {
781 ASSERT_EQ(nC, 2);
782 CheckValueList(depth, nC, penetration);
783 CheckPointList(norm, nC, real3(0, 0, -1));
784 CheckPointList(
785 pt1, {real3(sqrt(2.0) - 1, sqrt(2.0), 2 + penetration), real3(sqrt(2.0), sqrt(2.0), 2 + penetration)});
786 CheckPointList(pt2, {real3(sqrt(2.0) - 1, sqrt(2.0), 2), real3(sqrt(2.0), sqrt(2.0), 2)});
787 } else {
788 ASSERT_EQ(nC, 0);
789 }
790
791 // separated by more than 'separation'
792 penetration = +0.15;
793 shape1->position = pos1 + real3(0, 0, penetration);
794 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
795 ASSERT_EQ(nC, 0);
796 }
797
798 // Face-corner
799 {
800 real3 hdims1(1.0, 1.0, 1.0);
801 real3 pos1(0.0, 0.0, 0.0);
802 quaternion rot1(1, 0, 0, 0);
803
804 real3 hdims2(1.0, 1.0, 1.0);
805 real3 pos2(0.5, 0.5, 1.0 + sqrt(3.0));
806 quaternion rot2 = FromChQuaternion(Q_from_AngAxis(atan(sqrt(2.0)), ChVector<>(1, 1, 0).GetNormalized()));
807
808 ConvexShapeCustom* shape1 = new ConvexShapeCustom();
809 shape1->type = ChCollisionShape::Type::BOX;
810 shape1->position = pos1;
811 shape1->dimensions = hdims1;
812 shape1->rotation = rot1;
813
814 ConvexShapeCustom* shape2 = new ConvexShapeCustom();
815 shape2->type = ChCollisionShape::Type::BOX;
816 shape2->position = pos2;
817 shape2->dimensions = hdims2;
818 shape2->rotation = rot2;
819
820 // penetrated
821 penetration = -0.05;
822 shape2->position = pos2 + real3(0, 0, penetration);
823 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
824 ASSERT_EQ(nC, 1);
825 CheckValueList(depth, nC, penetration);
826 CheckPointList(norm, nC, real3(0, 0, 1));
827 CheckPointList(pt1, {real3(0.5, 0.5, 1)});
828 CheckPointList(pt2, {real3(0.5, 0.5, 1 + penetration)});
829
830 // penetrated, small penetration
831 penetration = -1e-5;
832 shape2->position = pos2 + real3(0, 0, penetration);
833 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
834 ASSERT_EQ(nC, 1);
835 CheckValueList(depth, nC, penetration);
836 CheckPointList(norm, nC, real3(0, 0, 1));
837 CheckPointList(pt1, {real3(0.5, 0.5, 1)});
838 CheckPointList(pt2, {real3(0.5, 0.5, 1 + penetration)});
839
840 // separated by less than 'separation'
841 penetration = +0.05;
842 shape2->position = pos2 + real3(0, 0, penetration);
843 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
844 if (sep) {
845 ASSERT_EQ(nC, 1);
846 CheckValueList(depth, nC, penetration);
847 CheckPointList(norm, nC, real3(0, 0, 1));
848 CheckPointList(pt1, {real3(0.5, 0.5, 1)});
849 CheckPointList(pt2, {real3(0.5, 0.5, 1 + penetration)});
850 } else {
851 ASSERT_EQ(nC, 0);
852 }
853
854 // separated by more than 'separation'
855 penetration = +0.15;
856 shape2->position = pos2 + real3(0, 0, penetration);
857 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
858 ASSERT_EQ(nC, 0);
859 }
860
861 // Edge-edge
862 // Bottom box rotated by 45 deg about y. Top box rotated by 45 deg about x.
863 {
864 real3 hdims1(1.0, 1.0, 1.0);
865 real3 pos1(0, 0, 0);
866 quaternion rot1 = FromChQuaternion(Q_from_AngY(CH_C_PI / 4));
867
868 real3 hdims2(1.0, 1.0, 1.0);
869 real3 pos2(0.0, 0.0, 2 * sqrt(2.0));
870 quaternion rot2 = FromChQuaternion(Q_from_AngX(CH_C_PI / 4));
871
872 ConvexShapeCustom* shape1 = new ConvexShapeCustom();
873 shape1->type = ChCollisionShape::Type::BOX;
874 shape1->position = pos1;
875 shape1->dimensions = hdims1;
876 shape1->rotation = rot1;
877
878 ConvexShapeCustom* shape2 = new ConvexShapeCustom();
879 shape2->type = ChCollisionShape::Type::BOX;
880 shape2->position = pos2;
881 shape2->dimensions = hdims2;
882 shape2->rotation = rot2;
883
884 // penetrated
885 penetration = -0.05;
886 shape2->position = pos2 + real3(0, 0, penetration);
887 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
888 ASSERT_EQ(nC, 1);
889 CheckValueList(depth, nC, penetration);
890 CheckPointList(norm, nC, real3(0, 0, 1));
891 CheckPointList(pt1, {real3(0, 0, sqrt(2.0))});
892 CheckPointList(pt2, {real3(0, 0, sqrt(2.0) + penetration)});
893
894 // penetrated, small penetration
895 penetration = -1e-5;
896 shape2->position = pos2 + real3(0, 0, penetration);
897 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
898 ASSERT_EQ(nC, 1);
899 CheckValueList(depth, nC, penetration);
900 CheckPointList(norm, nC, real3(0, 0, 1));
901 CheckPointList(pt1, {real3(0, 0, sqrt(2.0))});
902 CheckPointList(pt2, {real3(0, 0, sqrt(2.0) + penetration)});
903
904 // separated by less than 'separation'
905 penetration = +0.05;
906 shape2->position = pos2 + real3(0, 0, penetration);
907 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
908 if (sep) {
909 ASSERT_EQ(nC, 1);
910 CheckValueList(depth, nC, penetration);
911 CheckPointList(norm, nC, real3(0, 0, 1));
912 CheckPointList(pt1, {real3(0, 0, sqrt(2.0))});
913 CheckPointList(pt2, {real3(0, 0, sqrt(2.0) + penetration)});
914 } else {
915 ASSERT_EQ(nC, 0);
916 }
917
918 // separated by more than 'separation'
919 penetration = +0.15;
920 shape2->position = pos2 + real3(0, 0, penetration);
921 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
922 ASSERT_EQ(nC, 0);
923 }
924
925 // Corner-corner
926 // Notes:
927 // - the current algorithm cannot produce a proper corner-corner interaction. Because it always picks
928 // the direction of minimum overlap, for this case it will report a face-face interaction.
929 // - for similar reasons, the current algorithm will not report an interaction for a configuration
930 // where two boxes are separated, but with corners are in each others Voronoi region (no matter of
931 // the separation distance.
932 {
933 real3 hdims1(1.0, 1.0, 1.0);
934 real3 pos1(0.0, 0.0, 0.0);
935 quaternion rot1 = FromChQuaternion(Q_from_AngAxis(atan(sqrt(2.0)), ChVector<>(1, 1, 0).GetNormalized()));
936
937 real3 hdims2(1.0, 1.0, 1.0);
938 real3 pos2(0, 0, sqrt(3.0) + sqrt(3.0));
939 quaternion rot2 = FromChQuaternion(Q_from_AngAxis(atan(sqrt(2.0)), ChVector<>(1, 1, 0).GetNormalized()));
940
941 ConvexShapeCustom* shape1 = new ConvexShapeCustom();
942 shape1->type = ChCollisionShape::Type::BOX;
943 shape1->position = pos1;
944 shape1->dimensions = hdims1;
945 shape1->rotation = rot1;
946
947 ConvexShapeCustom* shape2 = new ConvexShapeCustom();
948 shape2->type = ChCollisionShape::Type::BOX;
949 shape2->position = pos2;
950 shape2->dimensions = hdims2;
951 shape2->rotation = rot2;
952
953 // penetrated
954 penetration = -0.05;
955 shape2->position = pos2 + real3(0, 0, penetration);
956 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
957 ASSERT_EQ(nC, 4);
958 CheckValueList(depth, nC, penetration / std::sqrt(3.0));
959 ////for (int i = 0; i < 4; i++) {
960 //// real3 p1 = RotateT((pt1[i] - pos1), rot1);
961 //// real3 p2 = RotateT((pt2[i] - pos2 - real3(0, 0, penetration)), rot2);
962 ////}
963
964 // penetrated, small penetration
965 penetration = -1e-5 * std::sqrt(3.0);
966 shape2->position = pos2 + real3(0, 0, penetration);
967 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
968 ASSERT_EQ(nC, 4);
969 CheckValueList(depth, nC, penetration / std::sqrt(3.0));
970
971 // separated by less than 'separation'
972 penetration = +0.05;
973 shape2->position = pos2 + real3(0, 0, penetration);
974 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shape1, shape2, separation, norm, pt1, pt2, depth, eff_rad, nC));
975 if (sep) {
976 // Limitation of current algorithm
977 ASSERT_EQ(nC, 0);
978 } else {
979 ASSERT_EQ(nC, 0);
980 }
981
982 // separated by more than 'separation'
983 penetration = +0.15;
984 shape2->position = pos2 + real3(0, 0, penetration);
985 ASSERT_EQ(nC, 0);
986 }
987 }
988
TEST_P(Collision,sphere_sphere)989 TEST_P(Collision, sphere_sphere) {
990 ConvexShapeCustom* shapeS1 = new ConvexShapeCustom();
991 shapeS1->type = ChCollisionShape::Type::SPHERE;
992 shapeS1->radius = 0;
993 shapeS1->rotation = quaternion(1, 0, 0, 0);
994
995 ConvexShapeCustom* shapeS2 = new ConvexShapeCustom();
996 shapeS2->type = ChCollisionShape::Type::SPHERE;
997 shapeS2->radius = 0;
998 shapeS2->rotation = quaternion(1, 0, 0, 0);
999
1000 real separation = sep ? 0.1 : 0.0;
1001
1002 // Output quantities.
1003 real3 norm;
1004 real3 pt1;
1005 real3 pt2;
1006 real depth;
1007 real eff_rad;
1008 int nC;
1009
1010 // separated (far)
1011 {
1012 shapeS1->position = real3(2, 2, 0);
1013 shapeS1->dimensions = real3(1, 0, 0);
1014
1015 shapeS2->position = real3(2, 0, 0);
1016 shapeS2->dimensions = real3(0.5, 0, 0);
1017
1018 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeS1, shapeS2, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1019 ASSERT_EQ(nC, 0);
1020 }
1021
1022 // separated (near)
1023 {
1024 shapeS1->position = real3(2, 2, 0);
1025 shapeS1->dimensions = real3(1, 0, 0);
1026
1027 shapeS2->position = real3(2, 0, 0);
1028 shapeS2->dimensions = real3(0.95, 0, 0);
1029
1030 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeS1, shapeS2, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1031 if (sep) {
1032 ASSERT_EQ(nC, 1);
1033 Assert_near(norm, real3(0, -1, 0), precision);
1034 ASSERT_NEAR(depth, 0.05, precision);
1035 Assert_near(pt1, real3(2, 1, 0), precision);
1036 Assert_near(pt2, real3(2, 0.95, 0), precision);
1037 ASSERT_NEAR(eff_rad, 0.95 / 1.95, precision);
1038 } else {
1039 ASSERT_EQ(nC, 0);
1040 }
1041 }
1042
1043 // touching
1044 {
1045 shapeS1->position = real3(2, 2, 0);
1046 shapeS1->dimensions = real3(1, 0, 0);
1047
1048 shapeS2->position = real3(2, 0, 0);
1049 shapeS2->dimensions = real3(1, 0, 0);
1050
1051 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeS1, shapeS2, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1052 if (sep) {
1053 ASSERT_EQ(nC, 1);
1054 Assert_near(norm, real3(0, -1, 0), precision);
1055 ASSERT_NEAR(depth, 0, precision);
1056 Assert_near(pt1, real3(2, 1, 0), precision);
1057 Assert_near(pt2, real3(2, 1, 0), precision);
1058 ASSERT_NEAR(eff_rad, 0.5, precision);
1059 } else {
1060 ASSERT_EQ(nC, 0);
1061 }
1062 }
1063
1064 // penetrated
1065 {
1066 shapeS1->position = real3(1, 1, 0);
1067 shapeS1->dimensions = real3(1, 0, 0);
1068
1069 shapeS2->position = real3(2.5, 1, 0);
1070 shapeS2->dimensions = real3(1, 0, 0);
1071
1072 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeS1, shapeS2, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1073 ASSERT_EQ(nC, 1);
1074 Assert_near(norm, real3(1, 0, 0), precision);
1075 ASSERT_NEAR(depth, -0.5, precision);
1076 Assert_near(pt1, real3(2, 1, 0), precision);
1077 Assert_near(pt2, real3(1.5, 1, 0), precision);
1078 ASSERT_NEAR(eff_rad, 0.5, precision);
1079 }
1080
1081 delete shapeS1;
1082 delete shapeS2;
1083 }
1084
1085 // -----------------------------------------------------------------------------
1086
TEST_P(Collision,box_sphere)1087 TEST_P(Collision, box_sphere) {
1088 // Fictitious radius of curvature for corners and edges
1089 real edge_radius = ChNarrowphase::GetDefaultEdgeRadius();
1090
1091 // Box position and orientation fixed for all tests.
1092 // Rotated by 45 degrees around Z axis and shifted by sqrt(2) in X direction.
1093 real3 b_hdims(1.0, 2.0, 3.0);
1094 real3 b_pos(sqrt(2.0), 0.0, 0.0);
1095 quaternion b_rot = FromChQuaternion(Q_from_AngAxis(CH_C_PI_4, ChVector<>(0, 0, 1)));
1096
1097 ConvexShapeCustom* shapeC = new ConvexShapeCustom();
1098 shapeC->type = ChCollisionShape::Type::BOX;
1099 shapeC->position = b_pos;
1100 shapeC->dimensions = b_hdims;
1101 shapeC->radius = 0;
1102 shapeC->rotation = b_rot;
1103
1104 // Sphere position changes for each test.
1105 real s_rad = 1.5; // sphere radius
1106
1107 ConvexShapeCustom* shapeS = new ConvexShapeCustom();
1108 shapeS->type = ChCollisionShape::Type::SPHERE;
1109 shapeS->dimensions = real3(s_rad, 0, 0);
1110 shapeS->radius = 0;
1111 shapeS->rotation = quaternion(1, 0, 0, 0);
1112
1113 real separation = sep ? 0.1 : 0.0;
1114
1115 // Output quantities.
1116 real3 norm;
1117 real3 pt1;
1118 real3 pt2;
1119 real depth;
1120 real eff_rad;
1121 int nC;
1122
1123 real oosqrt2 = sqrt(0.5); // 1/sqrt(2)
1124
1125 // sphere center inside box
1126 {
1127 shapeS->position = real3(0.5, 0.5, 1.0);
1128 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1129 ASSERT_EQ(nC, 0);
1130 }
1131
1132 // face interaction (separated far)
1133 {
1134 shapeS->position = real3(3.5, 2.5, 1.0);
1135 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1136 ASSERT_EQ(nC, 0);
1137 }
1138
1139 // face interaction (separated near)
1140 {
1141 shapeS->position = real3(4.55 * oosqrt2, 2.55 * oosqrt2, 1.0);
1142 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1143 if (sep) {
1144 ASSERT_EQ(nC, 1);
1145 Assert_near(norm, real3(oosqrt2, oosqrt2, 0.0), precision);
1146 ASSERT_NEAR(depth, 0.05, precision);
1147 Assert_near(pt1, real3(3.0 * oosqrt2, oosqrt2, 1.0), precision);
1148 Assert_near(pt2, real3(3.05 * oosqrt2, 1.05 * oosqrt2, 1.0), precision);
1149 } else {
1150 ASSERT_EQ(nC, 0);
1151 }
1152 }
1153
1154 // face interaction (penetrated)
1155 {
1156 shapeS->position = real3(4 * oosqrt2, 2.0 * oosqrt2, 1.0);
1157 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1158 ASSERT_EQ(nC, 1);
1159 Assert_near(norm, real3(oosqrt2, oosqrt2, 0.0), precision);
1160 ASSERT_NEAR(depth, -0.5, precision);
1161 Assert_near(pt1, real3(3.0 * oosqrt2, oosqrt2, 1.0), precision);
1162 Assert_near(pt2, real3(2.5 * oosqrt2, 0.5 * oosqrt2, 1.0), precision);
1163 ASSERT_NEAR(eff_rad, s_rad, precision);
1164 }
1165
1166 // edge interaction (separated far)
1167 {
1168 shapeS->position = real3(oosqrt2, 4.0, 1.0);
1169 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1170 ASSERT_EQ(nC, 0);
1171 }
1172
1173 // edge interaction (separated near)
1174 {
1175 shapeS->position = real3(oosqrt2, 3.0 * oosqrt2 + 1.55, 1.0);
1176 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1177 if (sep) {
1178 ASSERT_EQ(nC, 1);
1179 Assert_near(norm, real3(0.0, 1.0, 0.0), precision);
1180 ASSERT_NEAR(depth, 0.05, precision);
1181 Assert_near(pt1, real3(oosqrt2, 3.0 * oosqrt2, 1.0), precision);
1182 Assert_near(pt2, real3(oosqrt2, 3.0 * oosqrt2 + 0.05, 1.0), precision);
1183 } else {
1184 ASSERT_EQ(nC, 0);
1185 }
1186 }
1187
1188 // edge interaction (penetrated)
1189 {
1190 shapeS->position = real3(oosqrt2, 3.0 * oosqrt2 + 1.0, 1.0);
1191 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1192 ASSERT_EQ(nC, 1);
1193 Assert_near(norm, real3(0.0, 1.0, 0.0), precision);
1194 ASSERT_NEAR(depth, -0.5, precision);
1195 Assert_near(pt1, real3(oosqrt2, 3.0 * oosqrt2, 1.0), precision);
1196 Assert_near(pt2, real3(oosqrt2, 3.0 * oosqrt2 - 0.5, 1.0), precision);
1197 ASSERT_NEAR(eff_rad, s_rad * edge_radius / (s_rad + edge_radius), precision);
1198 }
1199
1200 // corner interaction (separated far)
1201 {
1202 shapeS->position = real3(oosqrt2, 4.0, 4.0);
1203 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1204 ASSERT_EQ(nC, 0);
1205 }
1206
1207 // corner interaction (separated near)
1208 {
1209 real3 s_pos(oosqrt2, 4.55 * oosqrt2, 3.0 + 1.55 * oosqrt2);
1210 shapeS->position = s_pos;
1211 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1212 if (sep) {
1213 ASSERT_EQ(nC, 1);
1214 Assert_near(norm, real3(0.0, oosqrt2, oosqrt2), precision);
1215 ASSERT_NEAR(depth, 0.05, precision);
1216 Assert_near(pt1, real3(oosqrt2, 3.0 * oosqrt2, 3.0), precision);
1217 Assert_near(pt2, s_pos - s_rad * norm, precision);
1218 } else {
1219 ASSERT_EQ(nC, 0);
1220 }
1221 }
1222
1223 // corner interaction (penetrated)
1224 {
1225 real3 s_pos(oosqrt2, 4.0 * oosqrt2, 3.0 + oosqrt2);
1226 shapeS->position = s_pos;
1227 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, separation, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1228 ASSERT_EQ(nC, 1);
1229 Assert_near(norm, real3(0.0, oosqrt2, oosqrt2), precision);
1230 ASSERT_NEAR(depth, -0.5, precision);
1231 Assert_near(pt1, real3(oosqrt2, 3.0 * oosqrt2, 3.0), precision);
1232 Assert_near(pt2, s_pos - s_rad * norm, precision);
1233 ASSERT_NEAR(eff_rad, s_rad * edge_radius / (s_rad + edge_radius), precision);
1234 }
1235
1236 delete shapeS;
1237 delete shapeC;
1238 }
1239
1240 // -----------------------------------------------------------------------------
1241
1242 //// TODO: include case with non-zero separation
TEST_P(Collision,capsule_sphere)1243 TEST_P(Collision, capsule_sphere) {
1244 // Capsule position and orientation fixed for all tests.
1245 // aligned with X axis and shifted by its half-length in the X direction.
1246 real c_rad = 0.5;
1247 real c_hlen = 2.0;
1248 real3 c_pos(c_hlen, 0, 0);
1249 quaternion c_rot = FromChQuaternion(Q_from_AngAxis(CH_C_PI_2, ChVector<>(0, 0, 1)));
1250
1251 ConvexShapeCustom* shapeC = new ConvexShapeCustom();
1252 shapeC->type = ChCollisionShape::Type::CAPSULE;
1253 shapeC->position = c_pos;
1254 shapeC->dimensions = real3(c_rad, c_hlen, c_rad);
1255 shapeC->radius = 0;
1256 shapeC->rotation = c_rot;
1257
1258 // Sphere position changes for each test.
1259 real s_rad = 1.0; // sphere radius
1260
1261 ConvexShapeCustom* shapeS = new ConvexShapeCustom();
1262 shapeS->type = ChCollisionShape::Type::SPHERE;
1263 shapeS->dimensions = real3(s_rad, 0, 0);
1264 shapeS->radius = 0;
1265 shapeS->rotation = quaternion(1, 0, 0, 0);
1266
1267 // Output quantities.
1268 real3 norm;
1269 real3 pt1;
1270 real3 pt2;
1271 real depth;
1272 real eff_rad;
1273 int nC;
1274
1275 real oosqrt2 = sqrt(0.5); // 1/sqrt(2)
1276
1277 // sphere center on capsule axis
1278 {
1279 shapeS->position = real3(3.0, 0.0, 0.0);
1280 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1281 ASSERT_EQ(nC, 0);
1282 }
1283
1284 // cap interaction (separated)
1285 {
1286 shapeS->position = real3(5.0, 1.5, 0.0);
1287 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1288 ASSERT_EQ(nC, 0);
1289 }
1290
1291 // cap interaction (penetrated)"
1292 {
1293 shapeS->position = real3(5.0, 1.0, 0.0);
1294 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1295 ASSERT_EQ(nC, 1);
1296 Assert_near(norm, real3(oosqrt2, oosqrt2, 0), precision);
1297 ASSERT_NEAR(depth, sqrt(2.0) - 1.5, precision);
1298 Assert_near(pt1, real3(4.0 + 0.5 * oosqrt2, 0.5 * oosqrt2, 0), precision);
1299 Assert_near(pt2, real3(5.0 - oosqrt2, 1.0 - oosqrt2, 0), precision);
1300 ASSERT_NEAR(eff_rad, s_rad * c_rad / (s_rad + c_rad), precision);
1301 }
1302
1303 // side interaction (separated)
1304 {
1305 shapeS->position = real3(2.5, 2.0, 0);
1306 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1307 ASSERT_EQ(nC, 0);
1308 }
1309
1310 // side interaction (penetrated)
1311 {
1312 shapeS->position = real3(2.5, 1.25, 0);
1313 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1314 ASSERT_EQ(nC, 1);
1315 Assert_near(norm, real3(0, 1, 0), precision);
1316 ASSERT_NEAR(depth, -0.25, precision);
1317 Assert_near(pt1, real3(2.5, 0.5, 0), precision);
1318 Assert_near(pt2, real3(2.5, 0.25, 0), precision);
1319 ASSERT_NEAR(eff_rad, s_rad * c_rad / (s_rad + c_rad), precision);
1320 }
1321
1322 delete shapeS;
1323 delete shapeC;
1324 }
1325
1326 // -----------------------------------------------------------------------------
1327
1328 //// TODO: include case with non-zero separation
TEST_P(Collision,cylinder_sphere)1329 TEST_P(Collision, cylinder_sphere) {
1330 // Fictitious radius of curvature for corners and edges
1331 real edge_radius = ChNarrowphase::GetDefaultEdgeRadius();
1332
1333 // Cylinder position and orientation fixed for all tests.
1334 // Aligned with X axis and shifted by its half-length in the X direction.
1335 real c_rad = 2.0;
1336 real c_hlen = 1.5;
1337 real3 c_pos(c_hlen, 0, 0);
1338 quaternion c_rot = FromChQuaternion(Q_from_AngAxis(CH_C_PI_2, ChVector<>(0, 0, 1)));
1339
1340 ConvexShapeCustom* shapeC = new ConvexShapeCustom();
1341 shapeC->type = ChCollisionShape::Type::CYLINDER;
1342 shapeC->position = c_pos;
1343 shapeC->dimensions = real3(c_rad, c_hlen, c_rad);
1344 shapeC->radius = 0;
1345 shapeC->rotation = c_rot;
1346
1347 // Sphere position changes for each test.
1348 real s_rad = 1.0; // sphere radius
1349
1350 ConvexShapeCustom* shapeS = new ConvexShapeCustom();
1351 shapeS->type = ChCollisionShape::Type::SPHERE;
1352 shapeS->dimensions = real3(s_rad, 0, 0);
1353 shapeS->radius = 0;
1354 shapeS->rotation = quaternion(1, 0, 0, 0);
1355
1356 // Output quantities.
1357 real3 norm;
1358 real3 pt1;
1359 real3 pt2;
1360 real depth;
1361 real eff_rad;
1362 int nC;
1363
1364 real oosqrt2 = sqrt(0.5); // 1/sqrt(2)
1365
1366 // sphere center inside cylinder
1367 {
1368 shapeS->position = real3(2.5, 1.5, 0);
1369 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1370 ASSERT_EQ(nC, 0);
1371 }
1372
1373 // cap interaction (separated)
1374 {
1375 shapeS->position = real3(4.5, 1.5, 0);
1376 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1377 ASSERT_EQ(nC, 0);
1378 }
1379
1380 // cap interaction (penetrated)
1381 {
1382 shapeS->position = real3(3.75, 1.5, 0);
1383 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1384 ASSERT_EQ(nC, 1);
1385 Assert_near(norm, real3(1, 0, 0), precision);
1386 ASSERT_NEAR(depth, -0.25, precision);
1387 Assert_near(pt1, real3(3, 1.5, 0), precision);
1388 Assert_near(pt2, real3(2.75, 1.5, 0), precision);
1389 ASSERT_NEAR(eff_rad, s_rad, precision);
1390 }
1391
1392 // side interaction (separated)
1393 {
1394 shapeS->position = real3(2.5, 3.5, 0);
1395 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1396 ASSERT_EQ(nC, 0);
1397 }
1398
1399 // side interaction (penetrated)
1400 {
1401 shapeS->position = real3(2.5, 2.5, 0);
1402 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1403 ASSERT_EQ(nC, 1);
1404 Assert_near(norm, real3(0, 1, 0), precision);
1405 ASSERT_NEAR(depth, -0.5, precision);
1406 Assert_near(pt1, real3(2.5, 2.0, 0), precision);
1407 Assert_near(pt2, real3(2.5, 1.5, 0), precision);
1408 ASSERT_NEAR(eff_rad, s_rad * c_rad / (s_rad + c_rad), precision);
1409 }
1410
1411 // edge interaction (separated)
1412 {
1413 shapeS->position = real3(4, 3, 0);
1414 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1415 ASSERT_EQ(nC, 0);
1416 }
1417
1418 // edge interaction (penetrated)
1419 {
1420 shapeS->position = real3(3.5, 2.5, 0);
1421 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1422 ASSERT_EQ(nC, 1);
1423 Assert_near(norm, real3(oosqrt2, oosqrt2, 0), precision);
1424 ASSERT_NEAR(depth, -1 + oosqrt2, precision);
1425 Assert_near(pt1, real3(3.0, 2.0, 0), precision);
1426 Assert_near(pt2, real3(3.5 - oosqrt2, 2.5 - oosqrt2, 0), precision);
1427 ASSERT_NEAR(eff_rad, s_rad * edge_radius / (s_rad + edge_radius), precision);
1428 }
1429
1430 delete shapeS;
1431 delete shapeC;
1432 }
1433
1434 // -----------------------------------------------------------------------------
1435
1436 //// TODO: include case with non-zero separation
TEST_P(Collision,roundedcyl_sphere)1437 TEST_P(Collision, roundedcyl_sphere) {
1438 // Rounded cylinder position and orientation fixed for all tests.
1439 // Aligned with X axis and shifted by its half-length in the X direction.
1440 real c_rad = 2.0; // radius of skeleton cylinder
1441 real c_hlen = 1.5; // half-length of skeleton cylinder
1442 real c_srad = 0.1; // radius of sweeping sphere
1443 real3 c_pos(c_hlen, 0, 0);
1444 quaternion c_rot = FromChQuaternion(Q_from_AngAxis(CH_C_PI_2, ChVector<>(0, 0, 1)));
1445
1446 ConvexShapeCustom* shapeC = new ConvexShapeCustom();
1447 shapeC->type = ChCollisionShape::Type::ROUNDEDCYL;
1448 shapeC->position = c_pos;
1449 shapeC->dimensions = real3(c_rad, c_hlen, c_rad);
1450 shapeC->radius = c_srad;
1451 shapeC->rotation = c_rot;
1452
1453 // Sphere position changes for each test.
1454 real s_rad = 1.0; // sphere radius
1455
1456 ConvexShapeCustom* shapeS = new ConvexShapeCustom();
1457 shapeS->type = ChCollisionShape::Type::SPHERE;
1458 shapeS->dimensions = real3(s_rad, 0, 0);
1459 shapeS->radius = 0;
1460 shapeS->rotation = quaternion(1, 0, 0, 0);
1461
1462 // Output quantities.
1463 real3 norm;
1464 real3 pt1;
1465 real3 pt2;
1466 real depth;
1467 real eff_rad;
1468 int nC;
1469
1470 real oosqrt2 = sqrt(0.5); // 1/sqrt(2)
1471
1472 // sphere center inside cylinder
1473 {
1474 shapeS->position = real3(2.5, 1.5, 0);
1475 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1476 ASSERT_EQ(nC, 0);
1477 }
1478
1479 // cap interaction (separated)
1480 {
1481 shapeS->position = real3(4.5, 1.5, 0);
1482 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1483 ASSERT_EQ(nC, 0);
1484 }
1485
1486 // cap interaction (penetrated)
1487 {
1488 shapeS->position = real3(3.75, 1.5, 0);
1489 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1490 ASSERT_EQ(nC, 1);
1491 Assert_near(norm, real3(1, 0, 0), precision);
1492 ASSERT_NEAR(depth, -0.35, precision);
1493 Assert_near(pt1, real3(3.1, 1.5, 0), precision);
1494 Assert_near(pt2, real3(2.75, 1.5, 0), precision);
1495 ASSERT_NEAR(eff_rad, s_rad, precision);
1496 }
1497
1498 // side interaction (separated)
1499 {
1500 shapeS->position = real3(2.5, 3.5, 0);
1501 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1502 ASSERT_EQ(nC, 0);
1503 }
1504
1505 // side interaction (penetrated)
1506 {
1507 shapeS->position = real3(2.5, 2.5, 0);
1508 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1509 ASSERT_EQ(nC, 1);
1510 Assert_near(norm, real3(0, 1, 0), precision);
1511 ASSERT_NEAR(depth, -0.6, precision);
1512 Assert_near(pt1, real3(2.5, 2.1, 0), precision);
1513 Assert_near(pt2, real3(2.5, 1.5, 0), precision);
1514 ASSERT_NEAR(eff_rad, s_rad * c_rad / (s_rad + c_rad), precision);
1515 }
1516
1517 // edge interaction (separated)
1518 {
1519 shapeS->position = real3(4, 3, 0);
1520 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1521 ASSERT_EQ(nC, 0);
1522 }
1523
1524 // edge interaction (penetrated)
1525 {
1526 shapeS->position = real3(3.5, 2.5, 0);
1527 ASSERT_TRUE(ChNarrowphase::PRIMSCollision(shapeC, shapeS, 0, &norm, &pt1, &pt2, &depth, &eff_rad, nC));
1528 ASSERT_EQ(nC, 1);
1529 Assert_near(norm, real3(oosqrt2, oosqrt2, 0), precision);
1530 ASSERT_NEAR(depth, -1.1 + oosqrt2, precision);
1531 Assert_near(pt1, real3(3.0 + 0.1 * oosqrt2, 2.0 + 0.1 * oosqrt2, 0), precision);
1532 Assert_near(pt2, real3(3.5 - oosqrt2, 2.5 - oosqrt2, 0), precision);
1533 ASSERT_NEAR(eff_rad, s_rad * c_srad / (s_rad + c_srad), precision);
1534 }
1535
1536 delete shapeS;
1537 delete shapeC;
1538 }
1539
1540 INSTANTIATE_TEST_SUITE_P(R, Collision, ::testing::Bool());
1541