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