1 /*
2  * Copyright (C) 2020 The Android Open Source Project
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 
17 package androidx.constraintlayout.core.motion.utils;
18 
19 /**
20  * This performs a simple linear interpolation in multiple dimensions
21  *
22  *
23  */
24 public class LinearCurveFit extends CurveFit {
25     private static final String TAG = "LinearCurveFit";
26     private double[] mT;
27     private double[][] mY;
28     private double mTotalLength = Double.NaN;
29     private boolean mExtrapolate = true;
30     double[] mSlopeTemp;
31 
LinearCurveFit(double[] time, double[][] y)32     public LinearCurveFit(double[] time, double[][] y) {
33         final int dim = y[0].length;
34         mSlopeTemp = new double[dim];
35         mT = time;
36         mY = y;
37         if (dim > 2) {
38             @SuppressWarnings("unused") double sum = 0;
39             double lastx = 0, lasty = 0;
40             for (int i = 0; i < time.length; i++) {
41                 double px = y[i][0];
42                 double py = y[i][0];
43                 if (i > 0) {
44                     sum += Math.hypot(px - lastx, py - lasty);
45                 }
46                 lastx = px;
47                 lasty = py;
48             }
49             mTotalLength = 0;
50         }
51     }
52 
53     /**
54      * Calculate the length traveled by the first two parameters assuming they are x and y.
55      * (Added for future work)
56      *
57      * @param t the point to calculate the length to
58      */
59     @SuppressWarnings("unused")
getLength2D(double t)60     private double getLength2D(double t) {
61         if (Double.isNaN(mTotalLength)) {
62             return 0;
63         }
64         final int n = mT.length;
65         if (t <= mT[0]) {
66             return 0;
67         }
68         if (t >= mT[n - 1]) {
69             return mTotalLength;
70         }
71         double sum = 0;
72         double last_x = 0, last_y = 0;
73 
74         for (int i = 0; i < n - 1; i++) {
75             double px = mY[i][0];
76             double py = mY[i][1];
77             if (i > 0) {
78                 sum += Math.hypot(px - last_x, py - last_y);
79             }
80             last_x = px;
81             last_y = py;
82             if (t == mT[i]) {
83                 return sum;
84             }
85             if (t < mT[i + 1]) {
86                 double h = mT[i + 1] - mT[i];
87                 double x = (t - mT[i]) / h;
88                 double x1 = mY[i][0];
89                 double x2 = mY[i + 1][0];
90                 double y1 = mY[i][1];
91                 double y2 = mY[i + 1][1];
92 
93                 py -= y1 * (1 - x) + y2 * x;
94                 px -= x1 * (1 - x) + x2 * x;
95                 sum += Math.hypot(py, px);
96 
97                 return sum;
98             }
99         }
100         return 0;
101     }
102 
103     // @TODO: add description
104     @Override
getPos(double t, double[] v)105     public void getPos(double t, double[] v) {
106         final int n = mT.length;
107         final int dim = mY[0].length;
108         if (mExtrapolate) {
109             if (t <= mT[0]) {
110                 getSlope(mT[0], mSlopeTemp);
111                 for (int j = 0; j < dim; j++) {
112                     v[j] = mY[0][j] + (t - mT[0]) * mSlopeTemp[j];
113                 }
114                 return;
115             }
116             if (t >= mT[n - 1]) {
117                 getSlope(mT[n - 1], mSlopeTemp);
118                 for (int j = 0; j < dim; j++) {
119                     v[j] = mY[n - 1][j] + (t - mT[n - 1]) * mSlopeTemp[j];
120                 }
121                 return;
122             }
123         } else {
124             if (t <= mT[0]) {
125                 for (int j = 0; j < dim; j++) {
126                     v[j] = mY[0][j];
127                 }
128                 return;
129             }
130             if (t >= mT[n - 1]) {
131                 for (int j = 0; j < dim; j++) {
132                     v[j] = mY[n - 1][j];
133                 }
134                 return;
135             }
136         }
137 
138         for (int i = 0; i < n - 1; i++) {
139             if (t == mT[i]) {
140                 for (int j = 0; j < dim; j++) {
141                     v[j] = mY[i][j];
142                 }
143             }
144             if (t < mT[i + 1]) {
145                 double h = mT[i + 1] - mT[i];
146                 double x = (t - mT[i]) / h;
147                 for (int j = 0; j < dim; j++) {
148                     double y1 = mY[i][j];
149                     double y2 = mY[i + 1][j];
150 
151                     v[j] = y1 * (1 - x) + y2 * x;
152                 }
153                 return;
154             }
155         }
156     }
157 
158     // @TODO: add description
159     @Override
getPos(double t, float[] v)160     public void getPos(double t, float[] v) {
161         final int n = mT.length;
162         final int dim = mY[0].length;
163         if (mExtrapolate) {
164             if (t <= mT[0]) {
165                 getSlope(mT[0], mSlopeTemp);
166                 for (int j = 0; j < dim; j++) {
167                     v[j] = (float) (mY[0][j] + (t - mT[0]) * mSlopeTemp[j]);
168                 }
169                 return;
170             }
171             if (t >= mT[n - 1]) {
172                 getSlope(mT[n - 1], mSlopeTemp);
173                 for (int j = 0; j < dim; j++) {
174                     v[j] = (float) (mY[n - 1][j] + (t - mT[n - 1]) * mSlopeTemp[j]);
175                 }
176                 return;
177             }
178         } else {
179             if (t <= mT[0]) {
180                 for (int j = 0; j < dim; j++) {
181                     v[j] = (float) mY[0][j];
182                 }
183                 return;
184             }
185             if (t >= mT[n - 1]) {
186                 for (int j = 0; j < dim; j++) {
187                     v[j] = (float) mY[n - 1][j];
188                 }
189                 return;
190             }
191         }
192 
193         for (int i = 0; i < n - 1; i++) {
194             if (t == mT[i]) {
195                 for (int j = 0; j < dim; j++) {
196                     v[j] = (float) mY[i][j];
197                 }
198             }
199             if (t < mT[i + 1]) {
200                 double h = mT[i + 1] - mT[i];
201                 double x = (t - mT[i]) / h;
202                 for (int j = 0; j < dim; j++) {
203                     double y1 = mY[i][j];
204                     double y2 = mY[i + 1][j];
205 
206                     v[j] = (float) (y1 * (1 - x) + y2 * x);
207                 }
208                 return;
209             }
210         }
211     }
212 
213     // @TODO: add description
214     @Override
getPos(double t, int j)215     public double getPos(double t, int j) {
216         final int n = mT.length;
217         if (mExtrapolate) {
218             if (t <= mT[0]) {
219                 return mY[0][j] + (t - mT[0]) * getSlope(mT[0], j);
220             }
221             if (t >= mT[n - 1]) {
222                 return mY[n - 1][j] + (t - mT[n - 1]) * getSlope(mT[n - 1], j);
223             }
224         } else {
225             if (t <= mT[0]) {
226                 return mY[0][j];
227             }
228             if (t >= mT[n - 1]) {
229                 return mY[n - 1][j];
230             }
231         }
232 
233         for (int i = 0; i < n - 1; i++) {
234             if (t == mT[i]) {
235                 return mY[i][j];
236             }
237             if (t < mT[i + 1]) {
238                 double h = mT[i + 1] - mT[i];
239                 double x = (t - mT[i]) / h;
240                 double y1 = mY[i][j];
241                 double y2 = mY[i + 1][j];
242                 return (y1 * (1 - x) + y2 * x);
243 
244             }
245         }
246         return 0; // should never reach here
247     }
248 
249     // @TODO: add description
250     @Override
getSlope(double t, double[] v)251     public void getSlope(double t, double[] v) {
252         final int n = mT.length;
253         int dim = mY[0].length;
254         if (t <= mT[0]) {
255             t = mT[0];
256         } else if (t >= mT[n - 1]) {
257             t = mT[n - 1];
258         }
259 
260         for (int i = 0; i < n - 1; i++) {
261             if (t <= mT[i + 1]) {
262                 double h = mT[i + 1] - mT[i];
263                 for (int j = 0; j < dim; j++) {
264                     double y1 = mY[i][j];
265                     double y2 = mY[i + 1][j];
266 
267                     v[j] = (y2 - y1) / h;
268                 }
269                 break;
270             }
271         }
272     }
273 
274     // @TODO: add description
275     @Override
getSlope(double t, int j)276     public double getSlope(double t, int j) {
277         final int n = mT.length;
278 
279         if (t < mT[0]) {
280             t = mT[0];
281         } else if (t >= mT[n - 1]) {
282             t = mT[n - 1];
283         }
284         for (int i = 0; i < n - 1; i++) {
285             if (t <= mT[i + 1]) {
286                 double h = mT[i + 1] - mT[i];
287                 double y1 = mY[i][j];
288                 double y2 = mY[i + 1][j];
289                 return (y2 - y1) / h;
290             }
291         }
292         return 0; // should never reach here
293     }
294 
295     @Override
getTimePoints()296     public double[] getTimePoints() {
297         return mT;
298     }
299 }
300