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