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
|
/* SPDX-License-Identifier: BSD-2-Clause */
/*
* Copyright (C) 2019, Raspberry Pi (Trading) Limited
*
* pwl.hpp - piecewise linear functions interface
*/
#pragma once
#include <math.h>
#include <vector>
#include <boost/property_tree/ptree.hpp>
namespace RPiController {
class Pwl
{
public:
struct Interval {
Interval(double _start, double _end) : start(_start), end(_end)
{
}
double start, end;
bool Contains(double value)
{
return value >= start && value <= end;
}
double Clip(double value)
{
return value < start ? start
: (value > end ? end : value);
}
double Len() const { return end - start; }
};
struct Point {
Point() : x(0), y(0) {}
Point(double _x, double _y) : x(_x), y(_y) {}
double x, y;
Point operator-(Point const &p) const
{
return Point(x - p.x, y - p.y);
}
Point operator+(Point const &p) const
{
return Point(x + p.x, y + p.y);
}
double operator%(Point const &p) const
{
return x * p.x + y * p.y;
}
Point operator*(double f) const { return Point(x * f, y * f); }
Point operator/(double f) const { return Point(x / f, y / f); }
double Len2() const { return x * x + y * y; }
double Len() const { return sqrt(Len2()); }
};
Pwl() {}
Pwl(std::vector<Point> const &points) : points_(points) {}
void Read(boost::property_tree::ptree const ¶ms);
void Append(double x, double y, const double eps = 1e-6);
void Prepend(double x, double y, const double eps = 1e-6);
Interval Domain() const;
Interval Range() const;
bool Empty() const;
// Evaluate Pwl, optionally supplying an initial guess for the
// "span". The "span" may be optionally be updated. If you want to know
// the "span" value but don't have an initial guess you can set it to
// -1.
double Eval(double x, int *span_ptr = nullptr,
bool update_span = true) const;
// Find perpendicular closest to xy, starting from span+1 so you can
// call it repeatedly to check for multiple closest points (set span to
// -1 on the first call). Also returns "pseudo" perpendiculars; see
// PerpType enum.
enum class PerpType {
None, // no perpendicular found
Start, // start of Pwl is closest point
End, // end of Pwl is closest point
Vertex, // vertex of Pwl is closest point
Perpendicular // true perpendicular found
};
PerpType Invert(Point const &xy, Point &perp, int &span,
const double eps = 1e-6) const;
// Compute the inverse function. Indicate if it is a proper (true)
// inverse, or only a best effort (e.g. input was non-monotonic).
Pwl Inverse(bool *true_inverse = nullptr, const double eps = 1e-6) const;
// Compose two Pwls together, doing "this" first and "other" after.
Pwl Compose(Pwl const &other, const double eps = 1e-6) const;
// Apply function to (x,y) values at every control point.
void Map(std::function<void(double x, double y)> f) const;
// Apply function to (x, y0, y1) values wherever either Pwl has a
// control point.
static void Map2(Pwl const &pwl0, Pwl const &pwl1,
std::function<void(double x, double y0, double y1)> f);
// Combine two Pwls, meaning we create a new Pwl where the y values are
// given by running f wherever either has a knot.
static Pwl
Combine(Pwl const &pwl0, Pwl const &pwl1,
std::function<double(double x, double y0, double y1)> f,
const double eps = 1e-6);
// Make "this" match (at least) the given domain. Any extension my be
// clipped or linear.
void MatchDomain(Interval const &domain, bool clip = true,
const double eps = 1e-6);
Pwl &operator*=(double d);
void Debug(FILE *fp = stdout) const;
private:
int findSpan(double x, int span) const;
std::vector<Point> points_;
};
} // namespace RPiController
|
