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: Radu Serban, Hammad Mazhar, Arman Pazouki
13 // =============================================================================
14 //
15 // Utility functions for various geometrical calculations.
16 //
17 // =============================================================================
18 
19 #ifndef CH_UTILS_GEOMETRY_H
20 #define CH_UTILS_GEOMETRY_H
21 
22 #include <cmath>
23 
24 #include "chrono/core/ChApiCE.h"
25 #include "chrono/core/ChVector.h"
26 #include "chrono/core/ChQuaternion.h"
27 #include "chrono/core/ChMathematics.h"
28 
29 #include "chrono/collision/ChCollisionModel.h"
30 
31 namespace chrono {
32 namespace utils {
33 
34 // -----------------------------------------------------------------------------
35 // These utility functions return the bounding radius of the corresponding
36 // shape. A sphere with this radius and centered at the origin of the frame
37 // defining the shape is a bounding sphere for that shape.
38 // -----------------------------------------------------------------------------
CalcSphereBradius(double radius)39 inline double CalcSphereBradius(double radius) {
40     return radius;
41 }
42 
CalcEllipsoidBradius(const ChVector<> & hdims)43 inline double CalcEllipsoidBradius(const ChVector<>& hdims) {
44     return hdims.LengthInf();
45 }
46 
CalcBoxBradius(const ChVector<> & hdims)47 inline double CalcBoxBradius(const ChVector<>& hdims) {
48     return hdims.Length();
49 }
50 
CalcCapsuleBradius(double radius,double hlen)51 inline double CalcCapsuleBradius(double radius, double hlen) {
52     return hlen + radius;
53 }
54 
CalcCylinderBradius(double radius,double hlen)55 inline double CalcCylinderBradius(double radius, double hlen) {
56     return sqrt(hlen * hlen + radius * radius);
57 }
58 
CalcConeBradius(double radius,double hlen)59 inline double CalcConeBradius(double radius, double hlen) {
60     return sqrt(hlen * hlen + radius * radius);
61 }
62 
CalcRoundedCylinderBradius(double radius,double hlen,double srad)63 inline double CalcRoundedCylinderBradius(double radius, double hlen, double srad) {
64     return sqrt(hlen * hlen + radius * radius) + srad;
65 }
66 
CalcRoundedBoxBradius(const ChVector<> & hdims,double srad)67 inline double CalcRoundedBoxBradius(const ChVector<>& hdims, double srad) {
68     return hdims.Length() + srad;
69 }
70 
CalcTorusBradius(double radius,double thickness)71 inline double CalcTorusBradius(double radius, double thickness) {
72     return radius + thickness;
73 }
74 
75 // -----------------------------------------------------------------------------
76 // These utility functions calculate the volume of the corresponding shape.
77 // -----------------------------------------------------------------------------
CalcSphereVolume(double radius)78 inline double CalcSphereVolume(double radius) {
79     return (4.0 / 3.0) * CH_C_PI * radius * radius * radius;
80 }
81 
CalcEllipsoidVolume(const ChVector<> & hdims)82 inline double CalcEllipsoidVolume(const ChVector<>& hdims) {
83     return (4.0 / 3.0) * CH_C_PI * hdims.x() * hdims.y() * hdims.z();
84 }
85 
CalcBoxVolume(const ChVector<> & hdims)86 inline double CalcBoxVolume(const ChVector<>& hdims) {
87     return 8.0 * hdims.x() * hdims.y() * hdims.z();
88 }
89 
CalcBiSphereVolume(double radius,double cDist)90 inline double CalcBiSphereVolume(double radius, double cDist) {
91     double delta = 2 * radius - cDist;
92     double cos_theta = (radius - 0.5 * delta) / radius;
93     return (4.0 / 3.0) * CH_C_PI * radius * radius * radius * (1 + cos_theta);
94 }
95 
CalcCapsuleVolume(double radius,double hlen)96 inline double CalcCapsuleVolume(double radius, double hlen) {
97     double tmp = radius * radius * hlen + (2.0 / 3.0) * radius * radius * radius;
98     return 2.0 * CH_C_PI * tmp;
99 }
100 
CalcCylinderVolume(double radius,double hlen)101 inline double CalcCylinderVolume(double radius, double hlen) {
102     return 2.0 * CH_C_PI * radius * radius * hlen;
103 }
104 
CalcConeVolume(double radius,double len)105 inline double CalcConeVolume(double radius, double len) {
106     return CH_C_PI * radius * radius * len / 3.0;
107 }
108 
CalcRoundedCylinderVolume(double radius,double hlen,double srad)109 inline double CalcRoundedCylinderVolume(double radius, double hlen, double srad) {
110     double tmp = (radius + srad) * (radius + srad) * hlen + srad * (radius * radius + (2.0 / 3.0) * srad * srad) +
111                  (CH_C_PI / 2.0 - 1.0) * radius * srad * srad;
112     return 2.0 * CH_C_PI * tmp;
113 }
114 
CalcRoundedBoxVolume(const ChVector<> & hdims,double srad)115 inline double CalcRoundedBoxVolume(const ChVector<>& hdims, double srad) {
116     return 8 * hdims.x() * hdims.y() * hdims.z() +
117            2 * srad * (hdims.x() * hdims.y() + hdims.y() * hdims.z() + hdims.z() * hdims.x()) +
118            (4.0 * CH_C_PI / 3.0) * srad * srad * srad;
119 }
120 
CalcTorusVolume(double radius,double thickness)121 inline double CalcTorusVolume(double radius, double thickness) {
122     return 2 * CH_C_PI * CH_C_PI * thickness * thickness * radius;
123 }
124 
125 // -----------------------------------------------------------------------------
126 // This utility function transforms the specified gyration tensor, assumed to
127 // be specified in a centroidal reference frame, to the 'parent' frame defined
128 // by the translation vector 'pos' and orientation 'rot' of the local frame.
129 // -----------------------------------------------------------------------------
TransformGyration(ChMatrix33<> & J,const ChVector<> & pos,const ChQuaternion<> & rot)130 inline void TransformGyration(ChMatrix33<>& J, const ChVector<>& pos, const ChQuaternion<>& rot) {
131     ////
132     ////  TODO
133     ////
134 }
135 
136 // -----------------------------------------------------------------------------
137 // These utility functions calculate the gyration tensor of the corresponding
138 // shape, given its position and orientation.
139 // -----------------------------------------------------------------------------
140 inline ChMatrix33<> CalcSphereGyration(double radius,
141                                        const ChVector<>& pos = ChVector<>(0, 0, 0),
142                                        const ChQuaternion<>& rot = ChQuaternion<>(1, 0, 0, 0)) {
143     double Jxx = (2.0 / 5.0) * radius * radius;
144 
145     ChMatrix33<> J;
146     J.setZero();
147     J(0, 0) = Jxx;
148     J(1, 1) = Jxx;
149     J(2, 2) = Jxx;
150 
151     TransformGyration(J, pos, rot);
152 
153     return J;
154 }
155 
156 inline ChMatrix33<> CalcEllipsoidGyration(const ChVector<>& hdims,
157                                           const ChVector<>& pos = ChVector<>(0, 0, 0),
158                                           const ChQuaternion<>& rot = ChQuaternion<>(1, 0, 0, 0)) {
159     ChMatrix33<> J;
160     J.setZero();
161     J(0, 0) = (1.0 / 5.0) * (hdims.y() * hdims.y() + hdims.z() * hdims.z());
162     J(1, 1) = (1.0 / 5.0) * (hdims.z() * hdims.z() + hdims.x() * hdims.x());
163     J(2, 2) = (1.0 / 5.0) * (hdims.x() * hdims.x() + hdims.y() * hdims.y());
164 
165     TransformGyration(J, pos, rot);
166 
167     return J;
168 }
169 
170 // Calculate the gyration tensor of a box. hdims is a vector of half dimensions of the box.
171 inline ChMatrix33<> CalcBoxGyration(const ChVector<>& hdims,
172                                     const ChVector<>& pos = ChVector<>(0, 0, 0),
173                                     const ChQuaternion<>& rot = ChQuaternion<>(1, 0, 0, 0)) {
174     ChMatrix33<> J;
175     J.setZero();
176     J(0, 0) = (1.0 / 3.0) * (hdims.y() * hdims.y() + hdims.z() * hdims.z());
177     J(1, 1) = (1.0 / 3.0) * (hdims.z() * hdims.z() + hdims.x() * hdims.x());
178     J(2, 2) = (1.0 / 3.0) * (hdims.x() * hdims.x() + hdims.y() * hdims.y());
179 
180     TransformGyration(J, pos, rot);
181 
182     return J;
183 }
184 
185 // Calculate the gyration tensor of a bisphere, which is two identical spheres attached to
186 // each other. delta is the overlap length and radius is the radius of each sphere
187 inline ChMatrix33<> CalcBiSphereGyration(double radius,
188                                          double cDist,
189                                          const ChVector<>& pos = ChVector<>(0, 0, 0),
190                                          const ChQuaternion<>& rot = ChQuaternion<>(1, 0, 0, 0)) {
191     // TODO: simple implementation for now
192 
193     double delta = 2 * radius - cDist;
194     double cos_theta = (radius - 0.5 * delta) / radius;
195     double z_prim = radius - 0.5 * delta;
196 
197     double comp1 = 0.4 * radius * radius * (1 + cos_theta);
198     double comp2 = -0.2 * radius * radius * (1. / 3. * (-cos_theta * cos_theta * cos_theta - 1) + (1 + cos_theta));
199     double comp3 = 2. / 3. * z_prim * z_prim * (1 + cos_theta);
200     double comp4 = 0.5 * radius * z_prim * sqrt(1 - cos_theta * cos_theta);
201     double numerator = 2 * (comp1 + comp2 + comp3 + comp4);
202     double denominator = 4. / 3. * (1 + cos_theta);
203     double Jxx = numerator / denominator;
204     double Jyy = 0.6 * radius * radius * (1. / 3. * (-cos_theta * cos_theta * cos_theta - 1) + (1 + cos_theta)) /
205                  (1 + cos_theta);
206 
207     ChMatrix33<> J;
208     J.setZero();
209     J(0, 0) = Jxx;
210     J(1, 1) = Jyy;
211     J(2, 2) = Jxx;
212 
213     TransformGyration(J, pos, rot);
214 
215     return J;
216 }
217 
218 // Calculate the gyration tensor of a capsule. hlen is the half length of the cylindrical part and radius
219 // is the radius of the spherical part
220 inline ChMatrix33<> CalcCapsuleGyration(double radius,
221                                         double hlen,
222                                         const ChVector<>& pos = ChVector<>(0, 0, 0),
223                                         const ChQuaternion<>& rot = ChQuaternion<>(1, 0, 0, 0)) {
224     double massRatio = 1.5 * hlen / radius;
225     double cmDist = hlen + 3.0 / 8 * radius;
226     double Ixx = massRatio / (1 + massRatio) * (1.0 / 12.0) * (3 * radius * radius + 4 * hlen * hlen) +
227                  1 / (1 + massRatio) * (0.259 * radius * radius + cmDist * cmDist);
228     double Iyy = massRatio / (1 + massRatio) * (1.0 / 2.0) * (radius * radius) +
229                  1 / (1 + massRatio) * (2.0 / 5.0) * (radius * radius);
230 
231     ChMatrix33<> J;
232     J.setZero();
233     J(0, 0) = Ixx;
234     J(1, 1) = Iyy;
235     J(2, 2) = Ixx;
236 
237     TransformGyration(J, pos, rot);
238 
239     return J;
240 }
241 
242 // Calculate the gyration tensor of a cylinder. hlen is the half length of the cylindrical part and radius
243 // is the base radius
244 inline ChMatrix33<> CalcCylinderGyration(double radius,
245                                          double hlen,
246                                          const ChVector<>& pos = ChVector<>(0, 0, 0),
247                                          const ChQuaternion<>& rot = ChQuaternion<>(1, 0, 0, 0)) {
248     ChMatrix33<> J;
249     J.setZero();
250     J(0, 0) = (1.0 / 12.0) * (3 * radius * radius + 4 * hlen * hlen);
251     J(1, 1) = (1.0 / 2.0) * (radius * radius);
252     J(2, 2) = (1.0 / 12.0) * (3 * radius * radius + 4 * hlen * hlen);
253 
254     TransformGyration(J, pos, rot);
255 
256     return J;
257 }
258 
259 // Calculate the gyration tensor of a cone. len is the length of the cone axis and radius
260 // is the base radius
261 inline ChMatrix33<> CalcConeGyration(double radius,
262                                      double len,
263                                      const ChVector<>& pos = ChVector<>(0, 0, 0),
264                                      const ChQuaternion<>& rot = ChQuaternion<>(1, 0, 0, 0)) {
265     double Ixx = (3.0 / 80.0) * (len * len) + (3.0 / 20.0) * (radius * radius);
266 
267     ChMatrix33<> J;
268     J.setZero();
269     J(0, 0) = Ixx;
270     J(1, 1) = (3.0 / 10.0) * (radius * radius);
271     J(2, 2) = Ixx;
272 
273     TransformGyration(J, pos, rot);
274 
275     return J;
276 }
277 
278 inline ChMatrix33<> CalcRoundedCylinderGyration(double radius,
279                                                 double hlen,
280                                                 double srad,
281                                                 const ChVector<>& pos = ChVector<>(0, 0, 0),
282                                                 const ChQuaternion<>& rot = ChQuaternion<>(1, 0, 0, 0)) {
283     double modifiedRadius = radius + srad;
284     double modifiedHlen = hlen + srad;
285 
286     ChMatrix33<> J;
287     J.setZero();
288     //// TODO: for now, use the gyration of the offset cylinder
289     J(0, 0) = (1.0 / 12.0) * (3 * modifiedRadius * modifiedRadius + 4 * modifiedHlen * modifiedHlen);
290     J(1, 1) = (1.0 / 2.0) * (modifiedRadius * modifiedRadius);
291     J(2, 2) = (1.0 / 12.0) * (3 * modifiedRadius * modifiedRadius + 4 * modifiedHlen * modifiedHlen);
292 
293     TransformGyration(J, pos, rot);
294 
295     return J;
296 }
297 
298 inline ChMatrix33<> CalcRoundedBoxGyration(const ChVector<>& hdims,
299                                            double srad,
300                                            const ChVector<>& pos = ChVector<>(0, 0, 0),
301                                            const ChQuaternion<>& rot = ChQuaternion<>(1, 0, 0, 0)) {
302     ChVector<> modifiedHdims = hdims + ChVector<>(srad, srad, srad);
303 
304     ChMatrix33<> J;
305     J.setZero();
306     //// TODO: for now, use the gyration of the offset box
307     J(0, 0) = (1.0 / 3.0) * (modifiedHdims.y() * modifiedHdims.y() + modifiedHdims.z() * modifiedHdims.z());
308     J(1, 1) = (1.0 / 3.0) * (modifiedHdims.z() * modifiedHdims.z() + modifiedHdims.x() * modifiedHdims.x());
309     J(2, 2) = (1.0 / 3.0) * (modifiedHdims.x() * modifiedHdims.x() + modifiedHdims.y() * modifiedHdims.y());
310 
311     TransformGyration(J, pos, rot);
312 
313     return J;
314 }
315 
316 inline ChMatrix33<> CalcTorusGyration(double radius,
317                                       double thickness,
318                                       const ChVector<>& pos = ChVector<>(0, 0, 0),
319                                       const ChQuaternion<>& rot = ChQuaternion<>(1, 0, 0, 0)) {
320     ChMatrix33<> J;
321     J.setZero();
322     J(0, 0) = (5.0 / 8.0) * (thickness * thickness) + (1.0 / 2.0) * (radius * radius);
323     J(1, 1) = (3.0 / 4.0) * (thickness * thickness) + (radius * radius);
324     J(2, 2) = (5.0 / 8.0) * (thickness * thickness) + (1.0 / 2.0) * (radius * radius);
325 
326     TransformGyration(J, pos, rot);
327 
328     return J;
329 }
330 
331 }  // end namespace utils
332 }  // end namespace chrono
333 
334 #endif
335