[2/4] ipa: libipa: Copy pwl from rpi
diff mbox series

Message ID 20240405080259.1806453-3-paul.elder@ideasonboard.com
State New
Headers show
Series
  • ipa: Move Pwl from Raspberry Pi to libipa
Related show

Commit Message

Paul Elder April 5, 2024, 8:02 a.m. UTC 
Copy the piecewise linear function code from Raspberry Pi.

Signed-off-by: Paul Elder <paul.elder@ideasonboard.com>
---
 src/ipa/libipa/meson.build |   2 +
 src/ipa/libipa/pwl.cpp     | 267 +++++++++++++++++++++++++++++++++++++
 src/ipa/libipa/pwl.h       | 123 +++++++++++++++++
 3 files changed, 392 insertions(+)
 create mode 100644 src/ipa/libipa/pwl.cpp
 create mode 100644 src/ipa/libipa/pwl.h

Comments

Naushir Patuck April 8, 2024, 8:14 a.m. UTC | #1 
Hi Paul,

On Fri, 5 Apr 2024 at 09:03, Paul Elder <paul.elder@ideasonboard.com> wrote:
>
> Copy the piecewise linear function code from Raspberry Pi.
>

This seems like a good time to mention something I was intending on
doing for a while...

Our PWL library is used internally in a few places
(libcamera/libpisp/rpicam-apps/some internal tools).  Right now each
of these libraries has a copy of pretty much the same pwl.c/pwl.h
file.  I was intending to create a stand alone Raspberry Pi PWL
library that could be linked with these so we only have one canonical
copy of the source code.  This could then be included into
libcamera/libpipa as a subproject if needed.  Do you think this would
be helpful and suitable for you?  We will need to look at possibly
templating the Point type if needed as well to be more appropriate for
libcamera's use.

Regards,
Naush

