1 /*
2 * Copyright (C) 2023 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.numerical.interpolation;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20
21 /**
22 * Computes curve interpolation of multidimensional points using cubic splines.
23 * This interpolator uses an ordered set of N tabulated points in dim dimensions that lie on
24 * a one-dimensional curve, x0... xn-1, and interpolates values along the curve.
25 */
26 public class CurveInterpolator {
27
28 /**
29 * Number of points dimensions.
30 */
31 private final int dim;
32
33 /**
34 * True indicates a closed curve, false indicates an open one.
35 */
36 private final boolean cls;
37
38 /**
39 * Result of interpolation.
40 */
41 private final double[] ans;
42
43 /**
44 * Array of one dimensional cubic spline interpolators.
45 */
46 private final CubicSplineInterpolator[] srp;
47
48 /**
49 * Constructor.
50 *
51 * @param ptsin matrix containing points on each row. Number of columns determines number of
52 * dimensions of each point.
53 * @param close true indicates that points lie in a closed curve, false indicates they lie on
54 * an open one.
55 * @throws InterpolationException if a numerical exception occurs.
56 */
57 public CurveInterpolator(final Matrix ptsin, final boolean close) throws InterpolationException {
58 try {
59 final var n = ptsin.getRows();
60 dim = ptsin.getColumns();
61 // The trick for closed curves is to duplicate half a period at the beginning and end,
62 // and then use the middle half of the resulting spline
63 final var in = close ? 2 * n : n;
64 cls = close;
65 final var pts = new Matrix(dim, in);
66 final var s = new double[in];
67 ans = new double[dim];
68 srp = new CubicSplineInterpolator[dim];
69
70 final var p1 = new double[dim];
71 final var p2 = new double[dim];
72 final var p = new double[in];
73 final var end = dim - 1;
74
75 int i;
76 int ii;
77 int im;
78 int j;
79 int ofs;
80 double ss;
81 double soff;
82 double db;
83 double de;
84 ofs = close ? n / 2 : 0;
85 s[0] = 0.;
86 for (i = 0; i < in; i++) {
87 ii = (i - ofs + n) % n;
88 im = (ii - 1 + n) % n;
89 for (j = 0; j < dim; j++) {
90 // store transpose
91 pts.setElementAt(j, i, ptsin.getElementAt(ii, j));
92 }
93
94 if (i > 0) {
95 // Accumulate arc length
96 ptsin.getSubmatrixAsArray(ii, 0, ii, end, p1);
97 ptsin.getSubmatrixAsArray(im, 0, im, end, p2);
98 s[i] = s[i - 1] + rad(p1, p2);
99 if (s[i] == s[i - 1]) {
100 // Consecutive points may not be identical. For a close curve, the last
101 // data point should not duplicate the first.
102 throw new InterpolationException();
103 }
104 }
105 }
106
107 // Rescale parameter so that the interval [0,1] is the whole curve (or one period)
108 ss = close ? s[ofs + n] - s[ofs] : s[n - 1] - s[0];
109 soff = s[ofs];
110 for (i = 0; i < in; i++) {
111 s[i] = (s[i] - soff) / ss;
112 }
113
114 // Construct the splines using endpoint derivatives
115 final var endPts = in - 1;
116 for (j = 0; j < dim; j++) {
117 if (in < 4) {
118 db = 1.e99;
119 de = 1.e99;
120 } else {
121 pts.getSubmatrixAsArray(j, 0, j, endPts, p);
122 db = fprime(s, p, 1, 0, 0);
123 de = fprime(s, p, -1, in - 1, in - 1);
124 }
125 pts.getSubmatrixAsArray(j, 0, j, endPts, p);
126 srp[j] = new CubicSplineInterpolator(s, p, db, de);
127 }
128 } catch (final WrongSizeException e) {
129 throw new InterpolationException(e);
130 }
131 }
132
133 /**
134 * Constructor assuming that curve is NOT closed.
135 *
136 * @param ptsin matrix containing points on each row. Number of columns determines number of
137 * dimensions of each point.
138 * @throws InterpolationException if a numerical exception occurs.
139 */
140 public CurveInterpolator(final Matrix ptsin) throws InterpolationException {
141 this(ptsin, false);
142 }
143
144 /**
145 * Interpolates a point on the stored curve. The point is parameterized by t, in the range
146 * [0,1].
147 * For open curves, values of t outside this range will return extrapolations (dangerous!).
148 * For closed curves t is periodic with period 1.
149 *
150 * @param t position in the curve to be interpolated.
151 * @return result of interpolation.
152 * @throws InterpolationException if interpolation fails for numerical reasons.
153 */
154 public double[] interpolate(double t) throws InterpolationException {
155 if (cls) {
156 t = t - Math.floor(t);
157 }
158
159 for (var j = 0; j < dim; j++) {
160 ans[j] = srp[j].interpolate(t);
161 }
162 return ans;
163 }
164
165 /**
166 * Utility for estimating the derivatives at the endpoints, x and y point to the abscissa and
167 * ordinate of the endpoint. If pm is +1, points to the right will be used (left endpoint): if it
168 * is -1, points to the left will be used (right endpoint).
169 *
170 * @param x abscissa of endpoint.
171 * @param y ordinate of endpoint.
172 * @param pm +1 or -1.
173 * @param xoff offset of x.
174 * @param yoff offset of y.
175 */
176 private double fprime(final double[] x, final double[] y, final int pm, final int xoff, final int yoff) {
177 final var s1 = x[xoff] - x[xoff + pm];
178 final var s2 = x[xoff] - x[xoff + pm * 2];
179 final var s3 = x[xoff] - x[xoff + pm * 3];
180 final var s12 = s1 - s2;
181 final var s13 = s1 - s3;
182 final var s23 = s2 - s3;
183 return -(s1 * s2 / (s13 * s23 * s3)) * y[yoff + pm * 3] + (s1 * s3 / (s12 * s2 * s23)) * y[yoff + pm * 2]
184 - (s2 * s3 / (s1 * s12 * s13)) * y[yoff + pm] + (1.0 / s1 + 1.0 / s2 + 1.0 / s3) * y[yoff];
185 }
186
187 /**
188 * Computes the euclidean distance between two points.
189 *
190 * @param p1 first point.
191 * @param p2 second point.
192 * @return euclidean distance.
193 */
194 private double rad(final double[] p1, final double[] p2) {
195 var sum = 0.0;
196 for (var i = 0; i < dim; i++) {
197 final var value = p1[i] - p2[i];
198 sum += value * value;
199 }
200 return Math.sqrt(sum);
201 }
202 }