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