From 7559610a7b84c9af89077c5ee12f28b0d5f15bcc Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Tue, 24 Jul 2018 08:34:14 +0200 Subject: [PATCH] BezierQuad: update description, replace Data with Vector3_t --- src/lib/bezier/BezierQuad.cpp | 62 +++++------ src/lib/bezier/BezierQuad.hpp | 136 ++++++++++++++--------- src/systemcmds/tests/test_bezierQuad.cpp | 6 +- 3 files changed, 111 insertions(+), 93 deletions(-) diff --git a/src/lib/bezier/BezierQuad.cpp b/src/lib/bezier/BezierQuad.cpp index 905ecd2c57..c69ba265e1 100644 --- a/src/lib/bezier/BezierQuad.cpp +++ b/src/lib/bezier/BezierQuad.cpp @@ -35,7 +35,6 @@ * Bezier function * * @author Dennis Mannhart - * */ #include "BezierQuad.hpp" @@ -47,7 +46,7 @@ namespace bezier #define RESOLUTION 0.0001 //represents resolution; end criterion for golden section search template -void BezierQuad::setBezier(const Data &pt0, const Data &ctrl, const Data &pt1, +void BezierQuad::setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, Tp duration) { _pt0 = pt0; @@ -55,11 +54,10 @@ void BezierQuad::setBezier(const Data &pt0, const Data &ctrl, const Data &pt _pt1 = pt1; _duration = duration; _cached_resolution = (Tp)(-1); - } template -void BezierQuad::getBezier(Data &pt0, Data &ctrl, Data &pt1) +void BezierQuad::getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1) { pt0 = _pt0; ctrl = _ctrl; @@ -87,7 +85,7 @@ matrix::Vector BezierQuad::getAcceleration() } template -void BezierQuad::getStates(Data &point, Data &vel, Data &acc, const Tp time) +void BezierQuad::getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp time) { point = getPoint(time); vel = getVelocity(time); @@ -95,22 +93,22 @@ void BezierQuad::getStates(Data &point, Data &vel, Data &acc, const Tp time) } template -void BezierQuad::getStatesClosest(Data &point, Data &vel, Data &acc, - const Data pose) +void BezierQuad::getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, + const Vector3_t pose) { - /* get t that corresponds to point closest on bezier point */ + // get t that corresponds to point closest on bezier point Tp t = _goldenSectionSearch(pose); - /* get states corresponding to t */ + // get states corresponding to t getStates(point, vel, acc, t); } template -void BezierQuad::setBezFromVel(const Data &ctrl, const Data &vel0, const Data &vel1, +void BezierQuad::setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, const Tp duration) { - /* update bezier points */ + // update bezier points _ctrl = ctrl; _duration = duration; _pt0 = _ctrl - vel0 * _duration / (Tp)2; @@ -129,8 +127,6 @@ Tp BezierQuad::getArcLength(const Tp resolution) // get number of elements int n = (int)(roundf(_duration / resolution)); - Data v0, vn; - Tp y0, yn; // check if n is even if (n % 2 == 1) { @@ -139,9 +135,10 @@ Tp BezierQuad::getArcLength(const Tp resolution) // step size Tp h = (_duration) / n; + // get integration - Data y; _cached_arc_length = (Tp)0; + Vector3_t y; for (int i = 1; i < n; i++) { @@ -155,10 +152,9 @@ Tp BezierQuad::getArcLength(const Tp resolution) } } - v0 = getVelocity((Tp)0); - vn = getVelocity(_duration); - y0 = v0.length(); - yn = vn.length(); + // velocity length at start and end points + Tp y0 = getVelocity((Tp)0).length(); + Tp yn = getVelocity(_duration).length(); // 1/3 simpsons rule _cached_arc_length = h / (Tp)3 * (y0 + yn + _cached_arc_length); @@ -170,27 +166,22 @@ Tp BezierQuad::getArcLength(const Tp resolution) } template -Tp BezierQuad::getDistToClosestPoint(const Data &pose) +Tp BezierQuad::getDistToClosestPoint(const Vector3_t &pose) { - - /* get t that corresponds to point closest on bezier point */ + // get t that corresponds to point closest on bezier point Tp t = _goldenSectionSearch(pose); - /* get closest point */ - Data point = getPoint(t); + // get closest point + Vector3_t point = getPoint(t); return (pose - point).length(); } -/* - * HELPER FUNCTIONS (private) - */ - template -Tp BezierQuad::_goldenSectionSearch(const Data &pose) +Tp BezierQuad::_goldenSectionSearch(const Vector3_t &pose) { Tp a, b, c, d; - a = (Tp)0; //represents most left point - b = _duration * (Tp)1; //represents most right point + a = (Tp)0; // represents most left point + b = _duration * (Tp)1; // represents most right point c = b - (b - a) / GOLDEN_RATIO; d = a + (b - a) / GOLDEN_RATIO; @@ -205,22 +196,21 @@ Tp BezierQuad::_goldenSectionSearch(const Data &pose) c = b - (b - a) / GOLDEN_RATIO; d = a + (b - a) / GOLDEN_RATIO; - } return (b + a) / (Tp)2; } template -Tp BezierQuad::_getDistanceSquared(const Tp t, const Data &pose) +Tp BezierQuad::_getDistanceSquared(const Tp t, const Vector3_t &pose) { - /* get point on bezier */ - Data vec = getPoint(t); + // get point on bezier + Vector3_t vec = getPoint(t); - /* get vector from point to pose */ + // get vector from point to pose vec = vec - pose; - /* norm squared */ + // norm squared return (vec * vec); } } diff --git a/src/lib/bezier/BezierQuad.hpp b/src/lib/bezier/BezierQuad.hpp index 0d03e99017..f46cb6d660 100644 --- a/src/lib/bezier/BezierQuad.hpp +++ b/src/lib/bezier/BezierQuad.hpp @@ -35,147 +35,175 @@ * @file BerzierQuad.hpp * * Quadratic bezier lib + * + * A quadratic bezier function/spline is completely defined by three 3D points in space and a time scaling factor. + * pt0 and pt1 define the start and end points of the spline. ctrl point is a point in space that effects the curvature + * of the spline. The time scaling factor (= duration) defines the time it takes to travel along the spline from pt0 to + * pt1. + * A bezier spline is a continuous function from which position, velocity and acceleration can be extracted. For a given spline, + * acceleration stays constant. */ #pragma once -#include -#include - #include namespace bezier { template -class __EXPORT BezierQuad +class BezierQuad { public: - using Data = matrix::Vector; + using Vector3_t = matrix::Vector; /** - * empty constructor + * Empty constructor */ BezierQuad(void) : - _pt0(Data()), _ctrl(Data()), _pt1(Data()), _duration(1.0f) {} + _pt0(Vector3_t()), _ctrl(Vector3_t()), _pt1(Vector3_t()), _duration(1.0f) {} /** - * constructor from array + * Constructor from array */ BezierQuad(const Tp pt0[3], const Tp ctrl[3], const Tp pt1[3], Tp duration = 1.0f) : - _pt0(Data(pt0)), _ctrl(Data(ctrl)), _pt1(Data(pt1)), _duration(duration) {} + _pt0(Vector3_t(pt0)), _ctrl(Vector3_t(ctrl)), _pt1(Vector3_t(pt1)), _duration(duration) {} /** - * constructor from vector + * Constructor from vector */ - BezierQuad(const Data &pt0, const Data &ctrl, const Data &pt1, + BezierQuad(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, Tp duration = 1.0f): _pt0(pt0), _ctrl(ctrl), _pt1(pt1), _duration(duration) {} /* - * return bezier points + * Get bezier points */ - void getBezier(Data &pt0, Data &ctrl, Data &pt1); + void getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1); /* - * get pt0 + * Return pt0 */ - Data getPt0() {return _pt0;} + Vector3_t getPt0() {return _pt0;} /* - * get ctrl + * Return ctrl */ - Data getCtrl() {return _ctrl;} + Vector3_t getCtrl() {return _ctrl;} /* - * get pt1 + * Return pt1 */ - Data getPt1() {return _pt1;} + Vector3_t getPt1() {return _pt1;} /** - * set new bezier points + * Set new bezier points and duration */ - void setBezier(const Data &pt0, const Data &ctrl, const Data &pt1, + void setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, Tp duration = 1.0f); /* - * set duration - */ + * Set duration + * + * @param time is the total time it takes to travel along the bezier spline. + */ void setDuration(const Tp time) {_duration = time;} /** - * get point on bezier point corresponding to t + * Return point on bezier point corresponding to time t + * + * @param t is a time in seconds in between [0, duration] + * @return a point on bezier */ - Data getPoint(const Tp t); + Vector3_t getPoint(const Tp t); /* * Distance to closest point given a position + * + * @param pose is a position in 3D space from which distance to bezier is computed. + * @return distance to closest point on bezier */ - Tp getDistToClosestPoint(const Data &pose); + Tp getDistToClosestPoint(const Vector3_t &pose); /* - * get velocity on bezier corresponding to t + * Return velocity on bezier corresponding to time t + * + * @param t is a time in seconds in between [0, duration] + * @return velocity vector at time t */ - Data getVelocity(const Tp t); + Vector3_t getVelocity(const Tp t); /* - * get acceleration on bezier corresponding to t + * Return acceleration on bezier corresponding to time t + * + * @return constant acceleration of bezier */ - Data getAcceleration(); + Vector3_t getAcceleration(); /* - * get states on bezier corresponding to t + * Get all states on bezier corresponding to time t */ - void getStates(Data &point, Data &vel, Data &acc, const Tp t); + void getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp t); /* - * get states on bezier which are closest to pose + * Get states on bezier which are closest to pose in space + * + * @param point is a posiiton on the spline that is closest to a given pose + * @param vel is the velocity at that given point + * @param acc is the acceleration for that spline + * @param pose represent a position in space from which closest point is computed */ - void getStatesClosest(Data &point, Data &vel, Data &acc, - const Data pose); + void getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, + const Vector3_t pose); /* - * compute bezier from velocity at bezier end points and ctrl point + * Compute bezier from velocity at bezier end points and ctrl point + * + * The bezier end points are fully defined by a given control point ctrl, the duration and + * the desired velocity vectors at the end points. */ - void setBezFromVel(const Data &ctrl, const Data &vel0, const Data &vel1, + void setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, const Tp duration = 1.0f); /* - * simpsons inegrattion applied to velocity + * Return the arc length of a bezier spline + * + * The arc length is computed with simpsons integration. + * @param resolution in meters. */ Tp getArcLength(const Tp resolution); private: - /* control points */ - Data _pt0; - Data _ctrl; - Data _pt1; - Tp _duration; + Vector3_t _pt0; /**< Bezier starting point */ + Vector3_t _ctrl; /**< Bezier control point */ + Vector3_t _pt1; /**< bezier end point */ + Tp _duration = (Tp)1; /**< Total time to travle along spline */ - /* cache */ - Tp _cached_arc_length; - Tp _cached_resolution = (Tp)(-1); // negative number means that cache is not up to date + Tp _cached_arc_length = (Tp)0; /**< The saved arc length of the spline */ + Tp _cached_resolution = (Tp)( + -1); /**< The resolution used to compute the arc length. Negative number means that cache is not up to date. */ /* - * Helper functions + * Golden section search */ - - /* golden section search */ - Tp _goldenSectionSearch(const Data &pose); + Tp _goldenSectionSearch(const Vector3_t &pose); /* - * get distance to point on bezier + * Get squared distance from 3D pose in space and a point on bezier. + * + * @param t is the time in between [0, duration] that defines a point on the bezier. + * @param pose is a 3D pose in space. */ - Tp _getDistanceSquared(const Tp t, const Data &pose); + Tp _getDistanceSquared(const Tp t, const Vector3_t &pose); }; -using BezierQuadf = BezierQuad; -using BezierQuadd = BezierQuad; +using BezierQuad_f = BezierQuad; +using BezierQuad_d = BezierQuad; } // include implementation diff --git a/src/systemcmds/tests/test_bezierQuad.cpp b/src/systemcmds/tests/test_bezierQuad.cpp index b224397481..97fa80e186 100644 --- a/src/systemcmds/tests/test_bezierQuad.cpp +++ b/src/systemcmds/tests/test_bezierQuad.cpp @@ -38,7 +38,7 @@ bool BezierQuadTest::_get_states_from_time() matrix::Vector3f pt1(0.5f, 0.0f, 0.0f); // create bezier with default t = [0,1] - bezier::BezierQuadf bz(pt0, ctrl, pt1); + bezier::BezierQuad_f bz(pt0, ctrl, pt1); matrix::Vector3f pos, vel, acc; float precision = 0.00001; @@ -165,7 +165,7 @@ bool BezierQuadTest::_get_arc_length() duration = random(0.0f, T); // create bezier - bezier::BezierQuadf bz(pt0, ctrl, pt1, duration); + bezier::BezierQuad_f bz(pt0, ctrl, pt1, duration); // compute arc length, triangle length and straigh length arc_length = bz.getArcLength(resolution); @@ -211,7 +211,7 @@ bool BezierQuadTest::_set_bez_from_vel() matrix::Vector3f vel1(random(low, max), random(low, max), random(low, max)); float duration = random(0.0f, 100.0f); - bezier::BezierQuadf bz;; + bezier::BezierQuad_f bz;; bz.setBezFromVel(ctrl, vel0, vel1, duration); // get velocity back