[v8,2/4] ipa: libipa: Copy pwl from rpi

20257 diff mbox series

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>

---
No change in v8

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

Hi Paul,

Thank you for the patch.

On Tue, Jun 11, 2024 at 10:24:28PM +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>

If we don't squash this with 3/4 before merging,

Reviewed-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>

> 
> ---
> No change in v8
> 
> 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 &params)
> +{
> +	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 &params);
> +	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 */