| Message ID | 20240610141941.2947785-3-paul.elder@ideasonboard.com |
|---|---|
| State | Superseded |
| Headers | show |
| Series |
|
| Related | show |
Hi Paul, Thank you for the patch. On Mon, Jun 10, 2024 at 11:19:39PM +0900, Paul Elder wrote: > Copy the piecewise linear function code from Raspberry Pi. > > Signed-off-by: Paul Elder <paul.elder@ideasonboard.com> > Reviewed-by: Stefan Klug <stefan.klug@ideasonboard.com> > Acked-by: David Plowman <david.plowman@raspberrypi.com> > Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com> This will break bisection due to missing documentation. Is that an issue ? Should we squash patches 2/4 and 3/4 ? > > --- > No change in v7 > > No change in v6 > > Changes in v5: > - remove meson.build to prevent compilation this early in the merge > > Changes in v4: > - update the copy > > No change in v3 > > No change in v2 > --- > src/ipa/libipa/pwl.cpp | 269 +++++++++++++++++++++++++++++++++++++++++ > src/ipa/libipa/pwl.h | 127 +++++++++++++++++++ > 2 files changed, 396 insertions(+) > create mode 100644 src/ipa/libipa/pwl.cpp > create mode 100644 src/ipa/libipa/pwl.h > > diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp > new file mode 100644 > index 000000000000..e39123767aa6 > --- /dev/null > +++ b/src/ipa/libipa/pwl.cpp > @@ -0,0 +1,269 @@ > +/* SPDX-License-Identifier: BSD-2-Clause */ > +/* > + * Copyright (C) 2019, Raspberry Pi Ltd > + * > + * piecewise linear functions > + */ > + > +#include <cassert> > +#include <cmath> > +#include <stdexcept> > + > +#include "pwl.h" > + > +using namespace RPiController; > + > +int Pwl::read(const libcamera::YamlObject ¶ms) > +{ > + if (!params.size() || params.size() % 2) > + return -EINVAL; > + > + const auto &list = params.asList(); > + > + for (auto it = list.begin(); it != list.end(); it++) { > + auto x = it->get<double>(); > + if (!x) > + return -EINVAL; > + if (it != list.begin() && *x <= points_.back().x) > + return -EINVAL; > + > + auto y = (++it)->get<double>(); > + if (!y) > + return -EINVAL; > + > + points_.push_back(Point(*x, *y)); > + } > + > + return 0; > +} > + > +void Pwl::append(double x, double y, const double eps) > +{ > + if (points_.empty() || points_.back().x + eps < x) > + points_.push_back(Point(x, y)); > +} > + > +void Pwl::prepend(double x, double y, const double eps) > +{ > + if (points_.empty() || points_.front().x - eps > x) > + points_.insert(points_.begin(), Point(x, y)); > +} > + > +Pwl::Interval Pwl::domain() const > +{ > + return Interval(points_[0].x, points_[points_.size() - 1].x); > +} > + > +Pwl::Interval Pwl::range() const > +{ > + double lo = points_[0].y, hi = lo; > + for (auto &p : points_) > + lo = std::min(lo, p.y), hi = std::max(hi, p.y); > + return Interval(lo, hi); > +} > + > +bool Pwl::empty() const > +{ > + return points_.empty(); > +} > + > +double Pwl::eval(double x, int *spanPtr, bool updateSpan) const > +{ > + int span = findSpan(x, spanPtr && *spanPtr != -1 ? *spanPtr : points_.size() / 2 - 1); > + if (spanPtr && updateSpan) > + *spanPtr = span; > + return points_[span].y + > + (x - points_[span].x) * (points_[span + 1].y - points_[span].y) / > + (points_[span + 1].x - points_[span].x); > +} > + > +int Pwl::findSpan(double x, int span) const > +{ > + /* > + * Pwls are generally small, so linear search may well be faster than > + * binary, though could review this if large PWls start turning up. > + */ > + int lastSpan = points_.size() - 2; > + /* > + * some algorithms may call us with span pointing directly at the last > + * control point > + */ > + span = std::max(0, std::min(lastSpan, span)); > + while (span < lastSpan && x >= points_[span + 1].x) > + span++; > + while (span && x < points_[span].x) > + span--; > + return span; > +} > + > +Pwl::PerpType Pwl::invert(Point const &xy, Point &perp, int &span, > + const double eps) const > +{ > + assert(span >= -1); > + bool prevOffEnd = false; > + for (span = span + 1; span < (int)points_.size() - 1; span++) { > + Point spanVec = points_[span + 1] - points_[span]; > + double t = ((xy - points_[span]) % spanVec) / spanVec.len2(); > + if (t < -eps) /* off the start of this span */ > + { > + if (span == 0) { > + perp = points_[span]; > + return PerpType::Start; > + } else if (prevOffEnd) { > + perp = points_[span]; > + return PerpType::Vertex; > + } > + } else if (t > 1 + eps) /* off the end of this span */ > + { > + if (span == (int)points_.size() - 2) { > + perp = points_[span + 1]; > + return PerpType::End; > + } > + prevOffEnd = true; > + } else /* a true perpendicular */ > + { > + perp = points_[span] + spanVec * t; > + return PerpType::Perpendicular; > + } > + } > + return PerpType::None; > +} > + > +Pwl Pwl::inverse(bool *trueInverse, const double eps) const > +{ > + bool appended = false, prepended = false, neither = false; > + Pwl inverse; > + > + for (Point const &p : points_) { > + if (inverse.empty()) > + inverse.append(p.y, p.x, eps); > + else if (std::abs(inverse.points_.back().x - p.y) <= eps || > + std::abs(inverse.points_.front().x - p.y) <= eps) > + /* do nothing */; > + else if (p.y > inverse.points_.back().x) { > + inverse.append(p.y, p.x, eps); > + appended = true; > + } else if (p.y < inverse.points_.front().x) { > + inverse.prepend(p.y, p.x, eps); > + prepended = true; > + } else > + neither = true; > + } > + > + /* > + * This is not a proper inverse if we found ourselves putting points > + * onto both ends of the inverse, or if there were points that couldn't > + * go on either. > + */ > + if (trueInverse) > + *trueInverse = !(neither || (appended && prepended)); > + > + return inverse; > +} > + > +Pwl Pwl::compose(Pwl const &other, const double eps) const > +{ > + double thisX = points_[0].x, thisY = points_[0].y; > + int thisSpan = 0, otherSpan = other.findSpan(thisY, 0); > + Pwl result({ { thisX, other.eval(thisY, &otherSpan, false) } }); > + while (thisSpan != (int)points_.size() - 1) { > + double dx = points_[thisSpan + 1].x - points_[thisSpan].x, > + dy = points_[thisSpan + 1].y - points_[thisSpan].y; > + if (std::abs(dy) > eps && > + otherSpan + 1 < (int)other.points_.size() && > + points_[thisSpan + 1].y >= > + other.points_[otherSpan + 1].x + eps) { > + /* > + * next control point in result will be where this > + * function's y reaches the next span in other > + */ > + thisX = points_[thisSpan].x + > + (other.points_[otherSpan + 1].x - > + points_[thisSpan].y) * > + dx / dy; > + thisY = other.points_[++otherSpan].x; > + } else if (std::abs(dy) > eps && otherSpan > 0 && > + points_[thisSpan + 1].y <= > + other.points_[otherSpan - 1].x - eps) { > + /* > + * next control point in result will be where this > + * function's y reaches the previous span in other > + */ > + thisX = points_[thisSpan].x + > + (other.points_[otherSpan + 1].x - > + points_[thisSpan].y) * > + dx / dy; > + thisY = other.points_[--otherSpan].x; > + } else { > + /* we stay in the same span in other */ > + thisSpan++; > + thisX = points_[thisSpan].x, > + thisY = points_[thisSpan].y; > + } > + result.append(thisX, other.eval(thisY, &otherSpan, false), > + eps); > + } > + return result; > +} > + > +void Pwl::map(std::function<void(double x, double y)> f) const > +{ > + for (auto &pt : points_) > + f(pt.x, pt.y); > +} > + > +void Pwl::map2(Pwl const &pwl0, Pwl const &pwl1, > + std::function<void(double x, double y0, double y1)> f) > +{ > + int span0 = 0, span1 = 0; > + double x = std::min(pwl0.points_[0].x, pwl1.points_[0].x); > + f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false)); > + while (span0 < (int)pwl0.points_.size() - 1 || > + span1 < (int)pwl1.points_.size() - 1) { > + if (span0 == (int)pwl0.points_.size() - 1) > + x = pwl1.points_[++span1].x; > + else if (span1 == (int)pwl1.points_.size() - 1) > + x = pwl0.points_[++span0].x; > + else if (pwl0.points_[span0 + 1].x > pwl1.points_[span1 + 1].x) > + x = pwl1.points_[++span1].x; > + else > + x = pwl0.points_[++span0].x; > + f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false)); > + } > +} > + > +Pwl Pwl::combine(Pwl const &pwl0, Pwl const &pwl1, > + std::function<double(double x, double y0, double y1)> f, > + const double eps) > +{ > + Pwl result; > + map2(pwl0, pwl1, [&](double x, double y0, double y1) { > + result.append(x, f(x, y0, y1), eps); > + }); > + return result; > +} > + > +void Pwl::matchDomain(Interval const &domain, bool clip, const double eps) > +{ > + int span = 0; > + prepend(domain.start, eval(clip ? points_[0].x : domain.start, &span), > + eps); > + span = points_.size() - 2; > + append(domain.end, eval(clip ? points_.back().x : domain.end, &span), > + eps); > +} > + > +Pwl &Pwl::operator*=(double d) > +{ > + for (auto &pt : points_) > + pt.y *= d; > + return *this; > +} > + > +void Pwl::debug(FILE *fp) const > +{ > + fprintf(fp, "Pwl {\n"); > + for (auto &p : points_) > + fprintf(fp, "\t(%g, %g)\n", p.x, p.y); > + fprintf(fp, "}\n"); > +} > diff --git a/src/ipa/libipa/pwl.h b/src/ipa/libipa/pwl.h > new file mode 100644 > index 000000000000..7d5e7e4d3fda > --- /dev/null > +++ b/src/ipa/libipa/pwl.h > @@ -0,0 +1,127 @@ > +/* SPDX-License-Identifier: BSD-2-Clause */ > +/* > + * Copyright (C) 2019, Raspberry Pi Ltd > + * > + * piecewise linear functions interface > + */ > +#pragma once > + > +#include <functional> > +#include <math.h> > +#include <vector> > + > +#include "libcamera/internal/yaml_parser.h" > + > +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) {} > + int read(const libcamera::YamlObject ¶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 *spanPtr = nullptr, > + bool updateSpan = 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 *trueInverse = 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 */
On Tue, Jun 11, 2024 at 03:00:41AM +0300, Laurent Pinchart wrote: > Hi Paul, > > Thank you for the patch. > > On Mon, Jun 10, 2024 at 11:19:39PM +0900, Paul Elder wrote: > > Copy the piecewise linear function code from Raspberry Pi. > > > > Signed-off-by: Paul Elder <paul.elder@ideasonboard.com> > > Reviewed-by: Stefan Klug <stefan.klug@ideasonboard.com> > > Acked-by: David Plowman <david.plowman@raspberrypi.com> > > Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com> > > This will break bisection due to missing documentation. Is that an issue > ? Should we squash patches 2/4 and 3/4 ? If we squash then we can't tell what changed, so it depends on if that is more valuable or if maintaining bisection is more valuable. Or we say that the former is only important for review and it's fine to squash. Paul > > > > > --- > > No change in v7 > > > > No change in v6 > > > > Changes in v5: > > - remove meson.build to prevent compilation this early in the merge > > > > Changes in v4: > > - update the copy > > > > No change in v3 > > > > No change in v2 > > --- > > src/ipa/libipa/pwl.cpp | 269 +++++++++++++++++++++++++++++++++++++++++ > > src/ipa/libipa/pwl.h | 127 +++++++++++++++++++ > > 2 files changed, 396 insertions(+) > > create mode 100644 src/ipa/libipa/pwl.cpp > > create mode 100644 src/ipa/libipa/pwl.h > > > > diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp > > new file mode 100644 > > index 000000000000..e39123767aa6 > > --- /dev/null > > +++ b/src/ipa/libipa/pwl.cpp > > @@ -0,0 +1,269 @@ > > +/* SPDX-License-Identifier: BSD-2-Clause */ > > +/* > > + * Copyright (C) 2019, Raspberry Pi Ltd > > + * > > + * piecewise linear functions > > + */ > > + > > +#include <cassert> > > +#include <cmath> > > +#include <stdexcept> > > + > > +#include "pwl.h" > > + > > +using namespace RPiController; > > + > > +int Pwl::read(const libcamera::YamlObject ¶ms) > > +{ > > + if (!params.size() || params.size() % 2) > > + return -EINVAL; > > + > > + const auto &list = params.asList(); > > + > > + for (auto it = list.begin(); it != list.end(); it++) { > > + auto x = it->get<double>(); > > + if (!x) > > + return -EINVAL; > > + if (it != list.begin() && *x <= points_.back().x) > > + return -EINVAL; > > + > > + auto y = (++it)->get<double>(); > > + if (!y) > > + return -EINVAL; > > + > > + points_.push_back(Point(*x, *y)); > > + } > > + > > + return 0; > > +} > > + > > +void Pwl::append(double x, double y, const double eps) > > +{ > > + if (points_.empty() || points_.back().x + eps < x) > > + points_.push_back(Point(x, y)); > > +} > > + > > +void Pwl::prepend(double x, double y, const double eps) > > +{ > > + if (points_.empty() || points_.front().x - eps > x) > > + points_.insert(points_.begin(), Point(x, y)); > > +} > > + > > +Pwl::Interval Pwl::domain() const > > +{ > > + return Interval(points_[0].x, points_[points_.size() - 1].x); > > +} > > + > > +Pwl::Interval Pwl::range() const > > +{ > > + double lo = points_[0].y, hi = lo; > > + for (auto &p : points_) > > + lo = std::min(lo, p.y), hi = std::max(hi, p.y); > > + return Interval(lo, hi); > > +} > > + > > +bool Pwl::empty() const > > +{ > > + return points_.empty(); > > +} > > + > > +double Pwl::eval(double x, int *spanPtr, bool updateSpan) const > > +{ > > + int span = findSpan(x, spanPtr && *spanPtr != -1 ? *spanPtr : points_.size() / 2 - 1); > > + if (spanPtr && updateSpan) > > + *spanPtr = span; > > + return points_[span].y + > > + (x - points_[span].x) * (points_[span + 1].y - points_[span].y) / > > + (points_[span + 1].x - points_[span].x); > > +} > > + > > +int Pwl::findSpan(double x, int span) const > > +{ > > + /* > > + * Pwls are generally small, so linear search may well be faster than > > + * binary, though could review this if large PWls start turning up. > > + */ > > + int lastSpan = points_.size() - 2; > > + /* > > + * some algorithms may call us with span pointing directly at the last > > + * control point > > + */ > > + span = std::max(0, std::min(lastSpan, span)); > > + while (span < lastSpan && x >= points_[span + 1].x) > > + span++; > > + while (span && x < points_[span].x) > > + span--; > > + return span; > > +} > > + > > +Pwl::PerpType Pwl::invert(Point const &xy, Point &perp, int &span, > > + const double eps) const > > +{ > > + assert(span >= -1); > > + bool prevOffEnd = false; > > + for (span = span + 1; span < (int)points_.size() - 1; span++) { > > + Point spanVec = points_[span + 1] - points_[span]; > > + double t = ((xy - points_[span]) % spanVec) / spanVec.len2(); > > + if (t < -eps) /* off the start of this span */ > > + { > > + if (span == 0) { > > + perp = points_[span]; > > + return PerpType::Start; > > + } else if (prevOffEnd) { > > + perp = points_[span]; > > + return PerpType::Vertex; > > + } > > + } else if (t > 1 + eps) /* off the end of this span */ > > + { > > + if (span == (int)points_.size() - 2) { > > + perp = points_[span + 1]; > > + return PerpType::End; > > + } > > + prevOffEnd = true; > > + } else /* a true perpendicular */ > > + { > > + perp = points_[span] + spanVec * t; > > + return PerpType::Perpendicular; > > + } > > + } > > + return PerpType::None; > > +} > > + > > +Pwl Pwl::inverse(bool *trueInverse, const double eps) const > > +{ > > + bool appended = false, prepended = false, neither = false; > > + Pwl inverse; > > + > > + for (Point const &p : points_) { > > + if (inverse.empty()) > > + inverse.append(p.y, p.x, eps); > > + else if (std::abs(inverse.points_.back().x - p.y) <= eps || > > + std::abs(inverse.points_.front().x - p.y) <= eps) > > + /* do nothing */; > > + else if (p.y > inverse.points_.back().x) { > > + inverse.append(p.y, p.x, eps); > > + appended = true; > > + } else if (p.y < inverse.points_.front().x) { > > + inverse.prepend(p.y, p.x, eps); > > + prepended = true; > > + } else > > + neither = true; > > + } > > + > > + /* > > + * This is not a proper inverse if we found ourselves putting points > > + * onto both ends of the inverse, or if there were points that couldn't > > + * go on either. > > + */ > > + if (trueInverse) > > + *trueInverse = !(neither || (appended && prepended)); > > + > > + return inverse; > > +} > > + > > +Pwl Pwl::compose(Pwl const &other, const double eps) const > > +{ > > + double thisX = points_[0].x, thisY = points_[0].y; > > + int thisSpan = 0, otherSpan = other.findSpan(thisY, 0); > > + Pwl result({ { thisX, other.eval(thisY, &otherSpan, false) } }); > > + while (thisSpan != (int)points_.size() - 1) { > > + double dx = points_[thisSpan + 1].x - points_[thisSpan].x, > > + dy = points_[thisSpan + 1].y - points_[thisSpan].y; > > + if (std::abs(dy) > eps && > > + otherSpan + 1 < (int)other.points_.size() && > > + points_[thisSpan + 1].y >= > > + other.points_[otherSpan + 1].x + eps) { > > + /* > > + * next control point in result will be where this > > + * function's y reaches the next span in other > > + */ > > + thisX = points_[thisSpan].x + > > + (other.points_[otherSpan + 1].x - > > + points_[thisSpan].y) * > > + dx / dy; > > + thisY = other.points_[++otherSpan].x; > > + } else if (std::abs(dy) > eps && otherSpan > 0 && > > + points_[thisSpan + 1].y <= > > + other.points_[otherSpan - 1].x - eps) { > > + /* > > + * next control point in result will be where this > > + * function's y reaches the previous span in other > > + */ > > + thisX = points_[thisSpan].x + > > + (other.points_[otherSpan + 1].x - > > + points_[thisSpan].y) * > > + dx / dy; > > + thisY = other.points_[--otherSpan].x; > > + } else { > > + /* we stay in the same span in other */ > > + thisSpan++; > > + thisX = points_[thisSpan].x, > > + thisY = points_[thisSpan].y; > > + } > > + result.append(thisX, other.eval(thisY, &otherSpan, false), > > + eps); > > + } > > + return result; > > +} > > + > > +void Pwl::map(std::function<void(double x, double y)> f) const > > +{ > > + for (auto &pt : points_) > > + f(pt.x, pt.y); > > +} > > + > > +void Pwl::map2(Pwl const &pwl0, Pwl const &pwl1, > > + std::function<void(double x, double y0, double y1)> f) > > +{ > > + int span0 = 0, span1 = 0; > > + double x = std::min(pwl0.points_[0].x, pwl1.points_[0].x); > > + f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false)); > > + while (span0 < (int)pwl0.points_.size() - 1 || > > + span1 < (int)pwl1.points_.size() - 1) { > > + if (span0 == (int)pwl0.points_.size() - 1) > > + x = pwl1.points_[++span1].x; > > + else if (span1 == (int)pwl1.points_.size() - 1) > > + x = pwl0.points_[++span0].x; > > + else if (pwl0.points_[span0 + 1].x > pwl1.points_[span1 + 1].x) > > + x = pwl1.points_[++span1].x; > > + else > > + x = pwl0.points_[++span0].x; > > + f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false)); > > + } > > +} > > + > > +Pwl Pwl::combine(Pwl const &pwl0, Pwl const &pwl1, > > + std::function<double(double x, double y0, double y1)> f, > > + const double eps) > > +{ > > + Pwl result; > > + map2(pwl0, pwl1, [&](double x, double y0, double y1) { > > + result.append(x, f(x, y0, y1), eps); > > + }); > > + return result; > > +} > > + > > +void Pwl::matchDomain(Interval const &domain, bool clip, const double eps) > > +{ > > + int span = 0; > > + prepend(domain.start, eval(clip ? points_[0].x : domain.start, &span), > > + eps); > > + span = points_.size() - 2; > > + append(domain.end, eval(clip ? points_.back().x : domain.end, &span), > > + eps); > > +} > > + > > +Pwl &Pwl::operator*=(double d) > > +{ > > + for (auto &pt : points_) > > + pt.y *= d; > > + return *this; > > +} > > + > > +void Pwl::debug(FILE *fp) const > > +{ > > + fprintf(fp, "Pwl {\n"); > > + for (auto &p : points_) > > + fprintf(fp, "\t(%g, %g)\n", p.x, p.y); > > + fprintf(fp, "}\n"); > > +} > > diff --git a/src/ipa/libipa/pwl.h b/src/ipa/libipa/pwl.h > > new file mode 100644 > > index 000000000000..7d5e7e4d3fda > > --- /dev/null > > +++ b/src/ipa/libipa/pwl.h > > @@ -0,0 +1,127 @@ > > +/* SPDX-License-Identifier: BSD-2-Clause */ > > +/* > > + * Copyright (C) 2019, Raspberry Pi Ltd > > + * > > + * piecewise linear functions interface > > + */ > > +#pragma once > > + > > +#include <functional> > > +#include <math.h> > > +#include <vector> > > + > > +#include "libcamera/internal/yaml_parser.h" > > + > > +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) {} > > + int read(const libcamera::YamlObject ¶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 *spanPtr = nullptr, > > + bool updateSpan = 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 *trueInverse = 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 */ > > -- > Regards, > > Laurent Pinchart
On Tue, Jun 11, 2024 at 06:58:15PM +0900, Paul Elder wrote: > On Tue, Jun 11, 2024 at 03:00:41AM +0300, Laurent Pinchart wrote: > > On Mon, Jun 10, 2024 at 11:19:39PM +0900, Paul Elder wrote: > > > Copy the piecewise linear function code from Raspberry Pi. > > > > > > Signed-off-by: Paul Elder <paul.elder@ideasonboard.com> > > > Reviewed-by: Stefan Klug <stefan.klug@ideasonboard.com> > > > Acked-by: David Plowman <david.plowman@raspberrypi.com> > > > Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com> > > > > This will break bisection due to missing documentation. Is that an issue > > ? Should we squash patches 2/4 and 3/4 ? > > If we squash then we can't tell what changed, so it depends on if that > is more valuable or if maintaining bisection is more valuable. > > Or we say that the former is only important for review and it's fine to > squash. For review splitting can be useful, although I reviewed 3/4 using a 'git diff HEAD~2' as the patch itself was really hard to read. I don't completely object to merging 2/4 and 3/4 as separate patches, but I'm not sure I see the value in this case. > > > --- > > > No change in v7 > > > > > > No change in v6 > > > > > > Changes in v5: > > > - remove meson.build to prevent compilation this early in the merge > > > > > > Changes in v4: > > > - update the copy > > > > > > No change in v3 > > > > > > No change in v2 > > > --- > > > src/ipa/libipa/pwl.cpp | 269 +++++++++++++++++++++++++++++++++++++++++ > > > src/ipa/libipa/pwl.h | 127 +++++++++++++++++++ > > > 2 files changed, 396 insertions(+) > > > create mode 100644 src/ipa/libipa/pwl.cpp > > > create mode 100644 src/ipa/libipa/pwl.h > > > > > > diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp > > > new file mode 100644 > > > index 000000000000..e39123767aa6 > > > --- /dev/null > > > +++ b/src/ipa/libipa/pwl.cpp > > > @@ -0,0 +1,269 @@ > > > +/* SPDX-License-Identifier: BSD-2-Clause */ > > > +/* > > > + * Copyright (C) 2019, Raspberry Pi Ltd > > > + * > > > + * piecewise linear functions > > > + */ > > > + > > > +#include <cassert> > > > +#include <cmath> > > > +#include <stdexcept> > > > + > > > +#include "pwl.h" > > > + > > > +using namespace RPiController; > > > + > > > +int Pwl::read(const libcamera::YamlObject ¶ms) > > > +{ > > > + if (!params.size() || params.size() % 2) > > > + return -EINVAL; > > > + > > > + const auto &list = params.asList(); > > > + > > > + for (auto it = list.begin(); it != list.end(); it++) { > > > + auto x = it->get<double>(); > > > + if (!x) > > > + return -EINVAL; > > > + if (it != list.begin() && *x <= points_.back().x) > > > + return -EINVAL; > > > + > > > + auto y = (++it)->get<double>(); > > > + if (!y) > > > + return -EINVAL; > > > + > > > + points_.push_back(Point(*x, *y)); > > > + } > > > + > > > + return 0; > > > +} > > > + > > > +void Pwl::append(double x, double y, const double eps) > > > +{ > > > + if (points_.empty() || points_.back().x + eps < x) > > > + points_.push_back(Point(x, y)); > > > +} > > > + > > > +void Pwl::prepend(double x, double y, const double eps) > > > +{ > > > + if (points_.empty() || points_.front().x - eps > x) > > > + points_.insert(points_.begin(), Point(x, y)); > > > +} > > > + > > > +Pwl::Interval Pwl::domain() const > > > +{ > > > + return Interval(points_[0].x, points_[points_.size() - 1].x); > > > +} > > > + > > > +Pwl::Interval Pwl::range() const > > > +{ > > > + double lo = points_[0].y, hi = lo; > > > + for (auto &p : points_) > > > + lo = std::min(lo, p.y), hi = std::max(hi, p.y); > > > + return Interval(lo, hi); > > > +} > > > + > > > +bool Pwl::empty() const > > > +{ > > > + return points_.empty(); > > > +} > > > + > > > +double Pwl::eval(double x, int *spanPtr, bool updateSpan) const > > > +{ > > > + int span = findSpan(x, spanPtr && *spanPtr != -1 ? *spanPtr : points_.size() / 2 - 1); > > > + if (spanPtr && updateSpan) > > > + *spanPtr = span; > > > + return points_[span].y + > > > + (x - points_[span].x) * (points_[span + 1].y - points_[span].y) / > > > + (points_[span + 1].x - points_[span].x); > > > +} > > > + > > > +int Pwl::findSpan(double x, int span) const > > > +{ > > > + /* > > > + * Pwls are generally small, so linear search may well be faster than > > > + * binary, though could review this if large PWls start turning up. > > > + */ > > > + int lastSpan = points_.size() - 2; > > > + /* > > > + * some algorithms may call us with span pointing directly at the last > > > + * control point > > > + */ > > > + span = std::max(0, std::min(lastSpan, span)); > > > + while (span < lastSpan && x >= points_[span + 1].x) > > > + span++; > > > + while (span && x < points_[span].x) > > > + span--; > > > + return span; > > > +} > > > + > > > +Pwl::PerpType Pwl::invert(Point const &xy, Point &perp, int &span, > > > + const double eps) const > > > +{ > > > + assert(span >= -1); > > > + bool prevOffEnd = false; > > > + for (span = span + 1; span < (int)points_.size() - 1; span++) { > > > + Point spanVec = points_[span + 1] - points_[span]; > > > + double t = ((xy - points_[span]) % spanVec) / spanVec.len2(); > > > + if (t < -eps) /* off the start of this span */ > > > + { > > > + if (span == 0) { > > > + perp = points_[span]; > > > + return PerpType::Start; > > > + } else if (prevOffEnd) { > > > + perp = points_[span]; > > > + return PerpType::Vertex; > > > + } > > > + } else if (t > 1 + eps) /* off the end of this span */ > > > + { > > > + if (span == (int)points_.size() - 2) { > > > + perp = points_[span + 1]; > > > + return PerpType::End; > > > + } > > > + prevOffEnd = true; > > > + } else /* a true perpendicular */ > > > + { > > > + perp = points_[span] + spanVec * t; > > > + return PerpType::Perpendicular; > > > + } > > > + } > > > + return PerpType::None; > > > +} > > > + > > > +Pwl Pwl::inverse(bool *trueInverse, const double eps) const > > > +{ > > > + bool appended = false, prepended = false, neither = false; > > > + Pwl inverse; > > > + > > > + for (Point const &p : points_) { > > > + if (inverse.empty()) > > > + inverse.append(p.y, p.x, eps); > > > + else if (std::abs(inverse.points_.back().x - p.y) <= eps || > > > + std::abs(inverse.points_.front().x - p.y) <= eps) > > > + /* do nothing */; > > > + else if (p.y > inverse.points_.back().x) { > > > + inverse.append(p.y, p.x, eps); > > > + appended = true; > > > + } else if (p.y < inverse.points_.front().x) { > > > + inverse.prepend(p.y, p.x, eps); > > > + prepended = true; > > > + } else > > > + neither = true; > > > + } > > > + > > > + /* > > > + * This is not a proper inverse if we found ourselves putting points > > > + * onto both ends of the inverse, or if there were points that couldn't > > > + * go on either. > > > + */ > > > + if (trueInverse) > > > + *trueInverse = !(neither || (appended && prepended)); > > > + > > > + return inverse; > > > +} > > > + > > > +Pwl Pwl::compose(Pwl const &other, const double eps) const > > > +{ > > > + double thisX = points_[0].x, thisY = points_[0].y; > > > + int thisSpan = 0, otherSpan = other.findSpan(thisY, 0); > > > + Pwl result({ { thisX, other.eval(thisY, &otherSpan, false) } }); > > > + while (thisSpan != (int)points_.size() - 1) { > > > + double dx = points_[thisSpan + 1].x - points_[thisSpan].x, > > > + dy = points_[thisSpan + 1].y - points_[thisSpan].y; > > > + if (std::abs(dy) > eps && > > > + otherSpan + 1 < (int)other.points_.size() && > > > + points_[thisSpan + 1].y >= > > > + other.points_[otherSpan + 1].x + eps) { > > > + /* > > > + * next control point in result will be where this > > > + * function's y reaches the next span in other > > > + */ > > > + thisX = points_[thisSpan].x + > > > + (other.points_[otherSpan + 1].x - > > > + points_[thisSpan].y) * > > > + dx / dy; > > > + thisY = other.points_[++otherSpan].x; > > > + } else if (std::abs(dy) > eps && otherSpan > 0 && > > > + points_[thisSpan + 1].y <= > > > + other.points_[otherSpan - 1].x - eps) { > > > + /* > > > + * next control point in result will be where this > > > + * function's y reaches the previous span in other > > > + */ > > > + thisX = points_[thisSpan].x + > > > + (other.points_[otherSpan + 1].x - > > > + points_[thisSpan].y) * > > > + dx / dy; > > > + thisY = other.points_[--otherSpan].x; > > > + } else { > > > + /* we stay in the same span in other */ > > > + thisSpan++; > > > + thisX = points_[thisSpan].x, > > > + thisY = points_[thisSpan].y; > > > + } > > > + result.append(thisX, other.eval(thisY, &otherSpan, false), > > > + eps); > > > + } > > > + return result; > > > +} > > > + > > > +void Pwl::map(std::function<void(double x, double y)> f) const > > > +{ > > > + for (auto &pt : points_) > > > + f(pt.x, pt.y); > > > +} > > > + > > > +void Pwl::map2(Pwl const &pwl0, Pwl const &pwl1, > > > + std::function<void(double x, double y0, double y1)> f) > > > +{ > > > + int span0 = 0, span1 = 0; > > > + double x = std::min(pwl0.points_[0].x, pwl1.points_[0].x); > > > + f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false)); > > > + while (span0 < (int)pwl0.points_.size() - 1 || > > > + span1 < (int)pwl1.points_.size() - 1) { > > > + if (span0 == (int)pwl0.points_.size() - 1) > > > + x = pwl1.points_[++span1].x; > > > + else if (span1 == (int)pwl1.points_.size() - 1) > > > + x = pwl0.points_[++span0].x; > > > + else if (pwl0.points_[span0 + 1].x > pwl1.points_[span1 + 1].x) > > > + x = pwl1.points_[++span1].x; > > > + else > > > + x = pwl0.points_[++span0].x; > > > + f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false)); > > > + } > > > +} > > > + > > > +Pwl Pwl::combine(Pwl const &pwl0, Pwl const &pwl1, > > > + std::function<double(double x, double y0, double y1)> f, > > > + const double eps) > > > +{ > > > + Pwl result; > > > + map2(pwl0, pwl1, [&](double x, double y0, double y1) { > > > + result.append(x, f(x, y0, y1), eps); > > > + }); > > > + return result; > > > +} > > > + > > > +void Pwl::matchDomain(Interval const &domain, bool clip, const double eps) > > > +{ > > > + int span = 0; > > > + prepend(domain.start, eval(clip ? points_[0].x : domain.start, &span), > > > + eps); > > > + span = points_.size() - 2; > > > + append(domain.end, eval(clip ? points_.back().x : domain.end, &span), > > > + eps); > > > +} > > > + > > > +Pwl &Pwl::operator*=(double d) > > > +{ > > > + for (auto &pt : points_) > > > + pt.y *= d; > > > + return *this; > > > +} > > > + > > > +void Pwl::debug(FILE *fp) const > > > +{ > > > + fprintf(fp, "Pwl {\n"); > > > + for (auto &p : points_) > > > + fprintf(fp, "\t(%g, %g)\n", p.x, p.y); > > > + fprintf(fp, "}\n"); > > > +} > > > diff --git a/src/ipa/libipa/pwl.h b/src/ipa/libipa/pwl.h > > > new file mode 100644 > > > index 000000000000..7d5e7e4d3fda > > > --- /dev/null > > > +++ b/src/ipa/libipa/pwl.h > > > @@ -0,0 +1,127 @@ > > > +/* SPDX-License-Identifier: BSD-2-Clause */ > > > +/* > > > + * Copyright (C) 2019, Raspberry Pi Ltd > > > + * > > > + * piecewise linear functions interface > > > + */ > > > +#pragma once > > > + > > > +#include <functional> > > > +#include <math.h> > > > +#include <vector> > > > + > > > +#include "libcamera/internal/yaml_parser.h" > > > + > > > +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) {} > > > + int read(const libcamera::YamlObject ¶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 *spanPtr = nullptr, > > > + bool updateSpan = 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 *trueInverse = 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 */
Quoting Paul Elder (2024-06-11 10:58:15) > On Tue, Jun 11, 2024 at 03:00:41AM +0300, Laurent Pinchart wrote: > > Hi Paul, > > > > Thank you for the patch. > > > > On Mon, Jun 10, 2024 at 11:19:39PM +0900, Paul Elder wrote: > > > Copy the piecewise linear function code from Raspberry Pi. > > > > > > Signed-off-by: Paul Elder <paul.elder@ideasonboard.com> > > > Reviewed-by: Stefan Klug <stefan.klug@ideasonboard.com> > > > Acked-by: David Plowman <david.plowman@raspberrypi.com> > > > Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com> > > > > This will break bisection due to missing documentation. Is that an issue > > ? Should we squash patches 2/4 and 3/4 ? > > If we squash then we can't tell what changed, so it depends on if that > is more valuable or if maintaining bisection is more valuable. > > Or we say that the former is only important for review and it's fine to > squash. I would say definitely valuable for review, so a valuable split to start with and I'm fine if it's preferred to squash for bisectability. -- Kieran
diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp new file mode 100644 index 000000000000..e39123767aa6 --- /dev/null +++ b/src/ipa/libipa/pwl.cpp @@ -0,0 +1,269 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * piecewise linear functions + */ + +#include <cassert> +#include <cmath> +#include <stdexcept> + +#include "pwl.h" + +using namespace RPiController; + +int Pwl::read(const libcamera::YamlObject ¶ms) +{ + if (!params.size() || params.size() % 2) + return -EINVAL; + + const auto &list = params.asList(); + + for (auto it = list.begin(); it != list.end(); it++) { + auto x = it->get<double>(); + if (!x) + return -EINVAL; + if (it != list.begin() && *x <= points_.back().x) + return -EINVAL; + + auto y = (++it)->get<double>(); + if (!y) + return -EINVAL; + + points_.push_back(Point(*x, *y)); + } + + return 0; +} + +void Pwl::append(double x, double y, const double eps) +{ + if (points_.empty() || points_.back().x + eps < x) + points_.push_back(Point(x, y)); +} + +void Pwl::prepend(double x, double y, const double eps) +{ + if (points_.empty() || points_.front().x - eps > x) + points_.insert(points_.begin(), Point(x, y)); +} + +Pwl::Interval Pwl::domain() const +{ + return Interval(points_[0].x, points_[points_.size() - 1].x); +} + +Pwl::Interval Pwl::range() const +{ + double lo = points_[0].y, hi = lo; + for (auto &p : points_) + lo = std::min(lo, p.y), hi = std::max(hi, p.y); + return Interval(lo, hi); +} + +bool Pwl::empty() const +{ + return points_.empty(); +} + +double Pwl::eval(double x, int *spanPtr, bool updateSpan) const +{ + int span = findSpan(x, spanPtr && *spanPtr != -1 ? *spanPtr : points_.size() / 2 - 1); + if (spanPtr && updateSpan) + *spanPtr = span; + return points_[span].y + + (x - points_[span].x) * (points_[span + 1].y - points_[span].y) / + (points_[span + 1].x - points_[span].x); +} + +int Pwl::findSpan(double x, int span) const +{ + /* + * Pwls are generally small, so linear search may well be faster than + * binary, though could review this if large PWls start turning up. + */ + int lastSpan = points_.size() - 2; + /* + * some algorithms may call us with span pointing directly at the last + * control point + */ + span = std::max(0, std::min(lastSpan, span)); + while (span < lastSpan && x >= points_[span + 1].x) + span++; + while (span && x < points_[span].x) + span--; + return span; +} + +Pwl::PerpType Pwl::invert(Point const &xy, Point &perp, int &span, + const double eps) const +{ + assert(span >= -1); + bool prevOffEnd = false; + for (span = span + 1; span < (int)points_.size() - 1; span++) { + Point spanVec = points_[span + 1] - points_[span]; + double t = ((xy - points_[span]) % spanVec) / spanVec.len2(); + if (t < -eps) /* off the start of this span */ + { + if (span == 0) { + perp = points_[span]; + return PerpType::Start; + } else if (prevOffEnd) { + perp = points_[span]; + return PerpType::Vertex; + } + } else if (t > 1 + eps) /* off the end of this span */ + { + if (span == (int)points_.size() - 2) { + perp = points_[span + 1]; + return PerpType::End; + } + prevOffEnd = true; + } else /* a true perpendicular */ + { + perp = points_[span] + spanVec * t; + return PerpType::Perpendicular; + } + } + return PerpType::None; +} + +Pwl Pwl::inverse(bool *trueInverse, const double eps) const +{ + bool appended = false, prepended = false, neither = false; + Pwl inverse; + + for (Point const &p : points_) { + if (inverse.empty()) + inverse.append(p.y, p.x, eps); + else if (std::abs(inverse.points_.back().x - p.y) <= eps || + std::abs(inverse.points_.front().x - p.y) <= eps) + /* do nothing */; + else if (p.y > inverse.points_.back().x) { + inverse.append(p.y, p.x, eps); + appended = true; + } else if (p.y < inverse.points_.front().x) { + inverse.prepend(p.y, p.x, eps); + prepended = true; + } else + neither = true; + } + + /* + * This is not a proper inverse if we found ourselves putting points + * onto both ends of the inverse, or if there were points that couldn't + * go on either. + */ + if (trueInverse) + *trueInverse = !(neither || (appended && prepended)); + + return inverse; +} + +Pwl Pwl::compose(Pwl const &other, const double eps) const +{ + double thisX = points_[0].x, thisY = points_[0].y; + int thisSpan = 0, otherSpan = other.findSpan(thisY, 0); + Pwl result({ { thisX, other.eval(thisY, &otherSpan, false) } }); + while (thisSpan != (int)points_.size() - 1) { + double dx = points_[thisSpan + 1].x - points_[thisSpan].x, + dy = points_[thisSpan + 1].y - points_[thisSpan].y; + if (std::abs(dy) > eps && + otherSpan + 1 < (int)other.points_.size() && + points_[thisSpan + 1].y >= + other.points_[otherSpan + 1].x + eps) { + /* + * next control point in result will be where this + * function's y reaches the next span in other + */ + thisX = points_[thisSpan].x + + (other.points_[otherSpan + 1].x - + points_[thisSpan].y) * + dx / dy; + thisY = other.points_[++otherSpan].x; + } else if (std::abs(dy) > eps && otherSpan > 0 && + points_[thisSpan + 1].y <= + other.points_[otherSpan - 1].x - eps) { + /* + * next control point in result will be where this + * function's y reaches the previous span in other + */ + thisX = points_[thisSpan].x + + (other.points_[otherSpan + 1].x - + points_[thisSpan].y) * + dx / dy; + thisY = other.points_[--otherSpan].x; + } else { + /* we stay in the same span in other */ + thisSpan++; + thisX = points_[thisSpan].x, + thisY = points_[thisSpan].y; + } + result.append(thisX, other.eval(thisY, &otherSpan, false), + eps); + } + return result; +} + +void Pwl::map(std::function<void(double x, double y)> f) const +{ + for (auto &pt : points_) + f(pt.x, pt.y); +} + +void Pwl::map2(Pwl const &pwl0, Pwl const &pwl1, + std::function<void(double x, double y0, double y1)> f) +{ + int span0 = 0, span1 = 0; + double x = std::min(pwl0.points_[0].x, pwl1.points_[0].x); + f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false)); + while (span0 < (int)pwl0.points_.size() - 1 || + span1 < (int)pwl1.points_.size() - 1) { + if (span0 == (int)pwl0.points_.size() - 1) + x = pwl1.points_[++span1].x; + else if (span1 == (int)pwl1.points_.size() - 1) + x = pwl0.points_[++span0].x; + else if (pwl0.points_[span0 + 1].x > pwl1.points_[span1 + 1].x) + x = pwl1.points_[++span1].x; + else + x = pwl0.points_[++span0].x; + f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false)); + } +} + +Pwl Pwl::combine(Pwl const &pwl0, Pwl const &pwl1, + std::function<double(double x, double y0, double y1)> f, + const double eps) +{ + Pwl result; + map2(pwl0, pwl1, [&](double x, double y0, double y1) { + result.append(x, f(x, y0, y1), eps); + }); + return result; +} + +void Pwl::matchDomain(Interval const &domain, bool clip, const double eps) +{ + int span = 0; + prepend(domain.start, eval(clip ? points_[0].x : domain.start, &span), + eps); + span = points_.size() - 2; + append(domain.end, eval(clip ? points_.back().x : domain.end, &span), + eps); +} + +Pwl &Pwl::operator*=(double d) +{ + for (auto &pt : points_) + pt.y *= d; + return *this; +} + +void Pwl::debug(FILE *fp) const +{ + fprintf(fp, "Pwl {\n"); + for (auto &p : points_) + fprintf(fp, "\t(%g, %g)\n", p.x, p.y); + fprintf(fp, "}\n"); +} diff --git a/src/ipa/libipa/pwl.h b/src/ipa/libipa/pwl.h new file mode 100644 index 000000000000..7d5e7e4d3fda --- /dev/null +++ b/src/ipa/libipa/pwl.h @@ -0,0 +1,127 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * piecewise linear functions interface + */ +#pragma once + +#include <functional> +#include <math.h> +#include <vector> + +#include "libcamera/internal/yaml_parser.h" + +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) {} + int read(const libcamera::YamlObject ¶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 *spanPtr = nullptr, + bool updateSpan = 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 *trueInverse = 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 */