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