src/vector/vinterpolator.cpp

   1 /*
   2  * Copyright (c) 2018 Samsung Electronics Co., Ltd. All rights reserved.
   3  *
   4  * Licensed under the Flora License, Version 1.1 (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://floralicense.org/license/
   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 #include "vinterpolator.h"
  18 #include <cmath>
  19
  20 V_BEGIN_NAMESPACE
  21
  22 #define NEWTON_ITERATIONS 4
  23 #define NEWTON_MIN_SLOPE 0.02
  24 #define SUBDIVISION_PRECISION 0.0000001
  25 #define SUBDIVISION_MAX_ITERATIONS 10
  26
  27 const float VInterpolator::kSampleStepSize =
  28     1.0 / float(VInterpolator::kSplineTableSize - 1);
  29
  30 void VInterpolator::init(float aX1, float aY1, float aX2, float aY2)
  31 {
  32     mX1 = aX1;
  33     mY1 = aY1;
  34     mX2 = aX2;
  35     mY2 = aY2;
  36
  37     if (mX1 != mY1 || mX2 != mY2) CalcSampleValues();
  38 }
  39
  40 /*static*/ float VInterpolator::CalcBezier(float aT, float aA1, float aA2)
  41 {
  42     // use Horner's scheme to evaluate the Bezier polynomial
  43     return ((A(aA1, aA2) * aT + B(aA1, aA2)) * aT + C(aA1)) * aT;
  44 }
  45
  46 void VInterpolator::CalcSampleValues()
  47 {
  48     for (int i = 0; i < kSplineTableSize; ++i) {
  49         mSampleValues[i] = CalcBezier(float(i) * kSampleStepSize, mX1, mX2);
  50     }
  51 }
  52
  53 float VInterpolator::GetSlope(float aT, float aA1, float aA2)
  54 {
  55     return 3.0 * A(aA1, aA2) * aT * aT + 2.0 * B(aA1, aA2) * aT + C(aA1);
  56 }
  57
  58 float VInterpolator::value(float aX) const
  59 {
  60     if (mX1 == mY1 && mX2 == mY2) return aX;
  61
  62     return CalcBezier(GetTForX(aX), mY1, mY2);
  63 }
  64
  65 float VInterpolator::GetTForX(float aX) const
  66 {
  67     // Find interval where t lies
  68     float              intervalStart = 0.0;
  69     const float*       currentSample = &mSampleValues[1];
  70     const float* const lastSample = &mSampleValues[kSplineTableSize - 1];
  71     for (; currentSample != lastSample && *currentSample <= aX;
  72          ++currentSample) {
  73         intervalStart += kSampleStepSize;
  74     }
  75     --currentSample;  // t now lies between *currentSample and *currentSample+1
  76
  77     // Interpolate to provide an initial guess for t
  78     float dist =
  79         (aX - *currentSample) / (*(currentSample + 1) - *currentSample);
  80     float guessForT = intervalStart + dist * kSampleStepSize;
  81
  82     // Check the slope to see what strategy to use. If the slope is too small
  83     // Newton-Raphson iteration won't converge on a root so we use bisection
  84     // instead.
  85     float initialSlope = GetSlope(guessForT, mX1, mX2);
  86     if (initialSlope >= NEWTON_MIN_SLOPE) {
  87         return NewtonRaphsonIterate(aX, guessForT);
  88     } else if (initialSlope == 0.0) {
  89         return guessForT;
  90     } else {
  91         return BinarySubdivide(aX, intervalStart,
  92                                intervalStart + kSampleStepSize);
  93     }
  94 }
  95
  96 float VInterpolator::NewtonRaphsonIterate(float aX, float aGuessT) const
  97 {
  98     // Refine guess with Newton-Raphson iteration
  99     for (int i = 0; i < NEWTON_ITERATIONS; ++i) {
 100         // We're trying to find where f(t) = aX,
 101         // so we're actually looking for a root for: CalcBezier(t) - aX
 102         float currentX = CalcBezier(aGuessT, mX1, mX2) - aX;
 103         float currentSlope = GetSlope(aGuessT, mX1, mX2);
 104
 105         if (currentSlope == 0.0) return aGuessT;
 106
 107         aGuessT -= currentX / currentSlope;
 108     }
 109
 110     return aGuessT;
 111 }
 112
 113 float VInterpolator::BinarySubdivide(float aX, float aA, float aB) const
 114 {
 115     float currentX;
 116     float currentT;
 117     int   i = 0;
 118
 119     do {
 120         currentT = aA + (aB - aA) / 2.0;
 121         currentX = CalcBezier(currentT, mX1, mX2) - aX;
 122
 123         if (currentX > 0.0) {
 124             aB = currentT;
 125         } else {
 126             aA = currentT;
 127         }
 128     } while (fabs(currentX) > SUBDIVISION_PRECISION &&
 129              ++i < SUBDIVISION_MAX_ITERATIONS);
 130
 131     return currentT;
 132 }
 133
 134 V_END_NAMESPACE