fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 06:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

BezierQuad: set cached_arc_length once computed

Browse Source
This commit is contained in:
Dennis Mannhart
2018-07-25 08:50:10 +02:00
parent fe48e06f9d
commit 6ddf214bbd
1 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/bezier/BezierQuad.cpp
+6 -6
View File
@@ -140,18 +140,18 @@ Tp BezierQuad<Tp>::getArcLength(const Tp resolution)
// step size // step size
Tp h = (_duration) / n; Tp h = (_duration) / n;
// get integration // get integration
Tp area = (Tp)0;
Data y; Data y;
_cached_arc_length = (Tp)0;
for (int i = 1; i < n; i++) { for (int i = 1; i < n; i++) {
y = getVelocity(h * i); y = getVelocity(h * i);
if (i % 2 == 1) { if (i % 2 == 1) {
area += (Tp)4 * y.length(); _cached_arc_length += (Tp)4 * y.length();
} else { } else {
area += (Tp)2 * y.length(); _cached_arc_length += (Tp)2 * y.length();
} }
} }
@@ -161,12 +161,12 @@ Tp BezierQuad<Tp>::getArcLength(const Tp resolution)
yn = vn.length(); yn = vn.length();
// 1/3 simpsons rule // 1/3 simpsons rule
area = h / (Tp)3 * (y0 + yn + area); _cached_arc_length = h / (Tp)3 * (y0 + yn + _cached_arc_length);
// update cached resolution // update cache
_cached_resolution = resolution; _cached_resolution = resolution;
return area; return _cached_arc_length;
} }
template<typename Tp> template<typename Tp>