summaryrefslogtreecommitdiff
path: root/src/ipa/libipa/pwl.cpp
blob: 9b213754ec7db86cdb0fb62f5f0a73348b8b6503 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright (C) 2019, Raspberry Pi Ltd
 * Copyright (C) 2024, Ideas on Board Oy
 *
 * Piecewise linear functions
 */

#include "pwl.h"

#include <assert.h>
#include <cmath>
#include <sstream>
#include <stdexcept>

/**
 * \file pwl.h
 * \brief Piecewise linear functions
 */

namespace libcamera {

namespace ipa {

/**
 * \class Pwl
 * \brief Describe a univariate piecewise linear function in two-dimensional
 * real space
 *
 * A piecewise linear function is a univariate function that maps reals to
 * reals, and it is composed of multiple straight-line segments.
 *
 * While a mathematical piecewise linear function would usually be defined by
 * a list of linear functions and for which values of the domain they apply,
 * this Pwl class is instead defined by a list of points at which these line
 * segments intersect. These intersecting points are known as knots.
 *
 * https://en.wikipedia.org/wiki/Piecewise_linear_function
 *
 * A consequence of the Pwl class being defined by knots instead of linear
 * functions is that the values of the piecewise linear function past the ends
 * of the function are constants as opposed to linear functions. In a
 * mathematical piecewise linear function that is defined by multiple linear
 * functions, the ends of the function are also linear functions and hence grow
 * to infinity (or negative infinity). However, since this Pwl class is defined
 * by knots, the y-value of the leftmost and rightmost knots will hold for all
 * x values to negative infinity and positive infinity, respectively.
 */

/**
 * \typedef Pwl::Point
 * \brief Describe a point in two-dimensional real space
 */

/**
 * \class Pwl::Interval
 * \brief Describe an interval in one-dimensional real space
 */

/**
 * \fn Pwl::Interval::Interval(double _start, double _end)
 * \brief Construct an interval
 * \param[in] _start Start of the interval
 * \param[in] _end End of the interval
 */

/**
 * \fn Pwl::Interval::contains
 * \brief Check if a given value falls within the interval
 * \param[in] value Value to check
 * \return True if the value falls within the interval, including its bounds,
 * or false otherwise
 */

/**
 * \fn Pwl::Interval::clamp
 * \brief Clamp a value such that it is within the interval
 * \param[in] value Value to clamp
 * \return The clamped value
 */

/**
 * \fn Pwl::Interval::length
 * \brief Compute the length of the interval
 * \return The length of the interval
 */

/**
 * \var Pwl::Interval::start
 * \brief Start of the interval
 */

/**
 * \var Pwl::Interval::end
 * \brief End of the interval
 */

/**
 * \brief Construct an empty piecewise linear function
 */
Pwl::Pwl()
{
}

/**
 * \brief Construct a piecewise linear function from a list of 2D points
 * \param[in] points Vector of points from which to construct the piecewise
 * linear function
 *
 * \a points must be in ascending order of x-value.
 */
Pwl::Pwl(const std::vector<Point> &points)
	: points_(points)
{
}

/**
 * \copydoc Pwl::Pwl(const std::vector<Point> &points)
 *
 * The contents of the \a points vector is moved to the newly constructed Pwl
 * instance.
 */
Pwl::Pwl(std::vector<Point> &&points)
	: points_(std::move(points))
{
}

/**
 * \brief Append a point to the end of the piecewise linear function
 * \param[in] x x-coordinate of the point to add to the piecewise linear function
 * \param[in] y y-coordinate of the point to add to the piecewise linear function
 * \param[in] eps Epsilon for the minimum x distance between points (optional)
 *
 * The point's x-coordinate must be greater than the x-coordinate of the last
 * (= greatest) point already in the piecewise linear function.
 */
void Pwl::append(double x, double y, const double eps)
{
	if (points_.empty() || points_.back().x() + eps < x)
		points_.push_back(Point({ x, y }));
}

/**
 * \brief Prepend a point to the beginning of the piecewise linear function
 * \param[in] x x-coordinate of the point to add to the piecewise linear function
 * \param[in] y y-coordinate of the point to add to the piecewise linear function
 * \param[in] eps Epsilon for the minimum x distance between points (optional)
 *
 * The point's x-coordinate must be less than the x-coordinate of the first
 * (= smallest) point already in the piecewise linear function.
 */
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 }));
}

/**
 * \fn Pwl::empty() const
 * \brief Check if the piecewise linear function is empty
 * \return True if there are no points in the function, false otherwise
 */

/**
 * \fn Pwl::size() const
 * \brief Retrieve the number of points in the piecewise linear function
 * \return The number of points in the piecewise linear function
 */

