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: Alessandro Tasora, Radu Serban
13 // =============================================================================
14
15 #include <cstdio>
16
17 #include "chrono/core/ChTransform.h"
18 #include "chrono/geometry/ChBox.h"
19
20 namespace chrono {
21 namespace geometry {
22
23 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChBox)24 CH_FACTORY_REGISTER(ChBox)
25
26 ChBox::ChBox(const ChVector<>& mpos, const ChMatrix33<>& mrot, const ChVector<>& mlengths)
27 : Pos(mpos), Rot(mrot), Size(0.5 * mlengths) {}
28
ChBox(const ChVector<> & mC0,const ChVector<> & mC1,const ChVector<> & mC2,const ChVector<> & mC3)29 ChBox::ChBox(const ChVector<>& mC0, const ChVector<>& mC1, const ChVector<>& mC2, const ChVector<>& mC3) {
30 ChVector<> D1 = Vsub(mC1, mC0);
31 ChVector<> D2 = Vsub(mC2, mC0);
32 ChVector<> D3 = Vsub(mC3, mC0);
33 ChVector<> C0 = mC0;
34
35 ChVector<> zax;
36 zax.Cross(D1, D2);
37 if (Vdot(D3, zax) < 0) {
38 C0 += D3;
39 D3 = -D3;
40 }
41
42 this->Size.x() = 0.5 * Vlength(D1);
43 this->Size.y() = 0.5 * Vlength(D2);
44 this->Size.z() = 0.5 * Vlength(D3);
45 this->Pos = Vadd(Vadd(Vadd(C0, Vmul(D1, 0.5)), Vmul(D2, 0.5)), Vmul(D3, 0.5));
46 this->Rot.Set_A_axis(Vnorm(D1), Vnorm(D2), Vnorm(D3));
47 }
48
ChBox(const ChBox & source)49 ChBox::ChBox(const ChBox& source) {
50 Pos = source.Pos;
51 Size = source.Size;
52 Rot = source.Rot;
53 }
54
Evaluate(ChVector<> & pos,const double parU,const double parV,const double parW) const55 void ChBox::Evaluate(ChVector<>& pos, const double parU, const double parV, const double parW) const {
56 ChVector<> Pr;
57 Pr.x() = Size.x() * (parU - 0.5);
58 Pr.y() = Size.y() * (parV - 0.5);
59 Pr.z() = Size.z() * (parW - 0.5);
60 pos = ChTransform<>::TransformLocalToParent(Pr, Pos, Rot);
61 }
62
GetP1() const63 ChVector<> ChBox::GetP1() const {
64 ChVector<> P1r;
65 P1r.x() = +Size.x();
66 P1r.y() = +Size.y();
67 P1r.z() = +Size.z();
68 return ChTransform<>::TransformLocalToParent(P1r, Pos, Rot);
69 }
GetP2() const70 ChVector<> ChBox::GetP2() const {
71 ChVector<> P2r;
72 P2r.x() = -Size.x();
73 P2r.y() = +Size.y();
74 P2r.z() = +Size.z();
75 return ChTransform<>::TransformLocalToParent(P2r, Pos, Rot);
76 }
GetP3() const77 ChVector<> ChBox::GetP3() const {
78 ChVector<> P3r;
79 P3r.x() = -Size.x();
80 P3r.y() = -Size.y();
81 P3r.z() = +Size.z();
82 return ChTransform<>::TransformLocalToParent(P3r, Pos, Rot);
83 }
GetP4() const84 ChVector<> ChBox::GetP4() const {
85 ChVector<> P4r;
86 P4r.x() = +Size.x();
87 P4r.y() = -Size.y();
88 P4r.z() = +Size.z();
89 return ChTransform<>::TransformLocalToParent(P4r, Pos, Rot);
90 }
GetP5() const91 ChVector<> ChBox::GetP5() const {
92 ChVector<> P5r;
93 P5r.x() = +Size.x();
94 P5r.y() = +Size.y();
95 P5r.z() = -Size.z();
96 return ChTransform<>::TransformLocalToParent(P5r, Pos, Rot);
97 }
GetP6() const98 ChVector<> ChBox::GetP6() const {
99 ChVector<> P6r;
100 P6r.x() = -Size.x();
101 P6r.y() = +Size.y();
102 P6r.z() = -Size.z();
103 return ChTransform<>::TransformLocalToParent(P6r, Pos, Rot);
104 }
GetP7() const105 ChVector<> ChBox::GetP7() const {
106 ChVector<> P7r;
107 P7r.x() = -Size.x();
108 P7r.y() = -Size.y();
109 P7r.z() = -Size.z();
110 return ChTransform<>::TransformLocalToParent(P7r, Pos, Rot);
111 }
GetP8() const112 ChVector<> ChBox::GetP8() const {
113 ChVector<> P8r;
114 P8r.x() = +Size.x();
115 P8r.y() = -Size.y();
116 P8r.z() = -Size.z();
117 return ChTransform<>::TransformLocalToParent(P8r, Pos, Rot);
118 }
119
GetPn(int ipoint) const120 ChVector<> ChBox::GetPn(int ipoint) const {
121 switch (ipoint) {
122 case 1:
123 return GetP1();
124 case 2:
125 return GetP2();
126 case 3:
127 return GetP3();
128 case 4:
129 return GetP4();
130 case 5:
131 return GetP5();
132 case 6:
133 return GetP6();
134 case 7:
135 return GetP7();
136 case 8:
137 return GetP8();
138 default:
139 return GetP1();
140 }
141 }
142
GetBoundingBox(double & xmin,double & xmax,double & ymin,double & ymax,double & zmin,double & zmax,ChMatrix33<> * bbRot) const143 void ChBox::GetBoundingBox(double& xmin,
144 double& xmax,
145 double& ymin,
146 double& ymax,
147 double& zmin,
148 double& zmax,
149 ChMatrix33<>* bbRot) const {
150 // TO OPTIMIZE for speed
151
152 ChVector<> p1, p2, p3, p4, p5, p6, p7, p8;
153
154 xmax = ymax = zmax = -10e20;
155 xmin = ymin = zmin = +10e20;
156
157 if (bbRot == NULL) {
158 p1 = GetP1();
159 p2 = GetP2();
160 p3 = GetP3();
161 p4 = GetP4();
162 p5 = GetP5();
163 p6 = GetP6();
164 p7 = GetP7();
165 p8 = GetP8();
166 } else {
167 p1 = bbRot->transpose() * GetP1();
168 p2 = bbRot->transpose() * GetP2();
169 p3 = bbRot->transpose() * GetP3();
170 p4 = bbRot->transpose() * GetP4();
171 p5 = bbRot->transpose() * GetP5();
172 p6 = bbRot->transpose() * GetP6();
173 p7 = bbRot->transpose() * GetP7();
174 p8 = bbRot->transpose() * GetP8();
175 }
176
177 if (p1.x() > xmax)
178 xmax = p1.x();
179 if (p1.y() > ymax)
180 ymax = p1.y();
181 if (p1.z() > zmax)
182 zmax = p1.z();
183 if (p2.x() > xmax)
184 xmax = p2.x();
185 if (p2.y() > ymax)
186 ymax = p2.y();
187 if (p2.z() > zmax)
188 zmax = p2.z();
189 if (p3.x() > xmax)
190 xmax = p3.x();
191 if (p3.y() > ymax)
192 ymax = p3.y();
193 if (p3.z() > zmax)
194 zmax = p3.z();
195 if (p4.x() > xmax)
196 xmax = p4.x();
197 if (p4.y() > ymax)
198 ymax = p4.y();
199 if (p4.z() > zmax)
200 zmax = p4.z();
201 if (p5.x() > xmax)
202 xmax = p5.x();
203 if (p5.y() > ymax)
204 ymax = p5.y();
205 if (p5.z() > zmax)
206 zmax = p5.z();
207 if (p6.x() > xmax)
208 xmax = p6.x();
209 if (p6.y() > ymax)
210 ymax = p6.y();
211 if (p6.z() > zmax)
212 zmax = p6.z();
213 if (p7.x() > xmax)
214 xmax = p7.x();
215 if (p7.y() > ymax)
216 ymax = p7.y();
217 if (p7.z() > zmax)
218 zmax = p7.z();
219 if (p8.x() > xmax)
220 xmax = p8.x();
221 if (p8.y() > ymax)
222 ymax = p8.y();
223 if (p8.z() > zmax)
224 zmax = p8.z();
225
226 if (p1.x() < xmin)
227 xmin = p1.x();
228 if (p1.y() < ymin)
229 ymin = p1.y();
230 if (p1.z() < zmin)
231 zmin = p1.z();
232 if (p2.x() < xmin)
233 xmin = p2.x();
234 if (p2.y() < ymin)
235 ymin = p2.y();
236 if (p2.z() < zmin)
237 zmin = p2.z();
238 if (p3.x() < xmin)
239 xmin = p3.x();
240 if (p3.y() < ymin)
241 ymin = p3.y();
242 if (p3.z() < zmin)
243 zmin = p3.z();
244 if (p4.x() < xmin)
245 xmin = p4.x();
246 if (p4.y() < ymin)
247 ymin = p4.y();
248 if (p4.z() < zmin)
249 zmin = p4.z();
250 if (p5.x() < xmin)
251 xmin = p5.x();
252 if (p5.y() < ymin)
253 ymin = p5.y();
254 if (p5.z() < zmin)
255 zmin = p5.z();
256 if (p6.x() < xmin)
257 xmin = p6.x();
258 if (p6.y() < ymin)
259 ymin = p6.y();
260 if (p6.z() < zmin)
261 zmin = p6.z();
262 if (p7.x() < xmin)
263 xmin = p7.x();
264 if (p7.y() < ymin)
265 ymin = p7.y();
266 if (p7.z() < zmin)
267 zmin = p7.z();
268 if (p8.x() < xmin)
269 xmin = p8.x();
270 if (p8.y() < ymin)
271 ymin = p8.y();
272 if (p8.z() < zmin)
273 zmin = p8.z();
274 }
275
CovarianceMatrix(ChMatrix33<> & C) const276 void ChBox::CovarianceMatrix(ChMatrix33<>& C) const {
277 ChVector<> p1, p2, p3, p4, p5, p6, p7, p8;
278
279 p1 = GetP1();
280 p2 = GetP2();
281 p3 = GetP3();
282 p4 = GetP4();
283 p5 = GetP5();
284 p6 = GetP6();
285 p7 = GetP7();
286 p8 = GetP8();
287
288 C.setZero();
289 C(0, 0) = p1.x() * p1.x() + p2.x() * p2.x() + p3.x() * p3.x() + p4.x() * p4.x() + p5.x() * p5.x() +
290 p6.x() * p6.x() + p7.x() * p7.x() + p8.x() * p8.x();
291 C(1, 1) = p1.y() * p1.y() + p2.y() * p2.y() + p3.y() * p3.y() + p4.y() * p4.y() + p5.y() * p5.y() +
292 p6.y() * p6.y() + p7.y() * p7.y() + p8.y() * p8.y();
293 C(2, 2) = p1.z() * p1.z() + p2.z() * p2.z() + p3.z() * p3.z() + p4.z() * p4.z() + p5.z() * p5.z() +
294 p6.z() * p6.z() + p7.z() * p7.z() + p8.z() * p8.z();
295 C(0, 1) = p1.x() * p1.y() + p2.x() * p2.y() + p3.x() * p3.y() + p4.x() * p4.y() + p5.x() * p5.y() +
296 p6.x() * p6.y() + p7.x() * p7.y() + p8.x() * p8.y();
297 C(0, 2) = p1.x() * p1.z() + p2.x() * p2.z() + p3.x() * p3.z() + p4.x() * p4.z() + p5.x() * p5.z() +
298 p6.x() * p6.z() + p7.x() * p7.z() + p8.x() * p8.z();
299 C(1, 2) = p1.y() * p1.z() + p2.y() * p2.z() + p3.y() * p3.z() + p4.y() * p4.z() + p5.y() * p5.z() +
300 p6.y() * p6.z() + p7.y() * p7.z() + p8.y() * p8.z();
301 }
302
ArchiveOUT(ChArchiveOut & marchive)303 void ChBox::ArchiveOUT(ChArchiveOut& marchive) {
304 // version number
305 marchive.VersionWrite<ChBox>();
306 // serialize parent class
307 ChVolume::ArchiveOUT(marchive);
308 // serialize all member data:
309 marchive << CHNVP(Pos);
310 marchive << CHNVP(Rot);
311 ChVector<> Lengths = GetLengths();
312 marchive << CHNVP(Lengths); // avoid storing 'Size', i.e. half lengths, because less intuitive
313 }
314
ArchiveIN(ChArchiveIn & marchive)315 void ChBox::ArchiveIN(ChArchiveIn& marchive) {
316 // version number
317 /*int version =*/ marchive.VersionRead<ChBox>();
318 // deserialize parent class
319 ChVolume::ArchiveIN(marchive);
320 // stream in all member data:
321 marchive >> CHNVP(Pos);
322 ////marchive >> CHNVP(Rot);
323 ChVector<> Lengths;
324 marchive >> CHNVP(Lengths); // avoid storing 'Size', i.e. half lengths, because less intuitive
325 SetLengths(Lengths);
326 }
327
328 } // end namespace geometry
329 } // end namespace chrono
330