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 import java.util.Arrays;
20 
21 /**
22  * This provides provides a curve fit system that stitches the x,y path together with
23  * quarter ellipses
24  */
25 
26 public class ArcCurveFit extends CurveFit {
27     public static final int ARC_START_VERTICAL = 1;
28     public static final int ARC_START_HORIZONTAL = 2;
29     public static final int ARC_START_FLIP = 3;
30     public static final int ARC_BELOW = 4;
31     public static final int ARC_ABOVE = 5;
32 
33     public static final int ARC_START_LINEAR = 0;
34 
35     private static final int START_VERTICAL = 1;
36     private static final int START_HORIZONTAL = 2;
37     private static final int START_LINEAR = 3;
38     private static final int DOWN_ARC = 4;
39     private static final int UP_ARC = 5;
40 
41     private final double[] mTime;
42     Arc[] mArcs;
43     private boolean mExtrapolate = true;
44 
45     @Override
getPos(double t, double[] v)46     public void getPos(double t, double[] v) {
47         if (mExtrapolate) {
48             if (t < mArcs[0].mTime1) {
49                 double t0 = mArcs[0].mTime1;
50                 double dt = t - mArcs[0].mTime1;
51                 int p = 0;
52                 if (mArcs[p].mLinear) {
53                     v[0] = (mArcs[p].getLinearX(t0) + dt * mArcs[p].getLinearDX(t0));
54                     v[1] = (mArcs[p].getLinearY(t0) + dt * mArcs[p].getLinearDY(t0));
55                 } else {
56                     mArcs[p].setPoint(t0);
57                     v[0] = mArcs[p].getX() + dt * mArcs[p].getDX();
58                     v[1] = mArcs[p].getY() + dt * mArcs[p].getDY();
59                 }
60                 return;
61             }
62             if (t > mArcs[mArcs.length - 1].mTime2) {
63                 double t0 = mArcs[mArcs.length - 1].mTime2;
64                 double dt = t - t0;
65                 int p = mArcs.length - 1;
66                 if (mArcs[p].mLinear) {
67                     v[0] = (mArcs[p].getLinearX(t0) + dt * mArcs[p].getLinearDX(t0));
68                     v[1] = (mArcs[p].getLinearY(t0) + dt * mArcs[p].getLinearDY(t0));
69                 } else {
70                     mArcs[p].setPoint(t);
71                     v[0] = mArcs[p].getX() + dt * mArcs[p].getDX();
72                     v[1] = mArcs[p].getY() + dt * mArcs[p].getDY();
73                 }
74                 return;
75             }
76         } else {
77             if (t < mArcs[0].mTime1) {
78                 t = mArcs[0].mTime1;
79             }
80             if (t > mArcs[mArcs.length - 1].mTime2) {
81                 t = mArcs[mArcs.length - 1].mTime2;
82             }
83         }
84 
85         for (int i = 0; i < mArcs.length; i++) {
86             if (t <= mArcs[i].mTime2) {
87                 if (mArcs[i].mLinear) {
88                     v[0] = mArcs[i].getLinearX(t);
89                     v[1] = mArcs[i].getLinearY(t);
90                     return;
91                 }
92                 mArcs[i].setPoint(t);
93                 v[0] = mArcs[i].getX();
94                 v[1] = mArcs[i].getY();
95                 return;
96             }
97         }
98     }
99 
100     @Override
getPos(double t, float[] v)101     public void getPos(double t, float[] v) {
102         if (mExtrapolate) {
103             if (t < mArcs[0].mTime1) {
104                 double t0 = mArcs[0].mTime1;
105                 double dt = t - mArcs[0].mTime1;
106                 int p = 0;
107                 if (mArcs[p].mLinear) {
108                     v[0] = (float) (mArcs[p].getLinearX(t0) + dt * mArcs[p].getLinearDX(t0));
109                     v[1] = (float) (mArcs[p].getLinearY(t0) + dt * mArcs[p].getLinearDY(t0));
110                 } else {
111                     mArcs[p].setPoint(t0);
112                     v[0] = (float) (mArcs[p].getX() + dt * mArcs[p].getDX());
113                     v[1] = (float) (mArcs[p].getY() + dt * mArcs[p].getDY());
114                 }
115                 return;
116             }
117             if (t > mArcs[mArcs.length - 1].mTime2) {
118                 double t0 = mArcs[mArcs.length - 1].mTime2;
119                 double dt = t - t0;
120                 int p = mArcs.length - 1;
121                 if (mArcs[p].mLinear) {
122                     v[0] = (float) (mArcs[p].getLinearX(t0) + dt * mArcs[p].getLinearDX(t0));
123                     v[1] = (float) (mArcs[p].getLinearY(t0) + dt * mArcs[p].getLinearDY(t0));
124                 } else {
125                     mArcs[p].setPoint(t);
126                     v[0] = (float) mArcs[p].getX();
127                     v[1] = (float) mArcs[p].getY();
128                 }
129                 return;
130             }
131         } else {
132             if (t < mArcs[0].mTime1) {
133                 t = mArcs[0].mTime1;
134             } else if (t > mArcs[mArcs.length - 1].mTime2) {
135                 t = mArcs[mArcs.length - 1].mTime2;
136             }
137         }
138         for (int i = 0; i < mArcs.length; i++) {
139             if (t <= mArcs[i].mTime2) {
140                 if (mArcs[i].mLinear) {
141                     v[0] = (float) mArcs[i].getLinearX(t);
142                     v[1] = (float) mArcs[i].getLinearY(t);
143                     return;
144                 }
145                 mArcs[i].setPoint(t);
146                 v[0] = (float) mArcs[i].getX();
147                 v[1] = (float) mArcs[i].getY();
148                 return;
149             }
150         }
151     }
152 
153     @Override
getSlope(double t, double[] v)154     public void getSlope(double t, double[] v) {
155         if (t < mArcs[0].mTime1) {
156             t = mArcs[0].mTime1;
157         } else if (t > mArcs[mArcs.length - 1].mTime2) {
158             t = mArcs[mArcs.length - 1].mTime2;
159         }
160 
161         for (int i = 0; i < mArcs.length; i++) {
162             if (t <= mArcs[i].mTime2) {
163                 if (mArcs[i].mLinear) {
164                     v[0] = mArcs[i].getLinearDX(t);
165                     v[1] = mArcs[i].getLinearDY(t);
166                     return;
167                 }
168                 mArcs[i].setPoint(t);
169                 v[0] = mArcs[i].getDX();
170                 v[1] = mArcs[i].getDY();
171                 return;
172             }
173         }
174     }
175 
176     @Override
getPos(double t, int j)177     public double getPos(double t, int j) {
178         if (mExtrapolate) {
179             if (t < mArcs[0].mTime1) {
180                 double t0 = mArcs[0].mTime1;
181                 double dt = t - mArcs[0].mTime1;
182                 int p = 0;
183                 if (mArcs[p].mLinear) {
184                     if (j == 0) {
185                         return mArcs[p].getLinearX(t0) + dt * mArcs[p].getLinearDX(t0);
186                     }
187                     return mArcs[p].getLinearY(t0) + dt * mArcs[p].getLinearDY(t0);
188                 } else {
189                     mArcs[p].setPoint(t0);
190                     if (j == 0) {
191                         return mArcs[p].getX() + dt * mArcs[p].getDX();
192                     }
193                     return mArcs[p].getY() + dt * mArcs[p].getDY();
194                 }
195             }
196             if (t > mArcs[mArcs.length - 1].mTime2) {
197                 double t0 = mArcs[mArcs.length - 1].mTime2;
198                 double dt = t - t0;
199                 int p = mArcs.length - 1;
200                 if (j == 0) {
201                     return mArcs[p].getLinearX(t0) + dt * mArcs[p].getLinearDX(t0);
202                 }
203                 return mArcs[p].getLinearY(t0) + dt * mArcs[p].getLinearDY(t0);
204             }
205         } else {
206             if (t < mArcs[0].mTime1) {
207                 t = mArcs[0].mTime1;
208             } else if (t > mArcs[mArcs.length - 1].mTime2) {
209                 t = mArcs[mArcs.length - 1].mTime2;
210             }
211         }
212 
213         for (int i = 0; i < mArcs.length; i++) {
214             if (t <= mArcs[i].mTime2) {
215 
216                 if (mArcs[i].mLinear) {
217                     if (j == 0) {
218                         return mArcs[i].getLinearX(t);
219                     }
220                     return mArcs[i].getLinearY(t);
221                 }
222                 mArcs[i].setPoint(t);
223 
224                 if (j == 0) {
225                     return mArcs[i].getX();
226                 }
227                 return mArcs[i].getY();
228             }
229         }
230         return Double.NaN;
231     }
232 
233     @Override
getSlope(double t, int j)234     public double getSlope(double t, int j) {
235         if (t < mArcs[0].mTime1) {
236             t = mArcs[0].mTime1;
237         }
238         if (t > mArcs[mArcs.length - 1].mTime2) {
239             t = mArcs[mArcs.length - 1].mTime2;
240         }
241 
242         for (int i = 0; i < mArcs.length; i++) {
243             if (t <= mArcs[i].mTime2) {
244                 if (mArcs[i].mLinear) {
245                     if (j == 0) {
246                         return mArcs[i].getLinearDX(t);
247                     }
248                     return mArcs[i].getLinearDY(t);
249                 }
250                 mArcs[i].setPoint(t);
251                 if (j == 0) {
252                     return mArcs[i].getDX();
253                 }
254                 return mArcs[i].getDY();
255             }
256         }
257         return Double.NaN;
258     }
259 
260     @Override
getTimePoints()261     public double[] getTimePoints() {
262         return mTime;
263     }
264 
ArcCurveFit(int[] arcModes, double[] time, double[][] y)265     public ArcCurveFit(int[] arcModes, double[] time, double[][] y) {
266         mTime = time;
267         mArcs = new Arc[time.length - 1];
268         int mode = START_VERTICAL;
269         int last = START_VERTICAL;
270         for (int i = 0; i < mArcs.length; i++) {
271             switch (arcModes[i]) {
272                 case ARC_START_VERTICAL:
273                     last = mode = START_VERTICAL;
274                     break;
275                 case ARC_START_HORIZONTAL:
276                     last = mode = START_HORIZONTAL;
277                     break;
278                 case ARC_START_FLIP:
279                     mode = (last == START_VERTICAL) ? START_HORIZONTAL : START_VERTICAL;
280                     last = mode;
281                     break;
282                 case ARC_START_LINEAR:
283                     mode = START_LINEAR;
284                     break;
285                 case ARC_ABOVE:
286                     mode = UP_ARC;
287                     break;
288                 case ARC_BELOW:
289                     mode = DOWN_ARC;
290                     break;
291             }
292             mArcs[i] =
293                     new Arc(mode, time[i], time[i + 1], y[i][0], y[i][1], y[i + 1][0], y[i + 1][1]);
294         }
295     }
296 
297     private static class Arc {
298         private static final String TAG = "Arc";
299         private static double[] sOurPercent = new double[91];
300         double[] mLut;
301         double mArcDistance;
302         double mTime1;
303         double mTime2;
304         double mX1, mX2, mY1, mY2;
305         double mOneOverDeltaTime;
306         double mEllipseA;
307         double mEllipseB;
308         double mEllipseCenterX; // also used to cache the slope in the unused center
309         double mEllipseCenterY; // also used to cache the slope in the unused center
310         double mArcVelocity;
311         double mTmpSinAngle;
312         double mTmpCosAngle;
313         boolean mVertical;
314         boolean mLinear = false;
315         private static final double EPSILON = 0.001;
316 
Arc(int mode, double t1, double t2, double x1, double y1, double x2, double y2)317         Arc(int mode, double t1, double t2, double x1, double y1, double x2, double y2) {
318             double dx = x2 - x1;
319             double dy = y2 - y1;
320             switch (mode) {
321                 case START_VERTICAL:
322                     mVertical = true;
323                     break;
324                 case UP_ARC:
325                     mVertical = dy < 0;
326                     break;
327                 case DOWN_ARC:
328                     mVertical = dy > 0;
329                     break;
330                 default:
331                     mVertical = false;
332             }
333 
334             mTime1 = t1;
335             mTime2 = t2;
336             mOneOverDeltaTime = 1 / (mTime2 - mTime1);
337             if (START_LINEAR == mode) {
338                 mLinear = true;
339             }
340 
341             if (mLinear || Math.abs(dx) < EPSILON || Math.abs(dy) < EPSILON) {
342                 mLinear = true;
343                 mX1 = x1;
344                 mX2 = x2;
345                 mY1 = y1;
346                 mY2 = y2;
347                 mArcDistance = Math.hypot(dy, dx);
348                 mArcVelocity = mArcDistance * mOneOverDeltaTime;
349                 mEllipseCenterX = dx / (mTime2 - mTime1); // cache the slope in the unused center
350                 mEllipseCenterY = dy / (mTime2 - mTime1); // cache the slope in the unused center
351                 return;
352             }
353             mLut = new double[101];
354             mEllipseA = dx * (mVertical ? -1 : 1);
355             mEllipseB = dy * (mVertical ? 1 : -1);
356             mEllipseCenterX = mVertical ? x2 : x1;
357             mEllipseCenterY = mVertical ? y1 : y2;
358             buildTable(x1, y1, x2, y2);
359             mArcVelocity = mArcDistance * mOneOverDeltaTime;
360         }
361 
setPoint(double time)362         void setPoint(double time) {
363             double percent = (mVertical ? (mTime2 - time) : (time - mTime1)) * mOneOverDeltaTime;
364             double angle = Math.PI * 0.5 * lookup(percent);
365 
366             mTmpSinAngle = Math.sin(angle);
367             mTmpCosAngle = Math.cos(angle);
368         }
369 
getX()370         double getX() {
371             return mEllipseCenterX + mEllipseA * mTmpSinAngle;
372         }
373 
getY()374         double getY() {
375             return mEllipseCenterY + mEllipseB * mTmpCosAngle;
376         }
377 
getDX()378         double getDX() {
379             double vx = mEllipseA * mTmpCosAngle;
380             double vy = -mEllipseB * mTmpSinAngle;
381             double norm = mArcVelocity / Math.hypot(vx, vy);
382             return mVertical ? -vx * norm : vx * norm;
383         }
384 
getDY()385         double getDY() {
386             double vx = mEllipseA * mTmpCosAngle;
387             double vy = -mEllipseB * mTmpSinAngle;
388             double norm = mArcVelocity / Math.hypot(vx, vy);
389             return mVertical ? -vy * norm : vy * norm;
390         }
391 
getLinearX(double t)392         public double getLinearX(double t) {
393             t = (t - mTime1) * mOneOverDeltaTime;
394             return mX1 + t * (mX2 - mX1);
395         }
396 
getLinearY(double t)397         public double getLinearY(double t) {
398             t = (t - mTime1) * mOneOverDeltaTime;
399             return mY1 + t * (mY2 - mY1);
400         }
401 
402         @SuppressWarnings("UnusedVariable")
getLinearDX(double t)403         public double getLinearDX(double t) {
404             return mEllipseCenterX;
405         }
406 
407         @SuppressWarnings("UnusedVariable")
getLinearDY(double t)408         public double getLinearDY(double t) {
409             return mEllipseCenterY;
410         }
411 
lookup(double v)412         double lookup(double v) {
413             if (v <= 0) {
414                 return 0;
415             }
416             if (v >= 1) {
417                 return 1;
418             }
419             double pos = v * (mLut.length - 1);
420             int iv = (int) pos;
421             double off = pos - (int) pos;
422 
423             return mLut[iv] + (off * (mLut[iv + 1] - mLut[iv]));
424         }
425 
buildTable(double x1, double y1, double x2, double y2)426         private void buildTable(double x1, double y1, double x2, double y2) {
427             double a = x2 - x1;
428             double b = y1 - y2;
429             double lx = 0, ly = 0;
430             double dist = 0;
431             for (int i = 0; i < sOurPercent.length; i++) {
432                 double angle = Math.toRadians(90.0 * i / (sOurPercent.length - 1));
433                 double s = Math.sin(angle);
434                 double c = Math.cos(angle);
435                 double px = a * s;
436                 double py = b * c;
437                 if (i > 0) {
438                     dist += Math.hypot(px - lx, py - ly);
439                     sOurPercent[i] = dist;
440                 }
441                 lx = px;
442                 ly = py;
443             }
444 
445             mArcDistance = dist;
446 
447             for (int i = 0; i < sOurPercent.length; i++) {
448                 sOurPercent[i] /= dist;
449             }
450             for (int i = 0; i < mLut.length; i++) {
451                 double pos = i / (double) (mLut.length - 1);
452                 int index = Arrays.binarySearch(sOurPercent, pos);
453                 if (index >= 0) {
454                     mLut[i] = index / (double) (sOurPercent.length - 1);
455                 } else if (index == -1) {
456                     mLut[i] = 0;
457                 } else {
458                     int p1 = -index - 2;
459                     int p2 = -index - 1;
460 
461                     double ans = (p1 + (pos - sOurPercent[p1])
462                             / (sOurPercent[p2] - sOurPercent[p1])) / (sOurPercent.length - 1);
463                     mLut[i] = ans;
464                 }
465             }
466         }
467     }
468 }
469