55 _cached_resolution = (Tp)(-1);
69 return (_pt0 * ((Tp)1 - t / _duration) * ((Tp)1 - t / _duration) + _ctrl * (Tp)2 * ((
70 Tp)1 - t / _duration) * t / _duration + _pt1 *
71 t / _duration * t / _duration);
77 return (((_ctrl - _pt0) * _duration + (_pt0 - _ctrl * (Tp)2 + _pt1) * t) * (Tp)2 / (_duration * _duration));
83 return ((_pt0 - _ctrl * (Tp)2 + _pt1) * (Tp)2 / (_duration * _duration));
89 point = getPoint(time);
90 vel = getVelocity(time);
91 acc = getAcceleration();
99 Tp t = _goldenSectionSearch(pose);
102 getStates(point, vel, acc, t);
106 template<
typename Tp>
112 _duration = duration;
113 _pt0 = _ctrl - vel0 * _duration / (Tp)2;
114 _pt1 = _ctrl + vel1 * _duration / (Tp)2;
115 _cached_resolution = (Tp)(-1);
118 template<
typename Tp>
123 if ((_cached_resolution > (Tp)0) && (_cached_resolution <= resolution)) {
124 return _cached_arc_length;
128 int n = (int)(roundf(_duration / resolution));
136 Tp h = (_duration) / n;
139 _cached_arc_length = (Tp)0;
142 for (
int i = 1; i < n; i++) {
144 y = getVelocity(h * i);
147 _cached_arc_length += (Tp)4 * y.
length();
150 _cached_arc_length += (Tp)2 * y.
length();
155 Tp y0 = getVelocity((Tp)0).length();
156 Tp yn = getVelocity(_duration).length();
159 _cached_arc_length = h / (Tp)3 * (y0 + yn + _cached_arc_length);
162 _cached_resolution = resolution;
164 return _cached_arc_length;
167 template<
typename Tp>
171 Tp t = _goldenSectionSearch(pose);
175 return (pose - point).length();
178 template<
typename Tp>
183 b = _duration * (Tp)1;
185 c = b - (b - a) / GOLDEN_RATIO;
186 d = a + (b - a) / GOLDEN_RATIO;
189 if (_getDistanceSquared(c, pose) < _getDistanceSquared(d, pose)) {
196 c = b - (b - a) / GOLDEN_RATIO;
197 d = a + (b - a) / GOLDEN_RATIO;
200 return (b + a) / (Tp)2;
203 template<
typename Tp>
Vector3_t getVelocity(const Tp t)
void getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1)
Tp getArcLength(const Tp resolution)
static constexpr double GOLDEN_RATIO
void setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, const Tp duration=(Tp) 1)
Vector3_t getAcceleration()
void getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Vector3_t pose)
void getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp t)
Tp getDistToClosestPoint(const Vector3_t &pose)
Vector3_t getPoint(const Tp t)
Return point on bezier point corresponding to time t.
Tp _getDistanceSquared(const Tp t, const Vector3_t &pose)
Tp _goldenSectionSearch(const Vector3_t &pose)
void setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, Tp duration=(Tp) 1)
Set new bezier points and duration.
static constexpr double RESOLUTION