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