path-vector.cc 7.8 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/core/path-vector.hh>

21
22
23
#include <boost/serialization/vector.hpp>
#include <boost/serialization/weak_ptr.hpp>

Joseph Mirabel's avatar
Joseph Mirabel committed
24
#include <hpp/util/indent.hh>
25
#include <hpp/util/serialization.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
26

Joseph Mirabel's avatar
Joseph Mirabel committed
27
28
#include <stdexcept>

29
30
31
32
33
34
namespace hpp {
  namespace core {

    std::size_t PathVector::rankAtParam (const value_type& param,
					 value_type& localParam) const
    {
35
      assert(!timeParameterization());
36
37
      if (paths_.empty())
        throw std::runtime_error ("PathVector is empty.");
38
39
40
41
      std::size_t res = 0;
      localParam = param;
      bool finished = false;

42
      while (res + 1 < paths_.size () && !finished) {
43
44
45
46
47
48
49
50
51
52
53
54
55
	if (localParam > paths_ [res]->length ()) {
	  localParam -= paths_ [res]->length ();
	  res ++;
	}
	else {
	  finished = true;
	}
      }
      if (localParam > paths_ [res]->length ()) {
	if (res != paths_.size () -1) {
	  throw std::runtime_error ("localparam out of range.");
	}
	localParam = paths_ [res]->timeRange ().second;
56
57
      } else {
	localParam += paths_ [res]->timeRange ().first;
58
      }
59
60
61
62
      assert (localParam >= paths_ [res]->timeRange ().first -
	      std::numeric_limits <float>::epsilon ());
      assert (localParam <= paths_ [res]->timeRange ().second +
	      std::numeric_limits <float>::epsilon ());
63
64
65
66
67
      return res;
    }

