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 <memory.h>
16 #include <cfloat>
17 #include <cmath>
18 #include <cstdio>
19 #include <cstdlib>
20 #include <cstring>
21
22 #include "chrono/geometry/ChLine.h"
23
24 namespace chrono {
25 namespace geometry {
26
27 // Register into the object factory, to enable run-time dynamic creation and persistence
28 // CH_FACTORY_REGISTER(ChLine) // NO! Abstract class!
29
ChLine(const ChLine & source)30 ChLine::ChLine(const ChLine& source) {
31 closed = source.closed;
32 complexityU = source.complexityU;
33 }
34
Derive(ChVector<> & dir,const double parU) const35 void ChLine::Derive(ChVector<>& dir, const double parU) const {
36 double bdf = 10e-9;
37 double uA = 0, uB = 0;
38
39 if (parU > 0.5) {
40 uB = parU;
41 uA = parU - bdf;
42 } else {
43 uB = parU + bdf;
44 uA = parU;
45 }
46 ChVector<> vA, vB;
47 Evaluate(vA, uA);
48 Evaluate(vB, uB);
49 dir = (vB - vA) * (1 / bdf);
50 }
51
FindNearestLinePoint(ChVector<> & point,double & resU,double approxU,double tol) const52 bool ChLine::FindNearestLinePoint(ChVector<>& point, double& resU, double approxU, double tol) const {
53 double mu;
54 int points = 20;
55 bool closed = false;
56 double bestU = 0;
57 double bestdist = 9999999;
58 double dist, d1, d2;
59 int iters = 0;
60 int maxiters = 11;
61
62 ChVector<> vres, vp1, vp2;
63
64 points = this->Get_complexity();
65 closed = this->Get_closed();
66
67 points = points * 4; // double sampling along line.
68
69 // first approximation
70 for (int i = 0; i <= points; i++) {
71 mu = (double)i / (double)points;
72 Evaluate(vres, mu);
73 dist = Vlength(Vsub(vres, point));
74 if (dist < bestdist) {
75 bestdist = dist;
76 bestU = mu;
77 }
78 }
79 // refine position with pseudo-NR
80 double step = 1 / (double)points;
81 double nrU = bestU;
82 double u1, u2;
83
84 u1 = nrU - step;
85 if (u1 < 0) {
86 if (!closed)
87 u1 = 0;
88 else
89 u1 = u1 + 1;
90 }
91 this->Evaluate(vres, u1);
92 d1 = Vlength(Vsub(vres, point));
93 vp1 = vres;
94
95 u2 = nrU + step;
96 if (u2 > 1) {
97 if (!closed)
98 u2 = 1;
99 else
100 u2 = u2 - 1;
101 }
102 this->Evaluate(vres, u2);
103 d2 = Vlength(Vsub(vres, point));
104 vp2 = vres;
105
106 while (true) {
107 iters++;
108
109 if (nrU < 0) {
110 if (!closed)
111 nrU = 0;
112 else
113 nrU = nrU + 1;
114 }
115 if (nrU > 1) {
116 if (!closed)
117 nrU = 1;
118 else
119 nrU = nrU - 1;
120 }
121 this->Evaluate(vres, nrU);
122 dist = Vlength(Vsub(vres, point));
123
124 bestU = nrU;
125
126 if (d1 < d2) {
127 u2 = nrU;
128 d2 = dist;
129 vp2 = vres; // move point 2 to nrU
130 step = step / 2;
131 nrU = nrU - step; // move nrU in middle
132 if (d1 < dist)
133 bestU = u1;
134 } else {
135 u1 = nrU;
136 d1 = dist;
137 vp1 = vres; // move point 1 to nrU
138 step = step / 2;
139 nrU = nrU + step; // move nrU in middle
140 if (d2 < dist)
141 bestU = u2;
142 }
143
144 if ((Vlength(Vsub(vp1, vp2)) <= tol) || (dist <= tol)) {
145 resU = bestU;
146 return true;
147 }
148 if (iters > maxiters) {
149 resU = bestU;
150 return false;
151 }
152 }
153
154 resU = bestU;
155 return true;
156 }
157
CurveCurveDist(ChLine * compline,int samples) const158 double ChLine::CurveCurveDist(ChLine* compline, int samples) const {
159 double mres = 0;
160 double par;
161 // distances this<->compare_line
162 for (par = 0; par < 1; par = par + 1 / ((double)samples)) {
163 double mpos;
164 ChVector<> ptB;
165 compline->Evaluate(ptB, par);
166 this->FindNearestLinePoint(ptB, mpos, 0, 0.00002);
167 ChVector<> ptA;
168 this->Evaluate(ptA, mpos);
169 mres += Vlength(Vsub(ptA, ptB));
170 }
171 // ..and viceversa
172 for (par = 0; par < 1; par = par + 1 / ((double)samples)) {
173 double mpos;
174 ChVector<> ptB;
175 this->Evaluate(ptB, par);
176 compline->FindNearestLinePoint(ptB, mpos, 0, 0.00002);
177 ChVector<> ptA;
178 compline->Evaluate(ptA, mpos);
179 mres += Vlength(Vsub(ptA, ptB));
180 }
181
182 return (mres / (samples * 2));
183 }
184
CurveCurveDistMax(ChLine * compline,int samples) const185 double ChLine::CurveCurveDistMax(ChLine* compline, int samples) const {
186 double mres = 0;
187 double par;
188 double mdis;
189 // distances this<->compare_line
190 for (par = 0; par < 1; par = par + 1 / ((double)samples)) {
191 double mpos;
192 ChVector<> ptB;
193 compline->Evaluate(ptB, par);
194 this->FindNearestLinePoint(ptB, mpos, 0, 0.00002);
195 ChVector<> ptA;
196 this->Evaluate(ptA, mpos);
197 mdis = Vlength(Vsub(ptA, ptB));
198 if (mres < mdis)
199 mres = mdis;
200 }
201 // ..and viceversa
202 for (par = 0; par < 1; par = par + 1 / ((double)samples)) {
203 double mpos;
204 ChVector<> ptB;
205 this->Evaluate(ptB, par);
206 compline->FindNearestLinePoint(ptB, mpos, 0, 0.00002);
207 ChVector<> ptA;
208 compline->Evaluate(ptA, mpos);
209 mdis = Vlength(Vsub(ptA, ptB));
210 if (mres < mdis)
211 mres = mdis;
212 }
213
214 return mres;
215 }
216
CurveSegmentDist(ChLine * complinesegm,int samples) const217 double ChLine::CurveSegmentDist(ChLine* complinesegm, int samples) const {
218 double mres = 0;
219 double par;
220 // distances this<->compare_line
221 for (par = 0; par < 1; par = par + 1 / ((double)samples)) {
222 double mpos;
223 ChVector<> ptB;
224 complinesegm->Evaluate(ptB, par);
225 this->FindNearestLinePoint(ptB, mpos, 0, 0.00002);
226 ChVector<> ptA;
227 this->Evaluate(ptA, mpos);
228 mres += Vlength(Vsub(ptA, ptB));
229 }
230 return (mres / samples);
231 }
232
CurveSegmentDistMax(ChLine * complinesegm,int samples) const233 double ChLine::CurveSegmentDistMax(ChLine* complinesegm, int samples) const {
234 double mres = 0;
235 double par;
236 double mdis;
237 // distances this<->compare_line
238 for (par = 0; par < 1; par = par + 1 / ((double)samples)) {
239 double mpos;
240 ChVector<> ptB;
241 complinesegm->Evaluate(ptB, par);
242 this->FindNearestLinePoint(ptB, mpos, 0, 0.00002);
243 ChVector<> ptA;
244 this->Evaluate(ptA, mpos);
245 mdis = Vlength(Vsub(ptA, ptB));
246 if (mres < mdis)
247 mres = mdis;
248 }
249 return (mres);
250 }
251
Length(int sampling) const252 double ChLine::Length(int sampling) const {
253 double mres = 0;
254 double par, step;
255
256 if (!closed)
257 step = 1 / ((double)(Get_complexity() - 1));
258 else
259 step = 1 / ((double)(Get_complexity()));
260
261 if (sampling > 1)
262 step = step / (double)sampling;
263
264 ChVector<> pA;
265 this->Evaluate(pA, 0.);
266 ChVector<> pB;
267 for (par = 0; par <= 1.000000001; par = par + step) {
268 this->Evaluate(pB, par);
269 mres += Vlength(Vsub(pA, pB));
270 pA = pB;
271 }
272 return mres;
273 }
274
275 // Draw into the current graph viewport of a ChFile_ps file
276
DrawPostscript(ChFile_ps * mfle,int markpoints,int bezier_interpolate)277 bool ChLine::DrawPostscript(ChFile_ps* mfle, int markpoints, int bezier_interpolate) {
278 ChVector2<> mp1;
279 ChVector<> mv1;
280
281 mfle->GrSave();
282 mfle->ClipRectangle(mfle->Get_G_p(), mfle->Get_Gs_p(), ChFile_ps::Space::PAGE);
283 // start a line, move cursor to beginning
284 mfle->StartLine();
285 this->Evaluate(mv1, 0.0);
286 mp1.x() = mv1.x();
287 mp1.y() = mv1.y();
288 mp1 = mfle->To_page_from_graph(mp1);
289 mfle->MoveTo(mp1);
290 double maxpoints = this->Get_complexity() * 10;
291 // add points into line
292 for (int i = 1; i <= maxpoints; i++) {
293 this->Evaluate(mv1, ((double)i) / ((double)maxpoints));
294 mp1.x() = mv1.x();
295 mp1.y() = mv1.y();
296 mp1 = mfle->To_page_from_graph(mp1);
297 mfle->AddLinePoint(mp1);
298 }
299 if (this->Get_closed())
300 mfle->CloseLine(); // if periodic curve, close it
301
302 mfle->PaintStroke(); // draw it!
303 mfle->GrRestore(); // restore old modes, with old clipping
304
305 return true;
306 }
307
ArchiveOUT(ChArchiveOut & marchive)308 void ChLine::ArchiveOUT(ChArchiveOut& marchive) {
309 // version number
310 marchive.VersionWrite<ChLine>();
311 // serialize parent class
312 ChGeometry::ArchiveOUT(marchive);
313 // serialize all member data:
314 marchive << CHNVP(closed);
315 marchive << CHNVP(complexityU);
316 }
317
ArchiveIN(ChArchiveIn & marchive)318 void ChLine::ArchiveIN(ChArchiveIn& marchive) {
319 // version number
320 /*int version =*/ marchive.VersionRead<ChLine>();
321 // deserialize parent class
322 ChGeometry::ArchiveIN(marchive);
323 // stream in all member data:
324 marchive >> CHNVP(closed);
325 marchive >> CHNVP(complexityU);
326 }
327
328 } // end namespace geometry
329 } // end namespace chrono
330