1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
|
/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
// Note: Originally cribbed from Ogre3d. Modified to implement Cardinal
// spline and catmull-rom spline
#ifndef IGNITION_MATH_SPLINE_HH_
#define IGNITION_MATH_SPLINE_HH_
#include <ignition/math/Helpers.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/config.hh>
namespace ignition
{
namespace math
{
inline namespace IGNITION_MATH_VERSION_NAMESPACE
{
// Forward declare private classes
class ControlPoint;
class SplinePrivate;
/// \class Spline Spline.hh ignition/math/Spline.hh
/// \brief Splines
class IGNITION_MATH_VISIBLE Spline
{
/// \brief constructor
public: Spline();
/// \brief destructor
public: ~Spline();
/// \brief Sets the tension parameter.
/// \remarks A value of 0 results in a Catmull-Rom
/// spline.
/// \param[in] _t Tension value between 0.0 and 1.0
public: void Tension(double _t);
/// \brief Gets the tension value.
/// \return the value of the tension, which is between 0.0 and 1.0.
public: double Tension() const;
/// \brief Gets spline arc length.
/// \return arc length or INF on error.
public: double ArcLength() const;
/// \brief Gets spline arc length up to
/// a given parameter value \p _t.
/// \param[in] _t parameter value (range 0 to 1).
/// \return arc length up to \p _t or INF on error.
public: double ArcLength(const double _t) const;
/// \brief Gets a spline segment arc length.
/// \param[in] _index of the spline segment.
/// \param[in] _t parameter value (range 0 to 1).
/// \return arc length of a given segment up to
/// \p _t or INF on error.
public: double ArcLength(const unsigned int _index,
const double _t) const;
/// \brief Adds a single control point to the
/// end of the spline.
/// \param[in] _p control point value to add.
public: void AddPoint(const Vector3d &_p);
/// \brief Adds a single control point to the end
/// of the spline with fixed tangent.
/// \param[in] _p control point value to add.
/// \param[in] _t tangent at \p _p.
public: void AddPoint(const Vector3d &_p, const Vector3d &_t);
/// \brief Adds a single control point to the end
/// of the spline.
/// \param[in] _cp control point to add.
/// \param[in] _fixed whether this control point
/// should not be subject to tangent recomputation.
private: void AddPoint(const ControlPoint &_cp, const bool _fixed);
/// \brief Gets the value for one of the control points
/// of the spline.
/// \param[in] _index the control point index.
/// \return the control point value, or [INF, INF, INF]
/// on error. Use Vector3d::IsFinite() to check for an error.
public: Vector3d Point(const unsigned int _index) const;
/// \brief Gets the tangent value for one of the control points
/// of the spline.
/// \param[in] _index the control point index.
/// \return the control point tangent, or [INF, INF, INF]
/// on error. Use Vector3d::IsFinite() to check for an error.
public: Vector3d Tangent(const unsigned int _index) const;
/// \brief Gets the mth derivative for one of the control points
/// of the spline.
/// \param[in] _index the control point index.
/// \param[in] _mth derivative order.
/// \return the control point mth derivative, or [INF, INF, INF]
/// on error. Use Vector3d::IsFinite() to check for an error.
public: Vector3d MthDerivative(const unsigned int _index,
const unsigned int _mth) const;
/// \brief Gets the number of control points in the spline.
/// \return the count
public: size_t PointCount() const;
/// \brief Clears all the points in the spline.
public: void Clear();
/// \brief Updates a single control point value in the spline,
/// keeping its tangent.
/// \param[in] _index the control point index.
/// \param[in] _p the new control point value.
/// \return True on success.
public: bool UpdatePoint(const unsigned int _index,
const Vector3d &_p);
/// \brief Updates a single control point in the spline, along
/// with its tangent.
/// \param[in] _index the control point index.
/// \param[in] _p the new control point value.
/// \param[in] _t the new control point tangent.
/// \return True on success.
public: bool UpdatePoint(const unsigned int _index,
const Vector3d &_p,
const Vector3d &_t);
/// \brief Updates a single control point in the spline.
/// \param[in] _index the control point index
/// \param[in] _cp the new control point
/// \param[in] _fixed whether the new control point should not be
/// subject to tangent recomputation
/// \return True on success.
private: bool UpdatePoint(const unsigned int _index,
const ControlPoint &_cp,
const bool _fixed);
/// \brief Interpolates a point on the spline
/// at parameter value \p _t.
/// \remarks Parameter value is normalized over the
/// whole spline arc length. Arc length is assumed
/// to be linear with the parameter.
/// \param[in] _t parameter value (range 0 to 1).
/// \return the interpolated point, or
/// [INF, INF, INF] on error. Use
/// Vector3d::IsFinite() to check for an error.
public: Vector3d Interpolate(const double _t) const;
/// \brief Interpolates a point on a segment of the spline
/// at parameter value \p _t.
/// \remarks Parameter value is normalized over the
/// segment arc length. Arc length is assumed
/// to be linear with the parameter.
/// \param[in] _fromIndex The point index to treat as t = 0.
/// fromIndex + 1 is deemed to be t = 1.
/// \param[in] _t parameter value (range 0 to 1).
/// \return the interpolated point, or [INF, INF, INF] on
/// error. Use Vector3d::IsFinite() to check for an error.
public: Vector3d Interpolate(const unsigned int _fromIndex,
const double _t) const;
/// \brief Interpolates a tangent on the spline at
/// parameter value \p _t.
/// \remarks Parameter value is normalized over the
/// whole spline arc length. Arc length is assumed
/// to be linear with the parameter.
/// \param[in] _t parameter value (range 0 to 1).
/// \return the interpolated point, or [INF, INF, INF]
/// on error. Use Vector3d::IsFinte() to check for an error.
public: Vector3d InterpolateTangent(const double _t) const;
/// \brief Interpolates the tangent on a segment of the spline
/// at parameter value \p _t.
/// \remarks Parameter value is normalized over the
/// segment arc length. Arc length is assumed
/// to be linear with the parameter.
/// \param[in] _fromIndex the point index to treat as t = 0.
/// fromIndex + 1 is deemed to be t = 1.
/// \param[in] _t parameter value (range 0 to 1).
/// \return the interpolated point, or [INF, INF, INF] on
/// error. Use Vector3d::IsFinte() to check for an error.
public: Vector3d InterpolateTangent(const unsigned int _fromIndex,
const double _t) const;
/// \brief Interpolates the mth derivative of the spline at
/// parameter value \p _t.
/// \param[in] _mth order of curve derivative to interpolate.
/// \param[in] _t parameter value (range 0 to 1).
/// \return the interpolated mth derivative, or [INF, INF, INF]
/// on error. Use Vector3d::IsFinite() to check for an error.
public: Vector3d InterpolateMthDerivative(const unsigned int _mth,
const double _1) const;
/// \brief Interpolates the mth derivative of a segment of the spline
/// at parameter value \p _t.
/// \remarks Parameter value is normalized over the segment
/// arc length. Arc length is assumed to be linear with the parameter.
/// \param[in] _fromIndex point index to treat as t = 0, fromIndex + 1
/// is deemed to be t = 1.
/// \param[in] _mth order of curve derivative to interpolate.
/// \param[in] _t parameter value (range 0 to 1).
/// \return the interpolated mth derivative, or [INF, INF, INF] on
/// error. Use Vector3d::IsFinite() to check for an error.
public: Vector3d InterpolateMthDerivative(const unsigned int _fromIndex,
const unsigned int _mth,
const double _s) const;
/// \brief Tells the spline whether it should automatically
/// calculate tangents on demand as points are added.
/// \remarks The spline calculates tangents at each point
/// automatically based on the input points. Normally it
/// does this every time a point changes. However, if you
/// have a lot of points to add in one go, you probably
/// don't want to incur this overhead and would prefer to
/// defer the calculation until you are finished setting all
/// the points. You can do this by calling this method with a
/// parameter of 'false'. Just remember to manually call the
/// recalcTangents method when you are done.
/// \param[in] _autoCalc If true, tangents are calculated for you whenever
/// a point changes. If false, you must call RecalcTangents to
/// recalculate them when it best suits.
public: void AutoCalculate(bool _autoCalc);
/// \brief Recalculates the tangents associated with this spline.
/// \remarks If you tell the spline not to update on demand by
/// calling setAutoCalculate(false) then you must call this
/// after completing your updates to the spline points.
public: void RecalcTangents();
/// \brief Rebuilds spline segments.
private: void Rebuild();
/// \internal
/// \brief Maps \p _t parameter value over the whole spline
/// to the right segment (starting at point \p _index) with
/// the proper parameter value fraction \p _fraction.
/// \remarks Arc length is assumed to be linear with the parameter.
/// \param[in] _t parameter value over the whole spline (range 0 to 1).
/// \param[out] _index point index at which the segment starts.
/// \param[out] _fraction parameter value fraction for the given segment.
/// \return True on success.
private: bool MapToSegment(const double _t,
unsigned int &_index,
double &_fraction) const;
/// \internal
/// \brief Private data pointer
private: SplinePrivate *dataPtr;
};
}
}
}
#endif
|