    void PathVector::appendPath (const PathPtr_t& path)
    {
68
      assert(!timeParameterization());
69
      paths_.push_back (path);
70
71
72
      interval_t tr = timeRange();
      tr.second += path->length ();
      timeRange (tr);
73
74
    }

75
76
    PathPtr_t PathVector::pathAtRank (std::size_t rank) const
    {
77
      PathPtr_t copy;
78
      if (constraints ()) {
79
	if (paths_ [rank]->constraints ()) {
80
81
82
83
84
85
	  throw std::runtime_error
	    ("Attempt to extract a path from a path vector where both "
	     "are subject to constraints. This is not supported.");
	} else {
	  ConstraintPtr_t constraintCopy (constraints ()->copy ());
	  HPP_STATIC_CAST_REF_CHECK (ConstraintSet, *constraintCopy);
86
87
	  copy = paths_ [rank]->copy (HPP_STATIC_PTR_CAST (ConstraintSet,
							   constraintCopy));
88
	}
89
90
      } else {
	copy = paths_ [rank]->copy ();
91
92
93
94
      }
      return copy;
    }

95
96
97
98
99
100
    void PathVector::concatenate (const PathVector& path)
    {
      for (std::size_t i=0; i<path.numberPaths (); ++i) {
	appendPath (path.pathAtRank (i)->copy ());
      }
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
101

102
103
104
105
106
107
108
    void PathVector::concatenate (const PathVectorPtr_t& path)
    {
      for (std::size_t i=0; i<path->numberPaths (); ++i) {
	appendPath (path->pathAtRank (i)->copy ());
      }
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
109
110
    void PathVector::flatten (PathVectorPtr_t p) const
    {
111
      assert(!timeParameterization());
Joseph Mirabel's avatar
Joseph Mirabel committed
112
113
114
115
116
117
118
119
      for (std::size_t i = 0; i < numberPaths (); ++i) {
        PathPtr_t path = pathAtRank (i);
        PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST(PathVector, path);
        if (pv) pv->flatten (p);
        else    p->appendPath (path);
      }
    }

120
    bool PathVector::impl_compute (ConfigurationOut_t result,
121
				   value_type param) const
122
    {
123
      assert(!timeParameterization());
124
125
126
      // Find direct path in vector corresponding to parameter.
      size_t rank;
      value_type localParam;
127
      rank = rankAtParam (param, localParam);
128
129

      PathPtr_t subpath = paths_ [rank];
130
      return (*subpath) (result, localParam);
131
132
    }

133
    void PathVector::impl_derivative (vectorOut_t result, const value_type& param,
134
135
				      size_type order) const
    {
136
      assert(!timeParameterization());
137
138
139
      // Find direct path in vector corresponding to parameter.
      size_t rank;
      value_type localParam;
140
      rank = rankAtParam (param, localParam);
141
142

      PathPtr_t subpath = paths_ [rank];
143
      subpath->derivative (result, localParam, order);
144
145
    }

146
    PathPtr_t PathVector::impl_extract (const interval_t& subInterval) const
147
    {
148
      assert(!timeParameterization());
149
      using std::make_pair;
150
      PathVectorPtr_t path = create (outputSize (), outputDerivativeSize ());
151
152
153
154
155
      bool reversed = subInterval.first > subInterval.second ? true : false;
      if (reversed) {
	value_type tmin = subInterval.second;
	value_type tmax = subInterval.first;
	value_type localtmin, localtmax;
156
157
	std::size_t imin = rankAtParam (tmin, localtmin);
	std::size_t imax = rankAtParam (tmax, localtmax);
158
	value_type t1min, t1max;
159
160
	long int i = imax; // i should be a signed int otherwise the loop below
	                   // fails.
161
	do {
162
	  // t1max >= t1min
163
164
	  t1min = paths_ [i]->timeRange ().second;
	  t1max = paths_ [i]->timeRange ().first;
165
	  if (i == (long int) imax) {
166
167
	    t1min = localtmax;
	  }
168
	  if (i == (long int) imin) {
169
170
	    t1max = localtmin;
	  }
171
172
173
174
175
176
	  assert (t1max - paths_ [i]->timeRange ().first >
		  -std::numeric_limits <float>::epsilon ());
	  assert (t1min - paths_ [i]->timeRange ().second <
		  std::numeric_limits <float>::epsilon ());
	  t1min = std::max (t1min, paths_ [i]->timeRange ().first);
	  t1max = std::min (t1max, paths_ [i]->timeRange ().second);
177
          if (t1min != t1max || path->numberPaths() == 0) {
178
179
            path->appendPath (paths_ [i]->extract (make_pair (t1min, t1max)));
          }
180
	  --i;
181
	} while (i >= (long int) imin);
182
183
184
185
186
187
188
189
190
      } else {
	value_type tmin = subInterval.first;
	value_type tmax = subInterval.second;
	value_type localtmin, localtmax;
	std::size_t imin = rankAtParam (tmin, localtmin);
	std::size_t imax = rankAtParam (tmax, localtmax);
	value_type t1min, t1max;
	std::size_t i = imin;
	do {
191
	  // t1max >= t1min
192
193
194
195
196
197
198
199
	  t1min = paths_ [i]->timeRange ().first;
	  t1max = paths_ [i]->timeRange ().second;
	  if (i == imin) {
	    t1min = localtmin;
	  }
	  if (i == imax) {
	    t1max = localtmax;
	  }
200
201
202
203
204
205
206
	  // Check numerical precision
	  assert (t1min - paths_ [i]->timeRange ().first >
		  -std::numeric_limits <float>::epsilon ());
	  assert (t1max - paths_ [i]->timeRange ().second <
		  std::numeric_limits <float>::epsilon ());
	  t1min = std::max (t1min, paths_ [i]->timeRange ().first);
	  t1max = std::min (t1max, paths_ [i]->timeRange ().second);
207
          if (t1min != t1max || path->numberPaths() == 0) {
208
209
            path->appendPath (paths_ [i]->extract (make_pair (t1min, t1max)));
          }
210
211
212
213
214
	  ++i;
	} while (i <= imax);
      }
      return path;
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
215

216
217
218
219
220
221
222
223
224
225
    PathPtr_t PathVector::reverse () const
    {
      assert(!timeParameterization());
      PathVectorPtr_t ret = create (outputSize (), outputDerivativeSize ());
      std::for_each (paths_.rbegin(), paths_.rend(), [&ret](const PathPtr_t& path) {
        ret->appendPath(path->reverse());
      });
      return ret;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
226
227
228
229
230
231
232
233
234
    std::ostream& PathVector::print (std::ostream &os) const
    {
      Path::print (os << "PathVector:") << incendl;
      for (Paths_t::const_iterator itPath = paths_.begin ();
          itPath != paths_.end (); ++itPath) {
        os << (**itPath) << iendl;
      }
      return os << decindent;
    }
235
236
237
238
239
240
241
242
243
244
245
246

    template<class Archive>
    void PathVector::serialize(Archive & ar, const unsigned int version)
    {
      using namespace boost::serialization;
      (void) version;
      ar & make_nvp("base", base_object<Path>(*this));
      ar & BOOST_SERIALIZATION_NVP(paths_);
      ar & BOOST_SERIALIZATION_NVP(weak_);
    }

    HPP_SERIALIZATION_IMPLEMENT(PathVector);
247
248
  } //   namespace core
} // namespace hpp
249

250
BOOST_CLASS_EXPORT_IMPLEMENT(hpp::core::PathVector)