> Signed-off-by: Paul Elder <paul.elder@ideasonboard.com>
> ---
>  src/ipa/libipa/meson.build |   2 +
>  src/ipa/libipa/pwl.cpp     | 267 +++++++++++++++++++++++++++++++++++++
>  src/ipa/libipa/pwl.h       | 123 +++++++++++++++++
>  3 files changed, 392 insertions(+)
>  create mode 100644 src/ipa/libipa/pwl.cpp
>  create mode 100644 src/ipa/libipa/pwl.h
>
> diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build
> index 31cc8d70..8fba16e1 100644
> --- a/src/ipa/libipa/meson.build
> +++ b/src/ipa/libipa/meson.build
> @@ -8,6 +8,7 @@ libipa_headers = files([
>      'fc_queue.h',
>      'histogram.h',
>      'module.h',
> +    'pwl.h',
>  ])
>
>  libipa_sources = files([
> @@ -18,6 +19,7 @@ libipa_sources = files([
>      'fc_queue.cpp',
>      'histogram.cpp',
>      'module.cpp',
> +    'pwl.cpp'
>  ])
>
>  libipa_includes = include_directories('..')
> diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp
> new file mode 100644
> index 00000000..09f5d65c
> --- /dev/null
> +++ b/src/ipa/libipa/pwl.cpp
> @@ -0,0 +1,267 @@
> +/* SPDX-License-Identifier: BSD-2-Clause */
> +/*
> + * Copyright (C) 2019, Raspberry Pi Ltd
> + *
> + * pwl.cpp - piecewise linear functions
> + */
> +
> +#include <cassert>
> +#include <cmath>
> +#include <stdexcept>
> +
> +#include "pwl.h"
> +
> +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 00000000..7a6a6452
> --- /dev/null
> +++ b/src/ipa/libipa/pwl.h
> @@ -0,0 +1,123 @@
> +/* SPDX-License-Identifier: BSD-2-Clause */
> +/*
> + * Copyright (C) 2019, Raspberry Pi Ltd
> + *
> + * pwl.h - piecewise linear functions interface
> + */
> +#pragma once
> +
> +#include <functional>
> +#include <math.h>
> +#include <vector>
> +
> +#include "libcamera/internal/yaml_parser.h"
> +
> +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_;
> +};
> --
> 2.39.2
>
Stefan Klug April 15, 2024, 12:07 p.m. UTC | #2 
Hi Paul,

thanks for the patch.

As this is only a copy of existing files, there is not much to review
here.

Reviewed-by: Stefan Klug <stefan.klug@ideasonboard.com> 

Cheers,
Stefan

On Fri, Apr 05, 2024 at 05:02:57PM +0900, Paul Elder wrote:
> Copy the piecewise linear function code from Raspberry Pi.
> 
> Signed-off-by: Paul Elder <paul.elder@ideasonboard.com>
> ---
>  src/ipa/libipa/meson.build |   2 +
>  src/ipa/libipa/pwl.cpp     | 267 +++++++++++++++++++++++++++++++++++++
>  src/ipa/libipa/pwl.h       | 123 +++++++++++++++++
>  3 files changed, 392 insertions(+)
>  create mode 100644 src/ipa/libipa/pwl.cpp
>  create mode 100644 src/ipa/libipa/pwl.h
> 
> diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build
> index 31cc8d70..8fba16e1 100644
> --- a/src/ipa/libipa/meson.build
> +++ b/src/ipa/libipa/meson.build
> @@ -8,6 +8,7 @@ libipa_headers = files([
>      'fc_queue.h',
>      'histogram.h',
>      'module.h',
> +    'pwl.h',
>  ])
>  
>  libipa_sources = files([
> @@ -18,6 +19,7 @@ libipa_sources = files([
>      'fc_queue.cpp',
>      'histogram.cpp',
>      'module.cpp',
> +    'pwl.cpp'
>  ])
>  
>  libipa_includes = include_directories('..')
> diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp
> new file mode 100644
> index 00000000..09f5d65c
> --- /dev/null
> +++ b/src/ipa/libipa/pwl.cpp
> @@ -0,0 +1,267 @@
> +/* SPDX-License-Identifier: BSD-2-Clause */
> +/*
> + * Copyright (C) 2019, Raspberry Pi Ltd
> + *
> + * pwl.cpp - piecewise linear functions
> + */
> +
> +#include <cassert>
> +#include <cmath>
> +#include <stdexcept>
> +
> +#include "pwl.h"
> +
> +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 00000000..7a6a6452
> --- /dev/null
> +++ b/src/ipa/libipa/pwl.h
> @@ -0,0 +1,123 @@
> +/* SPDX-License-Identifier: BSD-2-Clause */
> +/*
> + * Copyright (C) 2019, Raspberry Pi Ltd
> + *
> + * pwl.h - piecewise linear functions interface
> + */
> +#pragma once
> +
> +#include <functional>
> +#include <math.h>
> +#include <vector>
> +
> +#include "libcamera/internal/yaml_parser.h"
> +
> +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_;
> +};
> -- 
> 2.39.2
>
Paul Elder April 26, 2024, 7:39 a.m. UTC | #3 
Hi Naush,

On Mon, Apr 08, 2024 at 09:14:41AM +0100, Naushir Patuck wrote:
> Hi Paul,
> 
> On Fri, 5 Apr 2024 at 09:03, Paul Elder <paul.elder@ideasonboard.com> wrote:
> >
> > Copy the piecewise linear function code from Raspberry Pi.
> >
> 
> This seems like a good time to mention something I was intending on
> doing for a while...
> 
> Our PWL library is used internally in a few places
> (libcamera/libpisp/rpicam-apps/some internal tools).  Right now each
> of these libraries has a copy of pretty much the same pwl.c/pwl.h
> file.  I was intending to create a stand alone Raspberry Pi PWL
> library that could be linked with these so we only have one canonical
> copy of the source code.  This could then be included into
> libcamera/libpipa as a subproject if needed.  Do you think this would
> be helpful and suitable for you?  We will need to look at possibly

Yeah that might be a good idea. (although I'm not in the position to
make these decisions)

In any case we need this right now so for now we'll go with this :)


Thanks,

Paul

> templating the Point type if needed as well to be more appropriate for
> libcamera's use.
> 
> Regards,
> Naush
> 
> > Signed-off-by: Paul Elder <paul.elder@ideasonboard.com>
> > ---
> >  src/ipa/libipa/meson.build |   2 +
> >  src/ipa/libipa/pwl.cpp     | 267 +++++++++++++++++++++++++++++++++++++
> >  src/ipa/libipa/pwl.h       | 123 +++++++++++++++++
> >  3 files changed, 392 insertions(+)
> >  create mode 100644 src/ipa/libipa/pwl.cpp
> >  create mode 100644 src/ipa/libipa/pwl.h
> >
> > diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build
> > index 31cc8d70..8fba16e1 100644
> > --- a/src/ipa/libipa/meson.build
> > +++ b/src/ipa/libipa/meson.build
> > @@ -8,6 +8,7 @@ libipa_headers = files([
> >      'fc_queue.h',
> >      'histogram.h',
> >      'module.h',
> > +    'pwl.h',
> >  ])
> >
> >  libipa_sources = files([
> > @@ -18,6 +19,7 @@ libipa_sources = files([
> >      'fc_queue.cpp',
> >      'histogram.cpp',
> >      'module.cpp',
> > +    'pwl.cpp'
> >  ])
> >
> >  libipa_includes = include_directories('..')
> > diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp
> > new file mode 100644
> > index 00000000..09f5d65c
> > --- /dev/null
> > +++ b/src/ipa/libipa/pwl.cpp
> > @@ -0,0 +1,267 @@
> > +/* SPDX-License-Identifier: BSD-2-Clause */
> > +/*
> > + * Copyright (C) 2019, Raspberry Pi Ltd
> > + *
> > + * pwl.cpp - piecewise linear functions
> > + */
> > +
> > +#include <cassert>
> > +#include <cmath>
> > +#include <stdexcept>
> > +
> > +#include "pwl.h"
> > +
> > +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 00000000..7a6a6452
> > --- /dev/null
> > +++ b/src/ipa/libipa/pwl.h
> > @@ -0,0 +1,123 @@
> > +/* SPDX-License-Identifier: BSD-2-Clause */
> > +/*
> > + * Copyright (C) 2019, Raspberry Pi Ltd
> > + *
> > + * pwl.h - piecewise linear functions interface
> > + */
> > +#pragma once
> > +
> > +#include <functional>
> > +#include <math.h>
> > +#include <vector>
> > +
> > +#include "libcamera/internal/yaml_parser.h"
> > +
> > +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_;
> > +};
> > --
> > 2.39.2
> >

Patch
diff mbox series 

diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build
index 31cc8d70..8fba16e1 100644
--- a/src/ipa/libipa/meson.build
+++ b/src/ipa/libipa/meson.build
@@ -8,6 +8,7 @@  libipa_headers = files([
     'fc_queue.h',
     'histogram.h',
     'module.h',
+    'pwl.h',
 ])
 
 libipa_sources = files([
@@ -18,6 +19,7 @@  libipa_sources = files([
     'fc_queue.cpp',
     'histogram.cpp',
     'module.cpp',
+    'pwl.cpp'
 ])
 
 libipa_includes = include_directories('..')
diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp
new file mode 100644
index 00000000..09f5d65c
--- /dev/null
+++ b/src/ipa/libipa/pwl.cpp
@@ -0,0 +1,267 @@ 
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi Ltd
+ *
+ * pwl.cpp - piecewise linear functions
+ */
+
+#include <cassert>
+#include <cmath>
+#include <stdexcept>
+
+#include "pwl.h"
+
+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 00000000..7a6a6452
--- /dev/null
+++ b/src/ipa/libipa/pwl.h
@@ -0,0 +1,123 @@ 
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi Ltd
+ *
+ * pwl.h - piecewise linear functions interface
+ */
+#pragma once
+
+#include <functional>
+#include <math.h>
+#include <vector>
+
+#include "libcamera/internal/yaml_parser.h"
+
+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_;
+};