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