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 "chrono/geometry/ChLineCam.h"
16 
17 namespace chrono {
18 namespace geometry {
19 
20 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChLineCam)21 CH_FACTORY_REGISTER(ChLineCam)
22 
23 ChLineCam::ChLineCam() {
24     Set_complexity(10);
25     this->closed = true;
26     type = CAM_TYPE_SLIDEFOLLOWER;
27     Rb = 1.0;
28     Rr = 0.0;
29     p = 1.8;
30     d = 2;
31     b0 = CH_C_PI / 6.0;
32     center = VNULL;
33     e = 0;
34     s = Rb;
35     law = chrono_types::make_shared<ChFunction_Const>(0);  // default law = no follower motion
36     negative = false;
37     internal = false;
38 }
39 
ChLineCam(const ChLineCam & source)40 ChLineCam::ChLineCam(const ChLineCam& source) : ChLine(source) {
41     type = source.type;
42     Rb = source.Rb;
43     Rr = source.Rr;
44     p = source.p;
45     d = source.d;
46     b0 = source.b0;
47     center = source.center;
48     e = source.e;
49     s = source.s;
50     law = source.law;
51     negative = source.negative;
52     internal = source.internal;
53 }
54 
Set_Rb(double mrb)55 void ChLineCam::Set_Rb(double mrb) {
56     Rb = mrb;
57     if (e > 0.9 * Rb)
58         e = 0.9 * Rb;
59     if (e < -0.9 * Rb)
60         e = -0.9 * Rb;
61 }
62 
Set_rotating_follower(double mp,double md,double mb0)63 void ChLineCam::Set_rotating_follower(double mp, double md, double mb0) {
64     p = mp;
65     d = md;
66     b0 = mb0;
67     Rb = sqrt(p * p + d * d - 2 * p * d * cos(b0));
68 }
69 
Set_flat_oscillate(double me,double md,double mb0)70 void ChLineCam::Set_flat_oscillate(double me, double md, double mb0) {
71     e = me;
72     d = md;
73     b0 = mb0;
74     Rb = e + d * sin(b0);
75 }
76 
EvaluateCamPoint(double par,ChVector<> & res,double & g,double & q) const77 void ChLineCam::EvaluateCamPoint(double par, ChVector<>& res, double& g, double& q) const {
78     double a = par * 2 * CH_C_PI;  // range : par 0..1 -> angle 0...2PI
79     double r, f, b, B, fshift, y, ydx, ydxdx, sa, fxalpha, u, uh = 0;
80     double sign, signdx, signdxdx;
81 
82     // defaults
83     g = 0;
84     q = 0;
85     res = VNULL;
86 
87     double radius = internal ? -Rr : +Rr;
88     double ecc = negative ? -e : +e;
89 
90     fxalpha = a;
91     sign = signdx = signdxdx = +1;
92 
93     if (negative) {
94         sign = signdx = signdxdx = -1;  // reverse sign
95 
96         if ((type == CAM_TYPE_ROTATEFOLLOWER) || (type == CAM_TYPE_FLATOSCILLATE)) {
97             fxalpha = 2 * CH_C_PI - a;  // also reverse direction
98             signdx = signdxdx = +1;
99         }
100     }
101 
102     y = sign * law->Get_y(fxalpha);
103     ydx = signdx * law->Get_y_dx(fxalpha);
104     ydxdx = signdxdx * law->Get_y_dxdx(fxalpha);
105 
106     switch (this->type) {
107         case CAM_TYPE_SLIDEFOLLOWER:
108             g = atan(ydx / (Rb + y));
109             r = sqrt(radius * radius + pow(Rb + y, 2) - 2 * radius * (Rb + y) * cos(g));
110             fshift = asin(radius * sin(g) / r);
111             if (radius > Rb)
112                 fshift = CH_C_PI - fshift;
113             f = a + fshift;
114             q = pow(ydx * ydx + pow(Rb + y, 2), 1.5) / (pow(Rb + y, 2) - ydxdx * (Rb + y) + 2 * (ydx * ydx)) - radius;
115             break;
116         case CAM_TYPE_ROTATEFOLLOWER:
117             b = b0 + y;
118             u = atan2((p * sin(b) * (1 - ydx)), (d - p * cos(b) * (1 - ydx)));
119             g = CH_C_PI / 2.0 - b - u;
120             r = sqrt(pow(p * sin(b) - radius * sin(u), 2) + pow(d - p * cos(b) - radius * cos(u), 2));
121             fshift = atan2((p * sin(b) - radius * sin(u)), (d - p * cos(b) - radius * cos(u)));
122             f = (a + fshift);
123             uh =
124                 (p * (1 - ydx) * ydx * cos(b + u) - p * ydxdx * sin(b + u)) / (d * cos(u) - p * (1 - ydx) * cos(b + u));
125             q = ((p * cos(b0 + y) * (1 - ydx)) + d) / ((1 + uh) * cos(u)) - radius;
126             break;
127         case CAM_TYPE_ECCENTRICFOLLOWER: {
128             double s_dist = Get_s();
129             sa = s_dist + y;
130             g = atan((ydx - ecc) / (s_dist + y));
131             r = sqrt(pow((sa - radius * cos(g)), 2) + pow((ecc + radius * sin(g)), 2));
132             fshift = atan((ecc + radius * sin(g)) / (sa - radius * cos(g)));
133             if (radius > Rb)
134                 fshift = CH_C_PI + fshift;
135             f = a + fshift;
136             q = pow((pow(s_dist + y, 2) + pow(ecc - ydx, 2)), 1.5) /
137                     (pow(s_dist + y, 2) + (ecc - ydx) * (ecc - 2 * ydx) - (s_dist + y) * ydxdx) -
138                 radius;
139             break;
140         }
141         case CAM_TYPE_FLAT:
142             g = 0;
143             B = ydx;
144             r = sqrt(pow(Rb + y, 2) + ydx * ydx);
145             f = a + atan2(ydx, (Rb + y));
146             q = Rb + y + ydxdx;
147             break;
148         case CAM_TYPE_FLATOSCILLATE:
149             b = b0 + y;
150             B = (d * cos(b)) / (1 - ydx);
151             g = atan2(ecc, B);
152             r = sqrt(pow(d - ecc * sin(b) - B * cos(b), 2) + pow(B * sin(b) - ecc * cos(b), 2));
153             f = a + atan2((B * sin(b) - ecc * cos(b)), (d - ecc * sin(b) - B * cos(b)));
154             q = (d * sin(b) * (1 - 2 * ydx) + B * ydxdx) / (pow(1 - ydx, 2)) - ecc;
155             break;
156         default:
157             g = r = f = 0;
158             break;
159     }
160 
161     if (negative) {
162         if ((type == CAM_TYPE_FLAT) || (type == CAM_TYPE_SLIDEFOLLOWER) || (type == CAM_TYPE_ECCENTRICFOLLOWER))
163             f += CH_C_PI;  // polar 180
164 
165         if ((type == CAM_TYPE_ROTATEFOLLOWER) || (type == CAM_TYPE_FLATOSCILLATE)) {
166             f = -f;  // y mirror
167         }
168     }
169 
170     res.z() = 0;
171     res.x() = this->center.x() + r * cos(f + phase);
172     res.y() = this->center.y() + r * sin(f + phase);
173 }
174 
Evaluate(ChVector<> & pos,const double parU) const175 void ChLineCam::Evaluate(ChVector<>& pos, const double parU) const {
176     double qtmp, gtmp;
177     EvaluateCamPoint(parU, pos, gtmp, qtmp);
178 }
179 
ArchiveOUT(ChArchiveOut & marchive)180 void ChLineCam::ArchiveOUT(ChArchiveOut& marchive) {
181     // version number
182     marchive.VersionWrite<ChLineCam>();
183     // serialize parent class
184     ChLine::ArchiveOUT(marchive);
185     // serialize all member data:
186 
187     eChCamType_mapper mmapper;
188     marchive << CHNVP(mmapper(type), "type");
189     marchive << CHNVP(law);
190     marchive << CHNVP(phase);
191     marchive << CHNVP(Rb);
192     marchive << CHNVP(Rr);
193     marchive << CHNVP(p);
194     marchive << CHNVP(d);
195     marchive << CHNVP(b0);
196     marchive << CHNVP(e);
197     marchive << CHNVP(s);
198     marchive << CHNVP(negative);
199     marchive << CHNVP(internal);
200     marchive << CHNVP(center);
201 }
202 
ArchiveIN(ChArchiveIn & marchive)203 void ChLineCam::ArchiveIN(ChArchiveIn& marchive) {
204     // version number
205     /*int version =*/ marchive.VersionRead<ChLineCam>();
206     // deserialize parent class
207     ChLine::ArchiveIN(marchive);
208     // stream in all member data:
209     eChCamType_mapper mmapper;
210     marchive >> CHNVP(mmapper(type), "type");
211     marchive >> CHNVP(law);
212     marchive >> CHNVP(phase);
213     marchive >> CHNVP(Rb);
214     marchive >> CHNVP(Rr);
215     marchive >> CHNVP(p);
216     marchive >> CHNVP(d);
217     marchive >> CHNVP(b0);
218     marchive >> CHNVP(e);
219     marchive >> CHNVP(s);
220     marchive >> CHNVP(negative);
221     marchive >> CHNVP(internal);
222     marchive >> CHNVP(center);
223 }
224 
225 }  // end namespace geometry
226 }  // end namespace chrono
227