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/ChLinePath.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(ChLinePath)21 CH_FACTORY_REGISTER(ChLinePath)
22
23 ChLinePath::ChLinePath(const ChLinePath& source) : ChLine(source) {
24 lines = source.lines;
25 end_times = source.end_times;
26 durations = source.durations;
27 }
28
Length(int sampling) const29 double ChLinePath::Length(int sampling) const {
30 double tot = 0;
31 for (int i = 0; i < lines.size(); ++i) {
32 tot += lines[i]->Length(sampling);
33 }
34 return tot;
35 }
36
Evaluate(ChVector<> & pos,const double parU) const37 void ChLinePath::Evaluate(ChVector<>& pos, const double parU) const {
38 if (lines.size() == 0)
39 return;
40
41 double u = parU;
42
43 // wrap u if it is a closed loop.
44 if (this->closed)
45 u = fmod(parU, this->GetPathDuration());
46
47 double uA = 0;
48 // Search sub line covering the parU
49 // (brute force search.. assuming a limited number of added lines, it is ok anyway.)
50 int i;
51 for (i = 0; i < lines.size(); ++i) {
52 if (u <= end_times[i])
53 break;
54 }
55 if (i == lines.size()) // beyond end
56 i -= 1;
57 if (i > 0)
58 uA = end_times[i - 1];
59
60 double local_U = (u - uA) / durations[i];
61 lines[i]->Evaluate(pos, local_U);
62 }
63
SetSubLineDurationN(size_t n,double mduration)64 void ChLinePath::SetSubLineDurationN(size_t n, double mduration) {
65 durations[n] = mduration;
66
67 double last_t = 0;
68 if (n > 0)
69 last_t = end_times[n - 1];
70 for (size_t i = n; i < end_times.size(); ++i) {
71 last_t += durations[n];
72 end_times[n] = last_t;
73 }
74 }
75
AddSubLine(std::shared_ptr<ChLine> mline,double duration)76 void ChLinePath::AddSubLine(std::shared_ptr<ChLine> mline, double duration) {
77 lines.push_back(mline);
78 durations.push_back(0);
79 end_times.push_back(0);
80 SetSubLineDurationN(lines.size() - 1, duration);
81 }
82
AddSubLine(ChLine & mline,double duration)83 void ChLinePath::AddSubLine(ChLine& mline, double duration) {
84 std::shared_ptr<ChLine> pline((ChLine*)mline.Clone());
85 AddSubLine(pline, duration);
86 }
87
InsertSubLine(size_t n,std::shared_ptr<ChLine> mline,double duration)88 void ChLinePath::InsertSubLine(size_t n, std::shared_ptr<ChLine> mline, double duration) {
89 lines.insert(lines.begin() + n, mline);
90 durations.push_back(0);
91 end_times.push_back(0);
92 // force recompute following end times:
93 SetSubLineDurationN(n, duration);
94 }
95
InsertSubLine(size_t n,ChLine & mline,double duration)96 void ChLinePath::InsertSubLine(size_t n, ChLine& mline, double duration) {
97 std::shared_ptr<ChLine> pline((ChLine*)mline.Clone());
98 InsertSubLine(n, pline, duration);
99 }
100
EraseSubLine(size_t n)101 void ChLinePath::EraseSubLine(size_t n) {
102 lines.erase(lines.begin() + n);
103 durations.erase(durations.begin() + n);
104 end_times.pop_back();
105 // force recompute all end times:
106 if (lines.size())
107 SetSubLineDurationN(0, durations[0]);
108 }
109
GetPathDuration() const110 double ChLinePath::GetPathDuration() const {
111 if (end_times.size())
112 return end_times.back();
113 return 0;
114 }
115
SetPathDuration(double mUduration)116 void ChLinePath::SetPathDuration(double mUduration) {
117 double factor = mUduration / GetPathDuration();
118 double last_t = 0;
119 for (size_t i = 0; i < end_times.size(); ++i) {
120 durations[i] *= factor;
121 last_t += durations[i];
122 end_times[i] = last_t;
123 }
124 }
125
GetContinuityMaxError() const126 double ChLinePath::GetContinuityMaxError() const {
127 double maxerr = 0;
128 for (size_t i = 1; i < lines.size(); ++i) {
129 std::shared_ptr<ChLine> prec_line = lines[i - 1];
130 std::shared_ptr<ChLine> next_line = lines[i];
131 double gap = (prec_line->GetEndB() - next_line->GetEndA()).Length();
132 if (gap > maxerr)
133 maxerr = gap;
134 }
135 if (this->closed) {
136 double gap = (lines.back()->GetEndA() - lines.front()->GetEndB()).Length();
137 if (gap > maxerr)
138 maxerr = gap;
139 }
140 return maxerr;
141 }
142
ArchiveOUT(ChArchiveOut & marchive)143 void ChLinePath::ArchiveOUT(ChArchiveOut& marchive) {
144 // version number
145 marchive.VersionWrite<ChLinePath>();
146 // serialize parent class
147 ChLine::ArchiveOUT(marchive);
148 // serialize all member data:
149 marchive << CHNVP(lines);
150 marchive << CHNVP(end_times);
151 marchive << CHNVP(durations);
152 }
153
ArchiveIN(ChArchiveIn & marchive)154 void ChLinePath::ArchiveIN(ChArchiveIn& marchive) {
155 // version number
156 /*int version =*/ marchive.VersionRead<ChLinePath>();
157 // deserialize parent class
158 ChLine::ArchiveIN(marchive);
159 // stream in all member data:
160 marchive >> CHNVP(lines);
161 marchive >> CHNVP(end_times);
162 marchive >> CHNVP(durations);
163 }
164
165 } // end namespace geometry
166 } // end namespace chrono
167