/* SPDX-License-Identifier: BSD-2-Clause */ /* * Copyright (C) 2019, Raspberry Pi (Trading) Limited * * pwl.hpp - piecewise linear functions interface */ #pragma once #include #include #include 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 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 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 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 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 points_; }; } // namespace RPiController ='#n5'>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