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 <cstdlib>
14 #include <cmath>
15
16 #include "chrono/collision/edgetempest/ChCOBB.h"
17 #include "chrono/collision/edgetempest/ChCMatVec.h"
18 #include "chrono/collision/ChCollisionPair.h"
19
20 namespace chrono {
21 namespace collision {
22
CHOBB()23 CHOBB::CHOBB() {
24 first_child = 0;
25
26 Rot.setIdentity();
27
28 To = VNULL;
29 d = VNULL;
30 }
31
~CHOBB()32 CHOBB::~CHOBB() {
33 }
34
FitToGeometries(ChMatrix33<> & O,std::vector<geometry::ChGeometry * > mgeos,int firstgeo,int ngeos,double envelope)35 void CHOBB::FitToGeometries(ChMatrix33<>& O,
36 std::vector<geometry::ChGeometry*> mgeos,
37 int firstgeo,
38 int ngeos,
39 double envelope) {
40 // store orientation
41
42 this->Rot = O;
43
44 double minx, maxx, miny, maxy, minz, maxz;
45 minx = miny = minz = +10e20;
46 maxx = maxy = maxz = -10e20;
47
48 Vector c;
49
50 geometry::ChGeometry* nit;
51 for (int count = 0; count < ngeos; ++count) {
52 nit = mgeos[firstgeo + count];
53 if (nit) {
54 nit->InflateBoundingBox(minx, maxx, miny, maxy, minz, maxz, &Rot);
55 }
56 }
57
58 c.x() = 0.5 * (maxx + minx);
59 c.y() = 0.5 * (maxy + miny);
60 c.z() = 0.5 * (maxz + minz);
61 To = Rot * c;
62
63 d.x() = 0.5 * (maxx - minx) + envelope;
64 d.y() = 0.5 * (maxy - miny) + envelope;
65 d.z() = 0.5 * (maxz - minz) + envelope;
66 }
67
OBB_Overlap(ChMatrix33<> & B,Vector T,CHOBB * b1,CHOBB * b2)68 bool CHOBB::OBB_Overlap(ChMatrix33<>& B, Vector T, CHOBB* b1, CHOBB* b2) {
69 return OBB_Overlap(B, T, b1->d, b2->d);
70 }
71
OBB_Overlap(ChMatrix33<> & B,Vector T,Vector a,Vector b)72 bool CHOBB::OBB_Overlap(ChMatrix33<>& B, Vector T, Vector a, Vector b) {
73 double t, s;
74 int r;
75 static ChMatrix33<> Bf;
76 const double reps = (double)1e-6;
77
78 // Vector& a = b1->d;
79 // Vector& b = b2->d;
80
81 // Bf = fabs(B)
82 Bf(0, 0) = myfabs(B(0, 0));
83 Bf(0, 0) += reps;
84 Bf(0, 1) = myfabs(B(0, 1));
85 Bf(0, 1) += reps;
86 Bf(0, 2) = myfabs(B(0, 2));
87 Bf(0, 2) += reps;
88 Bf(1, 0) = myfabs(B(1, 0));
89 Bf(1, 0) += reps;
90 Bf(1, 1) = myfabs(B(1, 1));
91 Bf(1, 1) += reps;
92 Bf(1, 2) = myfabs(B(1, 2));
93 Bf(1, 2) += reps;
94 Bf(2, 0) = myfabs(B(2, 0));
95 Bf(2, 0) += reps;
96 Bf(2, 1) = myfabs(B(2, 1));
97 Bf(2, 1) += reps;
98 Bf(2, 2) = myfabs(B(2, 2));
99 Bf(2, 2) += reps;
100
101 // if any of these tests are one-sided, then the polyhedron are disjoint
102 r = 1;
103
104 // A1 x A2 = A0
105 t = myfabs(T.x());
106
107 r &= (t <= (a.x() + b.x() * Bf(0, 0) + b.y() * Bf(0, 1) + b.z() * Bf(0, 2)));
108 if (!r)
109 return false;
110
111 // B1 x B2 = B0
112 s = T.x() * B(0, 0) + T.y() * B(1, 0) + T.z() * B(2, 0);
113 t = myfabs(s);
114
115 r &= (t <= (b.x() + a.x() * Bf(0, 0) + a.y() * Bf(1, 0) + a.z() * Bf(2, 0)));
116 if (!r)
117 return false;
118
119 // A2 x A0 = A1
120 t = myfabs(T.y());
121
122 r &= (t <= (a.y() + b.x() * Bf(1, 0) + b.y() * Bf(1, 1) + b.z() * Bf(1, 2)));
123 if (!r)
124 return false;
125
126 // A0 x A1 = A2
127 t = myfabs(T.z());
128
129 r &= (t <= (a.z() + b.x() * Bf(2, 0) + b.y() * Bf(2, 1) + b.z() * Bf(2, 2)));
130 if (!r)
131 return false;
132
133 // B2 x B0 = B1
134 s = T.x() * B(0, 1) + T.y() * B(1, 1) + T.z() * B(2, 1);
135 t = myfabs(s);
136
137 r &= (t <= (b.y() + a.x() * Bf(0, 1) + a.y() * Bf(1, 1) + a.z() * Bf(2, 1)));
138 if (!r)
139 return false;
140
141 // B0 x B1 = B2
142 s = T.x() * B(0, 2) + T.y() * B(1, 2) + T.z() * B(2, 2);
143 t = myfabs(s);
144
145 r &= (t <= (b.z() + a.x() * Bf(0, 2) + a.y() * Bf(1, 2) + a.z() * Bf(2, 2)));
146 if (!r)
147 return false;
148
149 // A0 x B0
150 s = T.z() * B(1, 0) - T.y() * B(2, 0);
151 t = myfabs(s);
152
153 r &= (t <= (a.y() * Bf(2, 0) + a.z() * Bf(1, 0) + b.y() * Bf(0, 2) +
154 b.z() * Bf(0, 1)));
155 if (!r)
156 return false;
157
158 // A0 x B1
159 s = T.z() * B(1, 1) - T.y() * B(2, 1);
160 t = myfabs(s);
161
162 r &= (t <= (a.y() * Bf(2, 1) + a.z() * Bf(1, 1) + b.x() * Bf(0, 2) +
163 b.z() * Bf(0, 0)));
164 if (!r)
165 return false;
166
167 // A0 x B2
168 s = T.z() * B(1, 2) - T.y() * B(2, 2);
169 t = myfabs(s);
170
171 r &= (t <= (a.y() * Bf(2, 2) + a.z() * Bf(1, 2) + b.x() * Bf(0, 1) +
172 b.y() * Bf(0, 0)));
173 if (!r)
174 return false;
175
176 // A1 x B0
177 s = T.x() * B(2, 0) - T.z() * B(0, 0);
178 t = myfabs(s);
179
180 r &= (t <= (a.x() * Bf(2, 0) + a.z() * Bf(0, 0) + b.y() * Bf(1, 2) +
181 b.z() * Bf(1, 1)));
182 if (!r)
183 return false;
184
185 // A1 x B1
186 s = T.x() * B(2, 1) - T.z() * B(0, 1);
187 t = myfabs(s);
188
189 r &= (t <= (a.x() * Bf(2, 1) + a.z() * Bf(0, 1) + b.x() * Bf(1, 2) +
190 b.z() * Bf(1, 0)));
191 if (!r)
192 return false;
193
194 // A1 x B2
195 s = T.x() * B(2, 2) - T.z() * B(0, 2);
196 t = myfabs(s);
197
198 r &= (t <= (a.x() * Bf(2, 2) + a.z() * Bf(0, 2) + b.x() * Bf(1, 1) +
199 b.y() * Bf(1, 0)));
200 if (!r)
201 return false;
202
203 // A2 x B0
204 s = T.y() * B(0, 0) - T.x() * B(1, 0);
205 t = myfabs(s);
206
207 r &= (t <= (a.x() * Bf(1, 0) + a.y() * Bf(0, 0) + b.y() * Bf(2, 2) +
208 b.z() * Bf(2, 1)));
209 if (!r)
210 return false;
211
212 // A2 x B1
213 s = T.y() * B(0, 1) - T.x() * B(1, 1);
214 t = myfabs(s);
215
216 r &= (t <= (a.x() * Bf(1, 1) + a.y() * Bf(0, 1) + b.x() * Bf(2, 2) +
217 b.z() * Bf(2, 0)));
218 if (!r)
219 return false;
220
221 // A2 x B2
222 s = T.y() * B(0, 2) - T.x() * B(1, 2);
223 t = myfabs(s);
224
225 r &= (t <= (a.x() * Bf(1, 2) + a.y() * Bf(0, 2) + b.x() * Bf(2, 1) +
226 b.y() * Bf(2, 0)));
227 if (!r)
228 return false;
229
230 return true; // no separation: BV collide!!
231 }
232
233 } // end namespace collision
234 } // end namespace chrono
235