/**
 * \brief Get the domain of the piecewise linear function
 * \return An interval representing the domain
 */
Pwl::Interval Pwl::domain() const
{
	return Interval(points_[0].x(), points_[points_.size() - 1].x());
}

/**
 * \brief Get the range of the piecewise linear function
 * \return An interval representing the range
 */
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);
}

/**
 * \brief Evaluate the piecewise linear function
 * \param[in] x The x value to input into the function
 * \param[inout] span Initial guess for span
 * \param[in] updateSpan Set to true to update span
 *
 * 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.
 *
 *  \return The result of evaluating the piecewise linear function at position \a x
 */
double Pwl::eval(double x, int *span, bool updateSpan) const
{
	int index = findSpan(x, span && *span != -1
					? *span
					: points_.size() / 2 - 1);
	if (span && updateSpan)
		*span = index;
	return points_[index].y() +
	       (x - points_[index].x()) * (points_[index + 1].y() - points_[index].y()) /
		       (points_[index + 1].x() - points_[index].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;
}

/**
 * \brief Compute the inverse function
 * \param[in] eps Epsilon for the minimum x distance between points (optional)
 *
 * The output includes whether the resulting inverse function is a proper
 * (true) inverse, or only a best effort (e.g. input was non-monotonic).
 *
 * \return A pair of the inverse piecewise linear function, and whether or not
 * the result is a proper/true inverse
 */
std::pair<Pwl, bool> Pwl::inverse(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.
	 */
	bool trueInverse = !(neither || (appended && prepended));

	return { inverse, trueInverse };
}

/**
 * \brief Compose two piecewise linear functions together
 * \param[in] other The "other" piecewise linear function
 * \param[in] eps Epsilon for the minimum x distance between points (optional)
 *
 * The "this" function is done first, and "other" after.
 *
 * \return The composed piecewise linear function
 */
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({ Point({ 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;
}

/**
 * \brief Apply function to (x, y) values at every control point
 * \param[in] f Function to be applied
 */
void Pwl::map(std::function<void(double x, double y)> f) const
{
	for (auto &pt : points_)
		f(pt.x(), pt.y());
}

/**
 * \brief Apply function to (x, y0, y1) values wherever either Pwl has a
 * control point.
 * \param[in] pwl0 First piecewise linear function
 * \param[in] pwl1 Second piecewise linear function
 * \param[in] f Function to be applied
 *
 * This applies the function \a f to every parameter (x, y0, y1), where x is
 * the combined list of x-values from \a pwl0 and \a pwl1, y0 is the y-value
 * for the given x in \a pwl0, and y1 is the y-value for the same x in \a pwl1.
 */
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));
	}
}

/**
 * \brief Combine two Pwls
 * \param[in] pwl0 First piecewise linear function
 * \param[in] pwl1 Second piecewise linear function
 * \param[in] f Function to be applied
 * \param[in] eps Epsilon for the minimum x distance between points (optional)
 *
 * Create a new Pwl where the y values are given by running \a f wherever
 * either pwl has a knot.
 *
 * \return The combined pwl
 */
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;
}

/**
 * \brief Multiply the piecewise linear function
 * \param[in] d Scalar multiplier to multiply the function by
 * \return This function, after it has been multiplied by \a d
 */
Pwl &Pwl::operator*=(double d)
{
	for (auto &pt : points_)
		pt[1] *= d;
	return *this;
}

/**
 * \brief Assemble and return a string describing the piecewise linear function
 * \return A string describing the piecewise linear function
 */
std::string Pwl::toString() const
{
	std::stringstream ss;
	ss << "Pwl { ";
	for (auto &p : points_)
		ss << "(" << p.x() << ", " << p.y() << ") ";
	ss << "}";
	return ss.str();
}

} /* namespace ipa */

#ifndef __DOXYGEN__
/*
 * The YAML data shall be a list of numerical values with an even number of
 * elements. They are parsed in pairs into x and y points in the piecewise
 * linear function, and added in order. x must be monotonically increasing.
 */
template<>
std::optional<ipa::Pwl>
YamlObject::Getter<ipa::Pwl>::get(const YamlObject &obj) const
{
	if (!obj.size() || obj.size() % 2)
		return std::nullopt;

	ipa::Pwl pwl;

	const auto &list = obj.asList();

	for (auto it = list.begin(); it != list.end(); it++) {
		auto x = it->get<double>();
		if (!x)
			return std::nullopt;
		auto y = (++it)->get<double>();
		if (!y)
			return std::nullopt;

		pwl.append(*x, *y);
	}

	if (pwl.size() != obj.size() / 2)
		return std::nullopt;

	return pwl;
}
#endif /* __DOXYGEN__ */

} /* namespace libcamera */