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
13 #include <cstdio>
14 #include <cmath>
15
16 #include "chrono/collision/edgetempest/ChCGeometryCollider.h"
17 #include "chrono/collision/edgetempest/ChCOBB.h"
18 #include "chrono/collision/ChCollisionUtils.h"
19 #include "chrono/core/ChTransform.h"
20 #include "chrono/geometry/ChBox.h"
21 #include "chrono/geometry/ChSphere.h"
22 #include "chrono/geometry/ChTriangle.h"
23
24 #define COLL_PRECISION 0.000001
25
26 namespace chrono {
27 namespace collision {
28
ComputeCollisions(geometry::ChGeometry & mgeo1,ChMatrix33<> * R1,Vector * T1,geometry::ChGeometry & mgeo2,ChMatrix33<> * R2,Vector * T2,ChNarrowPhaseCollider & mcollider,ChMatrix33<> * relRot,Vector * relPos,bool just_intersection)29 int ChGeometryCollider::ComputeCollisions(
30 geometry::ChGeometry& mgeo1, ///< first geometric primitive
31 ChMatrix33<>* R1, ///< absolute rotation of first geometric primitive
32 Vector* T1, ///< absolute position of first geometric primitive
33 geometry::ChGeometry& mgeo2, ///< second geometric primitive
34 ChMatrix33<>* R2, ///< absolute rotation of second geometric primitive
35 Vector* T2, ///< absolute position of second geometric primitive
36 ChNarrowPhaseCollider& mcollider, ///< reference to a collider to store contacts into
37 ChMatrix33<>* relRot, ///< relative rotation of mgeo2 respect to mgeo1, if available, for optimization
38 Vector* relPos, ///< relative position of mgeo2 respect to mgeo1, if available, for optimization
39 bool just_intersection ///< if true, just report if intersecting (no further calculus on normal, depht, etc.)
40 ) {
41 // GetLog() << " ChGeometryCollider::ComputeCollisions! geos:" << (int)&mgeo1 << " " << (int)&mgeo2 << "\n";
42
43 // Dispatch all subcases of geometry-geometry pairs..
44
45 if ((mgeo1.GetClassType() == geometry::ChGeometry::TRIANGLE) &&
46 (mgeo2.GetClassType() == geometry::ChGeometry::TRIANGLE)) {
47 //***TO DO***
48 return 0;
49 }
50
51 if ((mgeo1.GetClassType() == geometry::ChGeometry::SPHERE) &&
52 (mgeo2.GetClassType() == geometry::ChGeometry::SPHERE)) {
53 geometry::ChSphere* msph1 = (geometry::ChSphere*)&mgeo1;
54 geometry::ChSphere* msph2 = (geometry::ChSphere*)&mgeo2;
55 Vector c1, c2;
56 c1 = ChTransform<>::TransformLocalToParent(msph1->center, *T1, *R1);
57 c2 = ChTransform<>::TransformLocalToParent(msph2->center, *T2, *R2);
58 return ComputeSphereSphereCollisions(*msph1, &c1, *msph2, &c2, mcollider, just_intersection);
59 }
60
61 if ((mgeo1.GetClassType() == geometry::ChGeometry::BOX) && (mgeo2.GetClassType() == geometry::ChGeometry::BOX)) {
62 geometry::ChBox* mbox1 = (geometry::ChBox*)&mgeo1;
63 geometry::ChBox* mbox2 = (geometry::ChBox*)&mgeo2;
64 return ComputeBoxBoxCollisions(*mbox1, R1, T1, *mbox2, R2, T2, mcollider, just_intersection);
65 }
66
67 if ((mgeo1.GetClassType() == geometry::ChGeometry::SPHERE) &&
68 (mgeo2.GetClassType() == geometry::ChGeometry::TRIANGLE)) {
69 geometry::ChSphere* msph = (geometry::ChSphere*)&mgeo1;
70 geometry::ChTriangle* mtri = (geometry::ChTriangle*)&mgeo2;
71 Vector c1 = ChTransform<>::TransformLocalToParent(msph->center, *T1, *R1);
72 return ComputeSphereTriangleCollisions(*msph, &c1, *mtri, R2, T2, mcollider, just_intersection, false);
73 }
74 if ((mgeo1.GetClassType() == geometry::ChGeometry::TRIANGLE) &&
75 (mgeo2.GetClassType() == geometry::ChGeometry::SPHERE)) {
76 geometry::ChSphere* msph = (geometry::ChSphere*)&mgeo2;
77 geometry::ChTriangle* mtri = (geometry::ChTriangle*)&mgeo1;
78 Vector c2 = ChTransform<>::TransformLocalToParent(msph->center, *T2, *R2);
79 return ComputeSphereTriangleCollisions(*msph, &c2, *mtri, R1, T1, mcollider, just_intersection, true);
80 }
81
82 if ((mgeo1.GetClassType() == geometry::ChGeometry::SPHERE) && (mgeo2.GetClassType() == geometry::ChGeometry::BOX)) {
83 geometry::ChSphere* msph = (geometry::ChSphere*)&mgeo1;
84 geometry::ChBox* mbox = (geometry::ChBox*)&mgeo2;
85 Vector c1 = ChTransform<>::TransformLocalToParent(msph->center, *T1, *R1);
86 return ComputeSphereBoxCollisions(*msph, &c1, *mbox, R2, T2, mcollider, just_intersection, false);
87 }
88 if ((mgeo1.GetClassType() == geometry::ChGeometry::BOX) && (mgeo2.GetClassType() == geometry::ChGeometry::SPHERE)) {
89 geometry::ChSphere* msph = (geometry::ChSphere*)&mgeo2;
90 geometry::ChBox* mbox = (geometry::ChBox*)&mgeo1;
91 Vector c2 = ChTransform<>::TransformLocalToParent(msph->center, *T2, *R2);
92 return ComputeSphereBoxCollisions(*msph, &c2, *mbox, R1, T1, mcollider, just_intersection, true);
93 }
94
95 if ((mgeo1.GetClassType() == geometry::ChGeometry::BOX) &&
96 (mgeo2.GetClassType() == geometry::ChGeometry::TRIANGLE)) {
97 geometry::ChBox* mbox = (geometry::ChBox*)&mgeo1;
98 geometry::ChTriangle* mtri = (geometry::ChTriangle*)&mgeo2;
99 return ComputeBoxTriangleCollisions(*mbox, R1, T1, *mtri, R2, T2, mcollider, just_intersection, false);
100 }
101 if ((mgeo1.GetClassType() == geometry::ChGeometry::TRIANGLE) &&
102 (mgeo2.GetClassType() == geometry::ChGeometry::BOX)) {
103 geometry::ChBox* mbox = (geometry::ChBox*)&mgeo2;
104 geometry::ChTriangle* mtri = (geometry::ChTriangle*)&mgeo1;
105 return ComputeBoxTriangleCollisions(*mbox, R2, T2, *mtri, R1, T1, mcollider, just_intersection, true);
106 }
107
108 // default: this collision pair is not supported
109 return 0;
110 }
111
112 // SPHERE - SPHERE
113
ComputeSphereSphereCollisions(geometry::ChSphere & mgeo1,Vector * c1,geometry::ChSphere & mgeo2,Vector * c2,ChNarrowPhaseCollider & mcollider,bool just_intersection)114 int ChGeometryCollider::ComputeSphereSphereCollisions(
115 geometry::ChSphere& mgeo1, ///< first sphere
116 Vector* c1, ///< absolute position of 1st
117 geometry::ChSphere& mgeo2, ///< second sphere
118 Vector* c2, ///< absolute position of 2nd
119 ChNarrowPhaseCollider& mcollider, ///< reference to a collider to store contacts into
120 bool just_intersection) {
121 Vector relPos = Vsub(*c2, *c1);
122 Vector dir = Vnorm(relPos);
123 double dist = Vlength(relPos);
124 if (just_intersection) {
125 if (dist < (mgeo1.rad + mgeo2.rad)) {
126 ChCollisionPair temp = ChCollisionPair(&mgeo1, &mgeo2);
127 mcollider.AddCollisionPair(&temp);
128 return 1;
129 }
130 } else {
131 if (dist < CH_COLL_ENVELOPE + (mgeo1.rad + mgeo2.rad)) {
132 ChCollisionPair temp = ChCollisionPair(&mgeo1, &mgeo2, // geometries
133 Vadd(*c1, Vmul(dir, mgeo1.rad)), // p1
134 Vadd(*c2, Vmul(dir, -mgeo2.rad)), // p2
135 dir); // normal
136 mcollider.AddCollisionPair(&temp);
137 return 1;
138 }
139 }
140
141 return 0;
142 }
143
ComputeSphereTriangleCollisions(geometry::ChSphere & mgeo1,Vector * c1,geometry::ChTriangle & mgeo2,ChMatrix33<> * R2,Vector * T2,ChNarrowPhaseCollider & mcollider,bool just_intersection,bool swap_pairs)144 int ChGeometryCollider::ComputeSphereTriangleCollisions(
145 geometry::ChSphere& mgeo1, ///< sphere
146 Vector* c1, ///< absolute position of 1st center
147 geometry::ChTriangle& mgeo2, ///< triangle
148 ChMatrix33<>* R2, ///< absolute roation of 2nd model
149 Vector* T2, ///< absolute position of 2nd model
150 ChNarrowPhaseCollider& mcollider, ///< reference to a collider to store contacts into
151 bool just_intersection, ///< if true, just report if intersecting (no further calculus on normal, depht, etc.)
152 bool swap_pairs ///< if true, pairs are reported as Triangle-Sphere (triangle will be the main ref.)
153 ) {
154 Vector v_center = *c1;
155 Vector v_1 = ChTransform<>::TransformLocalToParent(mgeo2.p1, *T2, *R2);
156 Vector v_2 = ChTransform<>::TransformLocalToParent(mgeo2.p2, *T2, *R2);
157 Vector v_3 = ChTransform<>::TransformLocalToParent(mgeo2.p3, *T2, *R2);
158
159 Vector mnormal, mp1, mp2, ept;
160 Vector ver_cen;
161 double mdist = 10e20;
162 double tdist = 10e20;
163 double mu, mv;
164 bool is_into = true;
165 bool touched = false;
166
167 // test sphere-vertexes
168 ver_cen = Vsub(v_1, v_center);
169 tdist = -mgeo1.rad + Vlength(ver_cen);
170 if (tdist < 0) {
171 touched = true;
172 mnormal = Vnorm(ver_cen);
173 mp1 = Vadd(v_center, Vmul(mnormal, mgeo1.rad));
174 mp2 = v_1;
175 mdist = tdist;
176 }
177 ver_cen = Vsub(v_2, v_center);
178 tdist = -mgeo1.rad + Vlength(ver_cen);
179 if (tdist < 0)
180 if (tdist < mdist) {
181 touched = true;
182 mnormal = Vnorm(ver_cen);
183 mp1 = Vadd(v_center, Vmul(mnormal, mgeo1.rad));
184 mp2 = v_2;
185 mdist = tdist;
186 }
187 ver_cen = Vsub(v_3, v_center);
188 tdist = -mgeo1.rad + Vlength(ver_cen);
189 if (tdist < 0)
190 if (tdist < mdist) {
191 touched = true;
192 mnormal = Vnorm(ver_cen);
193 mp1 = Vadd(v_center, Vmul(mnormal, mgeo1.rad));
194 mp2 = v_3;
195 mdist = tdist;
196 }
197
198 // test sphere-plane
199 tdist = -mgeo1.rad + fabs(utils::PointTriangleDistance(v_center, v_1, v_2, v_3, mu, mv, is_into, ept));
200 if (is_into)
201 if (tdist < 0)
202 if (tdist < mdist) {
203 touched = true;
204 mnormal = Vnorm(Vsub(ept, v_center));
205 mp1 = Vadd(v_center, Vmul(mnormal, mgeo1.rad));
206 mp2 = ept;
207 mdist = tdist;
208 }
209
210 // test edge-sphere
211 tdist = -mgeo1.rad + fabs(utils::PointLineDistance(v_center, v_1, v_2, mu, is_into));
212 if (is_into)
213 if (tdist < 0)
214 if (tdist < mdist) {
215 touched = true;
216 ept = Vadd(v_1, Vmul(Vsub(v_2, v_1), mu));
217 mnormal = Vnorm(Vsub(ept, v_center));
218 mp1 = Vadd(v_center, Vmul(mnormal, mgeo1.rad));
219 mp2 = ept;
220 mdist = tdist;
221 }
222 tdist = -mgeo1.rad + fabs(utils::PointLineDistance(v_center, v_2, v_3, mu, is_into));
223 if (is_into)
224 if (tdist < 0)
225 if (tdist < mdist) {
226 touched = true;
227 ept = Vadd(v_2, Vmul(Vsub(v_3, v_2), mu));
228 mnormal = Vnorm(Vsub(ept, v_center));
229 mp1 = Vadd(v_center, Vmul(mnormal, mgeo1.rad));
230 mp2 = ept;
231 mdist = tdist;
232 }
233 tdist = -mgeo1.rad + fabs(utils::PointLineDistance(v_center, v_3, v_1, mu, is_into));
234 if (is_into)
235 if (tdist < 0)
236 if (tdist < mdist) {
237 touched = true;
238 ept = Vadd(v_3, Vmul(Vsub(v_1, v_3), mu));
239 mnormal = Vnorm(Vsub(ept, v_center));
240 mp1 = Vadd(v_center, Vmul(mnormal, mgeo1.rad));
241 mp2 = ept;
242 mdist = tdist;
243 }
244
245 if (touched) {
246 ChCollisionPair mcoll(&mgeo1, &mgeo2, mp1, mp2, mnormal);
247 if (swap_pairs)
248 mcoll.SwapGeometries();
249 mcollider.AddCollisionPair(&mcoll);
250
251 return 1;
252 }
253
254 return 0;
255 }
256
257 class BoxBoxCollisionTest2 {
258 public:
259 // Collision Test.
260 //
261 // @param p_a Center of box A in WCS.
262 // @param p_b Center of box B in WCS
263 // @param R_a Box A's orientation in WCS
264 // @param R_b Box B's orientation in WCS
265 // @param a Extents of box A, i.e. half edge sizes.
266 // @param b Extents of box B, i.e. half edge sizes.
267 // @param envelope The size of the collision envelope. If cloest point are separted by more than this distance then
268 // there is no contact.
269 // @param p Pointer to array of contact points, must have room for at least eight vectors.
270 // @param n Upon return this argument holds the contact normal pointing from box A towards box B.
271 // @param distance Pointer to array of separation (or penetration) distances. Must have room for at least eight
272 // values.
273 //
274 // @return If contacts exist then the return value indicates the number of contacts, if no contacts exist
275 // the return valeu is zero.
276
BoxBoxContacts(Vector & p_a,ChMatrix33<> & R_a,Vector const & ext_a,Vector & p_b,ChMatrix33<> & R_b,Vector const & ext_b,double const & envelope,Vector * p,Vector & n,double * distances)277 static unsigned int BoxBoxContacts(Vector& p_a,
278 ChMatrix33<>& R_a,
279 Vector const& ext_a,
280 Vector& p_b,
281 ChMatrix33<>& R_b,
282 Vector const& ext_b,
283 double const& envelope,
284 Vector* p,
285 Vector& n,
286 double* distances) {
287 assert(p);
288 assert(distances);
289
290 unsigned int cnt = 0;
291
292 //--- Sign lookup table, could be precomputed!!!
293 ChVector<> sign[8];
294 for (unsigned int mask = 0; mask < 8; ++mask) {
295 sign[mask][0] = (mask & 0x0001) ? 1 : -1;
296 sign[mask][1] = ((mask >> 1) & 0x0001) ? 1 : -1;
297 sign[mask][2] = ((mask >> 2) & 0x0001) ? 1 : -1;
298 }
299 //--- extract axis of boxes in WCS
300 ChVector<> A[3];
301 A[0].x() = R_a(0, 0);
302 A[0].y() = R_a(1, 0);
303 A[0].z() = R_a(2, 0);
304 A[1].x() = R_a(0, 1);
305 A[1].y() = R_a(1, 1);
306 A[1].z() = R_a(2, 1);
307 A[2].x() = R_a(0, 2);
308 A[2].y() = R_a(1, 2);
309 A[2].z() = R_a(2, 2);
310 ChVector<> B[3];
311 B[0].x() = R_b(0, 0);
312 B[0].y() = R_b(1, 0);
313 B[0].z() = R_b(2, 0);
314 B[1].x() = R_b(0, 1);
315 B[1].y() = R_b(1, 1);
316 B[1].z() = R_b(2, 1);
317 B[2].x() = R_b(0, 2);
318 B[2].y() = R_b(1, 2);
319 B[2].z() = R_b(2, 2);
320
321 //--- To compat numerical round-offs, these tend to favor edge-edge
322 //--- cases, when one really rather wants a face-case. Truncating
323 //--- seems to let the algorithm pick face cases over edge-edge
324 //--- cases.
325 unsigned int i;
326
327 for (i = 0; i < 3; ++i)
328 for (unsigned int j = 0; j < 3; ++j) {
329 if (fabs(A[i][j]) < 10e-7)
330 A[i][j] = 0.;
331 if (fabs(B[i][j]) < 10e-7)
332 B[i][j] = 0.;
333 }
334
335 ChVector<> a[8];
336 ChVector<> b[8];
337 //--- corner points of boxes in WCS
338 for (i = 0; i < 8; ++i) {
339 a[i] =
340 A[2] * (sign[i][2] * ext_a[2]) + A[1] * (sign[i][1] * ext_a[1]) + A[0] * (sign[i][0] * ext_a[0]) + p_a;
341 b[i] =
342 B[2] * (sign[i][2] * ext_b[2]) + B[1] * (sign[i][1] * ext_b[1]) + B[0] * (sign[i][0] * ext_b[0]) + p_b;
343 }
344 //--- Potential separating axes in WCS
345 ChVector<> axis[15];
346 axis[0] = A[0];
347 axis[1] = A[1];
348 axis[2] = A[2];
349 axis[3] = B[0];
350 axis[4] = B[1];
351 axis[5] = B[2];
352 axis[6].Cross(A[0], B[0]);
353 if (axis[6][0] == 0 && axis[6][1] == 0 && axis[6][2] == 0)
354 axis[6] = A[0];
355 else
356 axis[6] /= sqrt(axis[6].Dot(axis[6]));
357 axis[7].Cross(A[0], B[1]);
358 if (axis[7][0] == 0 && axis[7][1] == 0 && axis[7][2] == 0)
359 axis[7] = A[0];
360 else
361 axis[7] /= sqrt(axis[7].Dot(axis[7]));
362 axis[8].Cross(A[0], B[2]);
363 if (axis[8][0] == 0 && axis[8][1] == 0 && axis[8][2] == 0)
364 axis[8] = A[0];
365 else
366 axis[8] /= sqrt(axis[8].Dot(axis[8]));
367 axis[9].Cross(A[1], B[0]);
368 if (axis[9][0] == 0 && axis[9][1] == 0 && axis[9][2] == 0)
369 axis[9] = A[1];
370 else
371 axis[9] /= sqrt(axis[9].Dot(axis[9]));
372 axis[10].Cross(A[1], B[1]);
373 if (axis[10][0] == 0 && axis[10][1] == 0 && axis[10][2] == 0)
374 axis[10] = A[1];
375 else
376 axis[10] /= sqrt(axis[10].Dot(axis[10]));
377 axis[11].Cross(A[1], B[2]);
378 if (axis[11][0] == 0 && axis[11][1] == 0 && axis[11][2] == 0)
379 axis[11] = A[1];
380 else
381 axis[11] /= sqrt(axis[11].Dot(axis[11]));
382 axis[12].Cross(A[2], B[0]);
383 if (axis[12][0] == 0 && axis[12][1] == 0 && axis[12][2] == 0)
384 axis[12] = A[2];
385 else
386 axis[12] /= sqrt(axis[12].Dot(axis[12]));
387 axis[13].Cross(A[2], B[1]);
388 if (axis[13][0] == 0 && axis[13][1] == 0 && axis[13][2] == 0)
389 axis[13] = A[2];
390 else
391 axis[13] /= sqrt(axis[13].Dot(axis[13]));
392 axis[14].Cross(A[2], B[2]);
393 if (axis[14][0] == 0 && axis[14][1] == 0 && axis[14][2] == 0)
394 axis[14] = A[2];
395 else
396 axis[14] /= sqrt(axis[14].Dot(axis[14]));
397 //--- project vertices of boxes onto separating axis
398 double min_proj_a[15];
399 double min_proj_b[15];
400 double max_proj_a[15];
401 double max_proj_b[15];
402 for (i = 0; i < 15; ++i) {
403 min_proj_a[i] = min_proj_b[i] = 10e30;
404 max_proj_a[i] = max_proj_b[i] = -10e30;
405 }
406 for (i = 0; i < 15; ++i) {
407 for (unsigned int j = 0; j < 8; ++j) {
408 double proj_a = a[j].Dot(axis[i]);
409 double proj_b = b[j].Dot(axis[i]);
410 min_proj_a[i] = ChMin(min_proj_a[i], proj_a);
411 max_proj_a[i] = ChMax(max_proj_a[i], proj_a);
412 min_proj_b[i] = ChMin(min_proj_b[i], proj_b);
413 max_proj_b[i] = ChMax(max_proj_b[i], proj_b);
414 }
415 //--- test for valid separation axis if so return
416 if (min_proj_a[i] > (max_proj_b[i] + envelope) || min_proj_b[i] > (max_proj_a[i] + envelope))
417 return 0;
418 }
419 //--- Compute box overlaps along all 15 separating axes, and determine
420 //--- minimum overlap
421 double overlap[15];
422 double minimum_overlap = -10e30;
423 unsigned int minimum_axis = 15;
424 bool flip_axis[15];
425 //--- Notice that edge-edge cases are testet last, so face cases
426 //--- are favored over edge-edge cases
427 for (i = 0; i < 15; ++i) {
428 flip_axis[i] = false;
429 overlap[i] = 10e30;
430 if (max_proj_a[i] <= min_proj_b[i]) {
431 overlap[i] = ChMin(overlap[i], min_proj_b[i] - max_proj_a[i]);
432 if (overlap[i] > minimum_overlap) {
433 minimum_overlap = overlap[i];
434 minimum_axis = i;
435 flip_axis[i] = false;
436 }
437 }
438 if (max_proj_b[i] <= min_proj_a[i]) {
439 overlap[i] = ChMin(overlap[i], min_proj_a[i] - max_proj_b[i]);
440 if (overlap[i] > minimum_overlap) {
441 minimum_overlap = overlap[i];
442 minimum_axis = i;
443 flip_axis[i] = true;
444 }
445 }
446 if (min_proj_a[i] <= min_proj_b[i] && min_proj_b[i] <= max_proj_a[i]) {
447 overlap[i] = ChMin(overlap[i], -(max_proj_a[i] - min_proj_b[i]));
448 if (overlap[i] > minimum_overlap) {
449 minimum_overlap = overlap[i];
450 minimum_axis = i;
451 flip_axis[i] = false;
452 }
453 }
454 if (min_proj_b[i] <= min_proj_a[i] && min_proj_a[i] <= max_proj_b[i]) {
455 overlap[i] = ChMin(overlap[i], -(max_proj_b[i] - min_proj_a[i]));
456 if (overlap[i] > minimum_overlap) {
457 minimum_overlap = overlap[i];
458 minimum_axis = i;
459 flip_axis[i] = true;
460 }
461 }
462 }
463 if (minimum_overlap > envelope)
464 return 0;
465 //--- Take care of normals, so they point in the correct direction.
466 for (i = 0; i < 15; ++i) {
467 if (flip_axis[i])
468 axis[i] = -axis[i];
469 }
470 //--- At this point we know that a projection along axis[minimum_axis] with
471 //--- value minimum_overlap will lead to non-penetration of the two boxes. We
472 //--- just need to generate the contact points!!!
473 unsigned int corners_inside = 0;
474 unsigned int corners_B_in_A = 0;
475 unsigned int corners_A_in_B = 0;
476 bool AinB[8];
477 bool BinA[8];
478 Coordsys WCStoA(p_a, R_a.Get_A_quaternion());
479 Coordsys WCStoB(p_b, R_b.Get_A_quaternion());
480 Vector eps_a = ext_a + Vector(envelope, envelope, envelope);
481 Vector eps_b = ext_b + Vector(envelope, envelope, envelope);
482 for (i = 0; i < 8; ++i) {
483 Vector a_in_B =
484 WCStoB.TransformParentToLocal(a[i]); // ChTransform<>::TransformParentToLocal(a[i], p_a, R_a);
485 // = WCStoB.TransformParentToLocal(a[i]);
486 Vector abs_a(fabs(a_in_B.x()), fabs(a_in_B.y()), fabs(a_in_B.z()));
487 if (abs_a <= eps_b) {
488 ++corners_inside;
489 ++corners_A_in_B;
490 AinB[i] = true;
491 } else
492 AinB[i] = false;
493 Vector b_in_A =
494 WCStoA.TransformParentToLocal(b[i]); //= ChTransform<>::TransformParentToLocal(b[i], p_b, R_b);
495 // = WCStoA.TransformParentToLocal(b[i]);
496 Vector abs_b(fabs(b_in_A.x()), fabs(b_in_A.y()), fabs(b_in_A.z()));
497 if (abs_b <= eps_a) {
498 ++corners_inside;
499 ++corners_B_in_A;
500 BinA[i] = true;
501 } else
502 BinA[i] = false;
503 }
504 //--- This may indicate an edge-edge case
505 if (minimum_axis >= 6) {
506 //--- However the edge-edge case may not be the best choice,
507 //--- so if we find a corner point of one box being inside
508 //--- the other, we fall back to use the face case with
509 //--- minimum overlap.
510 if (corners_inside) //--- Actually we only need to test end-points of edge for inclusion (4 points instead
511 // of 16!!!).
512 {
513 minimum_overlap = -10e30;
514 minimum_axis = 15;
515 for (unsigned int j = 0; j < 6; ++j) {
516 if (overlap[j] > minimum_overlap) {
517 minimum_overlap = overlap[j];
518 minimum_axis = j;
519 }
520 }
521 }
522 }
523
524 //--- now we can safely pick the contact normal, since we
525 //--- know whether we have a face-case or edge-edge case.
526 n = axis[minimum_axis];
527
528 //--- This is definitely an edge-edge case
529 if (minimum_axis >= 6) {
530 //--- Find a point p_a on the edge from box A.
531 for (int ci = 0; ci < 3; ++ci)
532 if (n.Dot(A[ci]) > 0.)
533 p_a += ext_a[ci] * A[ci];
534 else
535 p_a -= ext_a[ci] * A[ci];
536 //--- Find a point p_b on the edge from box B.
537 for (int ci = 0; ci < 3; ++ci)
538 if (n.Dot(B[ci]) < 0.)
539 p_b += ext_b[ci] * B[ci];
540 else
541 p_b -= ext_b[ci] * B[ci];
542
543 //--- Determine the indices of two unit edge direction vectors (columns
544 //--- of rotation matrices in WCS).
545 int columnA = ((minimum_axis)-6) / 3;
546 int columnB = ((minimum_axis)-6) % 3;
547 double s, t;
548 //--- Compute the edge-parameter values s and t corresponding to the closest
549 //--- points between the two infinite lines parallel to the two edges.
550 ClosestPointsBetweenLines()(p_a, A[columnA], p_b, B[columnB], s, t);
551 //--- Use the edge parameter values to compute the closest
552 //--- points between the two edges.
553 p_a += A[columnA] * s;
554 p_b += B[columnB] * t;
555 //--- Let the contact point be given by the mean of the closest points.
556 p[0] = (p_a + p_b) * .5;
557 distances[0] = overlap[minimum_axis];
558 return 1;
559 }
560 //--- This is a face-``something else'' case, we actually already have taken
561 //--- care of all corner points, but there might be some edge-edge crossings
562 //--- generating contact points
563
564 //--- Make sure that we work in the frame of the box that defines the contact
565 //--- normal. This coordinate frame is nice, because the contact-face is a axis
566 //--- aligned rectangle. We will refer to this frame as the reference frame, and
567 //--- use the letter 'r' or 'R' for it. The other box is named the incident box,
568 //--- its closest face towards the reference face is called the incident face, and
569 //--- is denoted by the letter 'i' or 'I'.
570 Vector *R_r, *R_i; //--- Box direction vectors in WCS
571 Vector ext_r, ext_i; //--- Box extents
572 Vector p_r, p_i; //--- Box centers in WCS
573 bool* incident_inside; //--- corner inside state of incident box.
574 if (minimum_axis < 3) {
575 //--- This means that box A is defining the reference frame
576 R_r = A;
577 R_i = B;
578 p_r = p_a;
579 p_i = p_b;
580 ext_r = ext_a;
581 ext_i = ext_b;
582 incident_inside = BinA;
583 } else {
584 //--- This means that box B is defining the reference frame
585 R_r = B;
586 R_i = A;
587 p_r = p_b;
588 p_i = p_a;
589 ext_r = ext_b;
590 ext_i = ext_a;
591 incident_inside = AinB;
592 }
593 //--- Following vectors are used for computing the corner points of the incident
594 //--- face. At first they are used to determine the axis of the incident box
595 //--- pointing towards the reference box.
596 //---
597 //--- n_r_wcs = normal pointing away from reference frame in WCS coordinates.
598 //--- n_r = normal vector of reference face dotted with axes of incident box.
599 //--- abs_n_r = absolute values of n_r.
600 Vector n_r_wcs, n_r, abs_n_r;
601 if (minimum_axis < 3) {
602 n_r_wcs = n;
603 } else {
604 n_r_wcs = -n;
605 }
606
607 //--- Each of these is a measure for how much the axis' of the incident box
608 //--- points in the direction of n_r_wcs. The largest absolute value give
609 //--- us the axis along which we will find the closest face towards the reference
610 //--- box. The sign will tell us if we should take the positive or negative
611 //--- face to get the closest incident face.
612 n_r[0] = R_i[0].Dot(n_r_wcs);
613 n_r[1] = R_i[1].Dot(n_r_wcs);
614 n_r[2] = R_i[2].Dot(n_r_wcs);
615 abs_n_r[0] = fabs(n_r[0]);
616 abs_n_r[1] = fabs(n_r[1]);
617 abs_n_r[2] = fabs(n_r[2]);
618 //--- Find the largest component of abs_n_r: This corresponds to the normal
619 //--- for the incident face. The axis number is stored in a3. the other
620 //--- axis numbers of the incident face are stored in a1,a2.
621 int a1, a2, a3;
622 if (abs_n_r[1] > abs_n_r[0]) {
623 if (abs_n_r[1] > abs_n_r[2]) {
624 a1 = 2;
625 a2 = 0;
626 a3 = 1;
627 } else {
628 a1 = 0;
629 a2 = 1;
630 a3 = 2;
631 }
632 } else {
633 if (abs_n_r[0] > abs_n_r[2]) {
634 a1 = 1;
635 a2 = 2;
636 a3 = 0;
637 } else {
638 a1 = 0;
639 a2 = 1;
640 a3 = 2;
641 }
642 }
643 //--- Now we have information enough to determine the incident face, that means we can
644 //--- compute the center point of incident face in WCS coordinates.
645
646 int plus_sign[3];
647 Vector center_i_wcs;
648 if (n_r[a3] < 0) {
649 center_i_wcs = p_i + ext_i[a3] * R_i[a3];
650 plus_sign[a3] = 1;
651 } else {
652 center_i_wcs = p_i - ext_i[a3] * R_i[a3];
653 plus_sign[a3] = 0;
654 }
655 //--- Compute difference of center point of incident face with center of reference coordinates.
656 Vector center_ir = center_i_wcs - p_r;
657 //--- Find the normal and non-normal axis numbers of the reference box
658 int code1, code2, code3;
659 if (minimum_axis < 3)
660 code3 = minimum_axis; // 012
661 else
662 code3 = minimum_axis - 3; // 345
663 if (code3 == 0) {
664 code1 = 1;
665 code2 = 2;
666 } else if (code3 == 1) {
667 code1 = 2;
668 code2 = 0;
669 } else {
670 code1 = 0;
671 code2 = 1;
672 }
673 //--- Find the four corners of the incident face, in reference-face coordinates
674 double quad[8]; //--- 2D coordinate of incident face (stored as x,y pairs).
675 bool inside[4]; //--- inside state of the four corners of the quad
676 //--- Project center_ri onto reference-face coordinate system (has origin
677 //--- at the center of the reference face, and the two orthogonal unit vectors
678 //--- denoted by R_r[code1] and R_r[code2] spanning the face-plane).
679 double c1 = R_r[code1].Dot(center_ir);
680 double c2 = R_r[code2].Dot(center_ir);
681 //--- Compute the projections of the axis spanning the incident
682 //--- face, onto the axis spanning the reference face.
683 //---
684 //--- This will allow us to determine the coordinates in the reference-face
685 //--- when we step along a direction of the incident face given by either
686 //--- a1 or a2.
687 double m11 = R_r[code1].Dot(R_i[a1]);
688 double m12 = R_r[code1].Dot(R_i[a2]);
689 double m21 = R_r[code2].Dot(R_i[a1]);
690 double m22 = R_r[code2].Dot(R_i[a2]);
691 {
692 double k1 = m11 * ext_i[a1];
693 double k2 = m21 * ext_i[a1];
694 double k3 = m12 * ext_i[a2];
695 double k4 = m22 * ext_i[a2];
696
697 plus_sign[a1] = 0;
698 plus_sign[a2] = 0;
699 unsigned int mask = ((plus_sign[a1] << a1) | (plus_sign[a2] << a2) | (plus_sign[a3] << a3));
700 inside[0] = incident_inside[mask];
701
702 quad[0] = c1 - k1 - k3;
703 quad[1] = c2 - k2 - k4;
704
705 plus_sign[a1] = 0;
706 plus_sign[a2] = 1;
707 mask = (plus_sign[a1] << a1 | plus_sign[a2] << a2 | plus_sign[a3] << a3);
708 inside[1] = incident_inside[mask];
709
710 quad[2] = c1 - k1 + k3;
711 quad[3] = c2 - k2 + k4;
712
713 plus_sign[a1] = 1;
714 plus_sign[a2] = 1;
715 mask = (plus_sign[a1] << a1 | plus_sign[a2] << a2 | plus_sign[a3] << a3);
716 inside[2] = incident_inside[mask];
717
718 quad[4] = c1 + k1 + k3;
719 quad[5] = c2 + k2 + k4;
720
721 plus_sign[a1] = 1;
722 plus_sign[a2] = 0;
723 mask = (plus_sign[a1] << a1 | plus_sign[a2] << a2 | plus_sign[a3] << a3);
724 inside[3] = incident_inside[mask];
725
726 quad[6] = c1 + k1 - k3;
727 quad[7] = c2 + k2 - k4;
728 }
729 //--- find the size of the reference face
730 double rect[2];
731 rect[0] = ext_r[code1];
732 rect[1] = ext_r[code2];
733
734 //--- Intersect the edges of the incident and the reference face
735 double crossings[16];
736 unsigned int edge_crossings = RectQuadEdgeIntersectionTest()(envelope, rect, quad, inside, crossings);
737 assert(edge_crossings <= 8);
738
739 if (!corners_inside && !edge_crossings)
740 return 0;
741
742 //--- Convert the intersection points into reference-face coordinates,
743 //--- and compute the contact position and depth for each point.
744 double det1 = 1. / (m11 * m22 - m12 * m21);
745 m11 *= det1;
746 m12 *= det1;
747 m21 *= det1;
748 m22 *= det1;
749
750 for (unsigned int j = 0; j < edge_crossings; ++j) {
751 //--- Get coordinates of edge-edge crossing point in reference face coordinate system.
752 double p0 = crossings[j * 2] - c1;
753 double p1 = crossings[j * 2 + 1] - c2;
754 //--- Compute intersection point in (almost) WCS. Actually we have
755 //--- displaced origin to center of reference frame box
756 double k1 = m22 * p0 - m12 * p1;
757 double k2 = -m21 * p0 + m11 * p1;
758 Vector point = center_ir + k1 * R_i[a1] + k2 * R_i[a2];
759 //--- Depth of intersection point
760 double depth = n_r_wcs.Dot(point) - ext_r[code3];
761 if (depth < envelope) {
762 p[cnt] = point + p_r; //--- Move origin from center of reference frame box to WCS
763 distances[cnt] = depth;
764 ++cnt;
765 }
766 }
767 // assert((corners_inside + cnt)<=8);//--- If not we are in serious trouble!!!
768 //--- I think there is a special case, if corners_inside = 8 and
769 //--- corners_in_A = 4 and corners_in_B = 4, then there really
770 //--- can only be 4 contacts???
771
772 if (corners_inside) {
773 unsigned int start_corner_A = cnt;
774 unsigned int end_corner_A = cnt;
775
776 //--- Compute Displacement of contact plane from origin of WCS, the
777 //--- contact plane is equal to the face plane of the reference box
778 double w = ext_r[code3] + n_r_wcs.Dot(p_r);
779
780 if (corners_A_in_B) {
781 for (unsigned int k = 0; k < 8; ++k) {
782 if (AinB[k]) {
783 Vector point = a[k];
784 double depth = n_r_wcs.Dot(point) - w;
785 if (depth < envelope) {
786 p[cnt] = point;
787 distances[cnt] = depth;
788 ++cnt;
789 }
790 }
791 }
792 end_corner_A = cnt;
793 }
794 if (corners_B_in_A) {
795 for (unsigned int k = 0; k < 8; ++k) {
796 if (BinA[k]) {
797 Vector point = b[k];
798 bool redundant = false;
799 for (unsigned int j = start_corner_A; j < end_corner_A; ++j) {
800 if (p[j].Equals(point, envelope)) {
801 redundant = true;
802 break;
803 }
804 }
805 if (redundant)
806 continue;
807 double depth = n_r_wcs.Dot(point) - w;
808 if (depth < envelope) {
809 p[cnt] = point;
810 distances[cnt] = depth;
811 ++cnt;
812 }
813 }
814 }
815 }
816 }
817 // assert(cnt<=8);//--- If not we are in serious trouble!!!
818 return cnt;
819 };
820
821 protected:
822 struct ClosestPointsBetweenLines {
operator ()chrono::collision::BoxBoxCollisionTest2::ClosestPointsBetweenLines823 void operator()(Vector& pA, Vector& uA, Vector& pB, Vector& uB, double& s, double& t) {
824 Vector r = pB - pA;
825 double k = uA.Dot(uB);
826 double q1 = uA.Dot(r);
827 double q2 = -uB.Dot(r);
828 double w = 1 - k * k;
829 s = 0;
830 t = 0;
831 if (fabs(w) > COLL_PRECISION) {
832 s = (q1 + k * q2) / w;
833 t = (q2 + k * q1) / w;
834 }
835 }
836 };
837
838 struct RectQuadEdgeIntersectionTest {
operator ()chrono::collision::BoxBoxCollisionTest2::RectQuadEdgeIntersectionTest839 int operator()(double const& envelope, double* rect, double* quad, bool* inside, double* ret) {
840 int cnt = 0;
841 double* r = ret;
842 {
843 //--- Test the four edges of the quad for crossing the edges of the rect.
844 double qx0, qy0, qx1, qy1, tst;
845 double* q = quad;
846 for (int i = 0; i < 4; ++i) {
847 qx0 = *q;
848 ++q;
849 qy0 = *q;
850 ++q;
851 double* nextq = (i == 3) ? quad : q;
852 qx1 = *nextq;
853 ++nextq;
854 qy1 = *nextq;
855 bool inside0 = inside[i];
856 bool inside1 = inside[(i + 1) % 4];
857 if (inside0 && inside1)
858 continue;
859 double dx = (qx1 - qx0);
860 double dy = (qy1 - qy0);
861 if (dx) {
862 double alpha = dy / dx;
863 tst = -rect[0]; //--- left side
864 if (((qx0 < tst) && (qx1 > tst)) || ((qx0 > tst) && (qx1 < tst))) {
865 double qxt = -rect[0];
866 double qyt = qy0 + (qxt - qx0) * alpha;
867 if ((-rect[1] < qyt) && (qyt < rect[1])) {
868 *r = qxt;
869 ++r;
870 *r = qyt;
871 ++r;
872 ++cnt;
873 }
874 }
875 tst = rect[0];
876 if (((qx0 < tst) && (qx1 > tst)) || ((qx0 > tst) && (qx1 < tst))) {
877 double qxt = rect[0];
878 double qyt = qy0 + (qxt - qx0) * alpha;
879 if ((-rect[1] < qyt) && (qyt < rect[1])) {
880 *r = qxt;
881 ++r;
882 *r = qyt;
883 ++r;
884 ++cnt;
885 }
886 }
887 }
888 if (dy) {
889 double inv_alpha = dx / dy;
890 tst = -rect[1]; //--- bottom side
891 if (((qy0 < tst) && (qy1 > tst)) || ((qy0 > tst) && (qy1 < tst))) {
892 double qyt = -rect[1];
893 double qxt = qx0 + (qyt - qy0) * inv_alpha;
894 if ((-rect[0] < qxt) && (qxt < rect[0])) {
895 *r = qxt;
896 ++r;
897 *r = qyt;
898 ++r;
899 ++cnt;
900 }
901 }
902 tst = rect[1]; //--- top side
903 if (((qy0 < tst) && (qy1 > tst)) || ((qy0 > tst) && (qy1 < tst))) {
904 double qyt = rect[1];
905 double qxt = qx0 + (qyt - qy0) * inv_alpha;
906 if ((-rect[0] < qxt) && (qxt < rect[0])) {
907 *r = qxt;
908 ++r;
909 *r = qyt;
910 ++r;
911 ++cnt;
912 }
913 }
914 }
915 }
916 }
917 return cnt;
918 };
919
920 }; // End of class
921 };
922
ComputeBoxBoxCollisions(geometry::ChBox & mgeo1,ChMatrix33<> * R1,Vector * T1,geometry::ChBox & mgeo2,ChMatrix33<> * R2,Vector * T2,ChNarrowPhaseCollider & mcollider,bool just_intersection)923 int ChGeometryCollider::ComputeBoxBoxCollisions(
924 geometry::ChBox& mgeo1, ///< box 1
925 ChMatrix33<>* R1, ///< absolute rotation of 1st model (with box 1)
926 Vector* T1, ///< absolute position of 1st model (with box 1)
927 geometry::ChBox& mgeo2, ///< box 2
928 ChMatrix33<>* R2, ///< absolute rotation of 2nd model (with box 2)
929 Vector* T2, ///< absolute position of 2nd model (with box 2)
930 ChNarrowPhaseCollider& mcollider, ///< reference to a collider to store contacts into
931 bool just_intersection ///< if true, just report if intersecting (no further calculus on normal, depht, etc.)
932 ) {
933 if (just_intersection) {
934 ChMatrix33<> relRot = R1->transpose() * (*R2);
935 Vector relPos = R1->transpose() * ((*T2) - (*T1));
936
937 if (CHOBB::OBB_Overlap(relRot, relPos, mgeo1.Size, mgeo2.Size)) {
938 ChCollisionPair temp = ChCollisionPair(&mgeo1, &mgeo2);
939 mcollider.AddCollisionPair(&temp);
940 return 1;
941 }
942 } else {
943 double ArrDist[12]; // 8 should be enough
944 Vector ArrP[12]; // 8 should be enough
945 Vector mnormal;
946
947 ChMatrix33<> aBox1Rot = (*R1) * mgeo1.Rot;
948 ChMatrix33<> aBox2Rot = (*R2) * mgeo2.Rot;
949 Vector aBox1Pos = ChTransform<>::TransformLocalToParent(mgeo1.Pos, (*T1), (*R1));
950 Vector aBox2Pos = ChTransform<>::TransformLocalToParent(mgeo2.Pos, (*T2), (*R2));
951
952 int ncontacts = BoxBoxCollisionTest2::BoxBoxContacts(aBox1Pos, aBox1Rot, mgeo1.Size, aBox2Pos, aBox2Rot,
953 mgeo2.Size, CH_COLL_ENVELOPE, ArrP, mnormal, ArrDist);
954
955 for (int i = 0; i < ncontacts; i++) {
956 if (ArrDist[i] < CH_COLL_ENVELOPE) {
957 ChCollisionPair temp = ChCollisionPair(&mgeo1, &mgeo2, // geometries
958 Vadd(ArrP[i], Vmul(mnormal, -ArrDist[i])), ArrP[i],
959 mnormal); // normal
960 mcollider.AddCollisionPair(&temp);
961 }
962 }
963 }
964 return 0;
965 }
966
967 /// SPHERE - BOX specific case
968 ///
ComputeSphereBoxCollisions(geometry::ChSphere & mgeo1,Vector * c1,geometry::ChBox & mgeo2,ChMatrix33<> * R2,Vector * T2,ChNarrowPhaseCollider & mcollider,bool just_intersection,bool swap_pairs)969 int ChGeometryCollider::ComputeSphereBoxCollisions(
970 geometry::ChSphere& mgeo1, ///< sphere
971 Vector* c1, ///< absolute position of center
972 geometry::ChBox& mgeo2, ///< box
973 ChMatrix33<>* R2, ///< absolute rotation of 2nd model (with box)
974 Vector* T2, ///< absolute position of 2nd model (with box)
975 ChNarrowPhaseCollider& mcollider, ///< reference to a collider to store contacts into
976 bool just_intersection, ///< if true, just report if intersecting (no further calculus on normal, depht, etc.)
977 bool swap_pairs ///< if true, pairs are reported as Triangle-Sphere (triangle will be the main ref.)
978 ) {
979 ChMatrix33<> aBoxRot = (*R2) * mgeo2.Rot;
980 Vector aBoxPos = ChTransform<>::TransformLocalToParent(mgeo2.Pos, (*T2), (*R2));
981
982 Vector relC = ChTransform<>::TransformParentToLocal(*c1, aBoxPos, aBoxRot);
983
984 if (just_intersection) {
985 if (((fabs(relC.x()) <= mgeo2.Size.x() + mgeo1.rad) && (fabs(relC.y()) <= mgeo2.Size.y()) &&
986 (fabs(relC.z()) <= mgeo2.Size.z())) ||
987 ((fabs(relC.y()) <= mgeo2.Size.y() + mgeo1.rad) && (fabs(relC.x()) <= mgeo2.Size.x()) &&
988 (fabs(relC.z()) <= mgeo2.Size.z())) ||
989 ((fabs(relC.z()) <= mgeo2.Size.z() + mgeo1.rad) && (fabs(relC.x()) <= mgeo2.Size.x()) &&
990 (fabs(relC.y()) <= mgeo2.Size.y())) ||
991 ((sqrt(pow(fabs(relC.x()) - mgeo2.Size.x(), 2) + pow(fabs(relC.y()) - mgeo2.Size.y(), 2)) <= mgeo1.rad) &&
992 (fabs(relC.z()) <= mgeo2.Size.z())) ||
993 ((sqrt(pow(fabs(relC.y()) - mgeo2.Size.y(), 2) + pow(fabs(relC.z()) - mgeo2.Size.z(), 2)) <= mgeo1.rad) &&
994 (fabs(relC.x()) <= mgeo2.Size.x())) ||
995 ((sqrt(pow(fabs(relC.z()) - mgeo2.Size.z(), 2) + pow(fabs(relC.x()) - mgeo2.Size.x(), 2)) <= mgeo1.rad) &&
996 (fabs(relC.y()) <= mgeo2.Size.y())) ||
997 ((sqrt(pow(fabs(relC.x()) - mgeo2.Size.x(), 2) + pow(fabs(relC.y()) - mgeo2.Size.y(), 2) +
998 pow(fabs(relC.z()) - mgeo2.Size.z(), 2)) <= mgeo1.rad))) {
999 ChCollisionPair temp = ChCollisionPair(&mgeo1, &mgeo2);
1000
1001 mcollider.AddCollisionPair(&temp);
1002 return 1;
1003 }
1004 } else {
1005 Vector pt_loc;
1006 bool done = false;
1007
1008 if ((fabs(relC.x()) <= mgeo2.Size.x() + mgeo1.rad) && (fabs(relC.y()) <= mgeo2.Size.y()) &&
1009 (fabs(relC.z()) <= mgeo2.Size.z())) {
1010 if (relC.x() >= 0) {
1011 pt_loc = relC;
1012 pt_loc.x() = mgeo2.Size.x();
1013 done = true;
1014 } else {
1015 pt_loc = relC;
1016 pt_loc.x() = -mgeo2.Size.x();
1017 done = true;
1018 }
1019 }
1020 if (!done) {
1021 if ((fabs(relC.y()) <= mgeo2.Size.y() + mgeo1.rad) && (fabs(relC.z()) <= mgeo2.Size.z()) &&
1022 (fabs(relC.x()) <= mgeo2.Size.x())) {
1023 if (relC.y() >= 0) {
1024 pt_loc = relC;
1025 pt_loc.y() = mgeo2.Size.y();
1026 done = true;
1027 } else {
1028 pt_loc = relC;
1029 pt_loc.y() = -mgeo2.Size.y();
1030 done = true;
1031 }
1032 }
1033 }
1034 if (!done) {
1035 if ((fabs(relC.z()) <= mgeo2.Size.z() + mgeo1.rad) && (fabs(relC.x()) <= mgeo2.Size.x()) &&
1036 (fabs(relC.y()) <= mgeo2.Size.y())) {
1037 if (relC.z() >= 0) {
1038 pt_loc = relC;
1039 pt_loc.z() = mgeo2.Size.z();
1040 done = true;
1041 } else {
1042 pt_loc = relC;
1043 pt_loc.z() = -mgeo2.Size.z();
1044 done = true;
1045 }
1046 }
1047 }
1048 if (!done) {
1049 if ((sqrt(pow(fabs(relC.x()) - mgeo2.Size.x(), 2) + pow(fabs(relC.y()) - mgeo2.Size.y(), 2)) <=
1050 mgeo1.rad) &&
1051 (fabs(relC.z()) <= mgeo2.Size.z())) {
1052 if (relC.x() > 0) {
1053 if (relC.y() > 0) {
1054 pt_loc = relC;
1055 pt_loc.x() = mgeo2.Size.x();
1056 pt_loc.y() = mgeo2.Size.y();
1057 done = true;
1058 } else {
1059 pt_loc = relC;
1060 pt_loc.x() = mgeo2.Size.x();
1061 pt_loc.y() = -mgeo2.Size.y();
1062 done = true;
1063 }
1064 } else if (relC.y() > 0) {
1065 pt_loc = relC;
1066 pt_loc.x() = -mgeo2.Size.x();
1067 pt_loc.y() = mgeo2.Size.y();
1068 done = true;
1069 } else {
1070 pt_loc = relC;
1071 pt_loc.x() = -mgeo2.Size.x();
1072 pt_loc.y() = -mgeo2.Size.y();
1073 done = true;
1074 }
1075 }
1076 }
1077 if (!done) {
1078 if ((sqrt(pow(fabs(relC.y()) - mgeo2.Size.y(), 2) + pow(fabs(relC.z()) - mgeo2.Size.z(), 2)) <=
1079 mgeo1.rad) &&
1080 (fabs(relC.x()) <= mgeo2.Size.x())) {
1081 if (relC.y() > 0) {
1082 if (relC.z() > 0) {
1083 pt_loc = relC;
1084 pt_loc.y() = mgeo2.Size.y();
1085 pt_loc.z() = mgeo2.Size.z();
1086 done = true;
1087 } else {
1088 pt_loc = relC;
1089 pt_loc.y() = mgeo2.Size.y();
1090 pt_loc.z() = -mgeo2.Size.z();
1091 done = true;
1092 }
1093 } else if (relC.z() > 0) {
1094 pt_loc = relC;
1095 pt_loc.y() = -mgeo2.Size.y();
1096 pt_loc.z() = mgeo2.Size.z();
1097 done = true;
1098 } else {
1099 pt_loc = relC;
1100 pt_loc.y() = -mgeo2.Size.y();
1101 pt_loc.z() = -mgeo2.Size.z();
1102 done = true;
1103 }
1104 }
1105 }
1106 if (!done) {
1107 if ((sqrt(pow(fabs(relC.z()) - mgeo2.Size.z(), 2) + pow(fabs(relC.x()) - mgeo2.Size.x(), 2)) <=
1108 mgeo1.rad) &&
1109 (fabs(relC.y()) <= mgeo2.Size.y())) {
1110 if (relC.z() > 0) {
1111 if (relC.x() > 0) {
1112 pt_loc = relC;
1113 pt_loc.z() = mgeo2.Size.z();
1114 pt_loc.x() = mgeo2.Size.x();
1115 done = true;
1116 } else {
1117 pt_loc = relC;
1118 pt_loc.z() = mgeo2.Size.z();
1119 pt_loc.x() = -mgeo2.Size.x();
1120 done = true;
1121 }
1122 } else if (relC.x() > 0) {
1123 pt_loc = relC;
1124 pt_loc.z() = -mgeo2.Size.z();
1125 pt_loc.x() = mgeo2.Size.x();
1126 done = true;
1127 } else {
1128 pt_loc = relC;
1129 pt_loc.z() = -mgeo2.Size.z();
1130 pt_loc.x() = -mgeo2.Size.x();
1131 done = true;
1132 }
1133 }
1134 }
1135 for (int i = 1; i <= 8; i++) {
1136 if (done)
1137 break;
1138 Vector loc_corner = ChTransform<>::TransformParentToLocal(mgeo2.GetPn(i), mgeo2.Pos, mgeo2.Rot);
1139 if (Vlength(loc_corner - relC) <= mgeo1.rad) {
1140 pt_loc = loc_corner;
1141 done = true;
1142 }
1143 }
1144
1145 if (done) {
1146 Vector pt_2 = ChTransform<>::TransformLocalToParent(pt_loc, aBoxPos, aBoxRot);
1147 Vector mnormal = Vnorm(pt_2 - *c1);
1148 if (Vdot(mnormal, aBoxPos - *c1) < 0)
1149 mnormal = -mnormal;
1150 Vector pt_1 = *c1 + (mnormal * mgeo1.rad);
1151 ChCollisionPair mcoll(&mgeo1, &mgeo2, // geometries
1152 pt_1, // p1
1153 pt_2, // p2
1154 mnormal // normal
1155 );
1156 if (swap_pairs)
1157 mcoll.SwapGeometries();
1158 mcollider.AddCollisionPair(&mcoll);
1159 }
1160 }
1161
1162 return 0;
1163 }
1164
1165 /// BOX - TRIANGLE specific case
1166 ///
ComputeBoxTriangleCollisions(geometry::ChBox & mbox,ChMatrix33<> * R1,Vector * T1,geometry::ChTriangle & mtri,ChMatrix33<> * R2,Vector * T2,ChNarrowPhaseCollider & mcollider,bool just_intersection,bool swap_pairs)1167 int ChGeometryCollider::ComputeBoxTriangleCollisions(
1168 geometry::ChBox& mbox, ///< box
1169 ChMatrix33<>* R1, ///< absolute rotation of 1st model (with box)
1170 Vector* T1, ///< absolute position of 1st model (with box)
1171 geometry::ChTriangle& mtri, ///< triangle
1172 ChMatrix33<>* R2, ///< absolute rotation of 2nd model (with triangle)
1173 Vector* T2, ///< absolute position of 2nd model (with triangle)
1174 ChNarrowPhaseCollider& mcollider, ///< reference to a collider to store contacts into
1175 bool just_intersection, ///< if true, just report if intersecting (no further calculus on normal, depht, etc.)
1176 bool swap_pairs ///< if true, pairs are reported as Triangle-Box (triangle will be the main ref.)
1177 ) {
1178 Vector v_1 = ChTransform<>::TransformLocalToParent(mtri.p1, *T2, *R2);
1179 Vector v_2 = ChTransform<>::TransformLocalToParent(mtri.p2, *T2, *R2);
1180 Vector v_3 = ChTransform<>::TransformLocalToParent(mtri.p3, *T2, *R2);
1181
1182 ChMatrix33<> aBoxRot = (*R1) * mbox.Rot;
1183 Vector aBoxPos = ChTransform<>::TransformLocalToParent(mbox.Pos, (*T1), (*R1));
1184
1185 Vector v_b[3];
1186 v_b[0] = ChTransform<>::TransformParentToLocal(v_1, aBoxPos, aBoxRot);
1187 v_b[1] = ChTransform<>::TransformParentToLocal(v_2, aBoxPos, aBoxRot);
1188 v_b[2] = ChTransform<>::TransformParentToLocal(v_3, aBoxPos, aBoxRot);
1189
1190 double distc;
1191 ////bool hit = false;
1192 Vector P1_b;
1193 Vector P2_b;
1194 Vector P1_w;
1195 Vector P2_w;
1196
1197 int nv;
1198
1199 for (nv = 0; nv < 3; nv++) // triangle points against cube volume
1200 {
1201 distc = -10e24;
1202 P1_b = v_b[nv];
1203 P2_b = v_b[nv];
1204 if ((fabs(v_b[nv].x()) <= mbox.Size.x()) && (fabs(v_b[nv].y()) <= mbox.Size.y()) &&
1205 (fabs(v_b[nv].z()) <= mbox.Size.z())) {
1206 ////hit = true;
1207 if (v_b[nv].x() - mbox.Size.x() > distc) {
1208 distc = v_b[nv].x() - mbox.Size.x();
1209 P1_b = v_b[nv];
1210 P1_b.x() = mbox.Size.x();
1211 }
1212 if (-v_b[nv].x() - mbox.Size.x() > distc) {
1213 distc = -v_b[nv].x() - mbox.Size.x();
1214 P1_b = v_b[nv];
1215 P1_b.x() = -mbox.Size.x();
1216 }
1217 if (v_b[nv].y() - mbox.Size.y() > distc) {
1218 distc = v_b[nv].y() - mbox.Size.y();
1219 P1_b = v_b[nv];
1220 P1_b.y() = mbox.Size.y();
1221 }
1222 if (-v_b[nv].y() - mbox.Size.y() > distc) {
1223 distc = -v_b[nv].y() - mbox.Size.y();
1224 P1_b = v_b[nv];
1225 P1_b.y() = -mbox.Size.y();
1226 }
1227 if (v_b[nv].z() - mbox.Size.z() > distc) {
1228 distc = v_b[nv].z() - mbox.Size.z();
1229 P1_b = v_b[nv];
1230 P1_b.z() = mbox.Size.z();
1231 }
1232 if (-v_b[nv].z() - mbox.Size.z() > distc) {
1233 distc = -v_b[nv].z() - mbox.Size.z();
1234 P1_b = v_b[nv];
1235 P1_b.z() = -mbox.Size.z();
1236 }
1237
1238 P1_w = ChTransform<>::TransformLocalToParent(P1_b, aBoxPos, aBoxRot);
1239 P2_w = ChTransform<>::TransformLocalToParent(P2_b, aBoxPos, aBoxRot);
1240 Vector mnormal = Vnorm(P1_w - P2_w);
1241 ChCollisionPair mcoll(&mbox, &mtri, // geometries
1242 P1_w, // p1
1243 P2_w, // p2
1244 mnormal // normal
1245 );
1246 if (swap_pairs)
1247 mcoll.SwapGeometries();
1248 mcollider.AddCollisionPair(&mcoll);
1249 }
1250 }
1251
1252 for (nv = 0; nv < 8; nv++) // box points against triangle
1253 {
1254 distc = -10e24;
1255
1256 Vector v_w = ChTransform<>::TransformLocalToParent(mbox.GetPn(nv), *T1, *R1);
1257
1258 P1_w = v_w;
1259 P2_w = v_w;
1260 bool is_into;
1261 double mu, mv;
1262
1263 /*double dist =*/ utils::PointTriangleDistance(P1_w, v_1, v_2, v_3, mu, mv, is_into, P2_w);
1264
1265 Vector dir = P2_w - P1_w;
1266
1267 if (is_into)
1268 if (Vdot(dir, mtri.GetNormal()) < 0) {
1269 Vector mnormal = Vnorm(P1_w - P2_w);
1270 ChCollisionPair mcoll(&mbox, &mtri, // geometries
1271 P1_w, // p1
1272 P2_w, // p2
1273 mnormal // normal
1274 );
1275 if (swap_pairs)
1276 mcoll.SwapGeometries();
1277 mcollider.AddCollisionPair(&mcoll);
1278 ////hit = true;
1279 }
1280 }
1281
1282 return 0;
1283 }
1284
1285 } // end namespace collision
1286 } // end namespace chrono
1287