summaryrefslogtreecommitdiff
path: root/ssg/src/util/sginterpolator.cpp
blob: c301aafa3fc12caabc95c69c029deef89e5adb7d (plain) (blame)
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
#include"sginterpolator.h"
#include<cmath>

#define NEWTON_ITERATIONS          4
#define NEWTON_MIN_SLOPE           0.02
#define SUBDIVISION_PRECISION      0.0000001
#define SUBDIVISION_MAX_ITERATIONS 10

const float SGInterpolator::kSampleStepSize =
                                        1.0 / float(SGInterpolator::kSplineTableSize - 1);

void
SGInterpolator::init(float aX1, float aY1, float aX2, float aY2)
{
    mX1 = aX1;
    mY1 = aY1;
    mX2 = aX2;
    mY2 = aY2;

    if (mX1 != mY1 || mX2 != mY2)
      CalcSampleValues();
}

/*static*/ float
SGInterpolator::CalcBezier(float aT,
                           float aA1,
                           float aA2)
{
  // use Horner's scheme to evaluate the Bezier polynomial
  return ((A(aA1, aA2)*aT + B(aA1, aA2))*aT + C(aA1))*aT;
}

void
SGInterpolator::CalcSampleValues()
{
    for (int i = 0; i < kSplineTableSize; ++i) {
        mSampleValues[i] = CalcBezier(float(i) * kSampleStepSize, mX1, mX2);
    }
}

float
SGInterpolator::GetSlope(float aT,
                         float aA1,
                         float aA2)
{
  return 3.0 * A(aA1, aA2)*aT*aT + 2.0 * B(aA1, aA2) * aT + C(aA1);
}


float
SGInterpolator::value(float aX) const
{
  if (mX1 == mY1 && mX2 == mY2)
    return aX;

  return CalcBezier(GetTForX(aX), mY1, mY2);
}

float
SGInterpolator::GetTForX(float aX) const
{
  // Find interval where t lies
  float intervalStart = 0.0;
  const float* currentSample = &mSampleValues[1];
  const float* const lastSample = &mSampleValues[kSplineTableSize - 1];
  for (; currentSample != lastSample && *currentSample <= aX;
        ++currentSample) {
    intervalStart += kSampleStepSize;
  }
  --currentSample; // t now lies between *currentSample and *currentSample+1

  // Interpolate to provide an initial guess for t
  float dist = (aX - *currentSample) /
                (*(currentSample+1) - *currentSample);
  float guessForT = intervalStart + dist * kSampleStepSize;

  // Check the slope to see what strategy to use. If the slope is too small
  // Newton-Raphson iteration won't converge on a root so we use bisection
  // instead.
  float initialSlope = GetSlope(guessForT, mX1, mX2);
  if (initialSlope >= NEWTON_MIN_SLOPE) {
    return NewtonRaphsonIterate(aX, guessForT);
  } else if (initialSlope == 0.0) {
    return guessForT;
  } else {
    return BinarySubdivide(aX, intervalStart, intervalStart + kSampleStepSize);
  }
}

float
SGInterpolator::NewtonRaphsonIterate(float aX, float aGuessT) const
{
  // Refine guess with Newton-Raphson iteration
  for (int i = 0; i < NEWTON_ITERATIONS; ++i) {
    // We're trying to find where f(t) = aX,
    // so we're actually looking for a root for: CalcBezier(t) - aX
    float currentX = CalcBezier(aGuessT, mX1, mX2) - aX;
    float currentSlope = GetSlope(aGuessT, mX1, mX2);

    if (currentSlope == 0.0)
      return aGuessT;

    aGuessT -= currentX / currentSlope;
  }

  return aGuessT;
}

float
SGInterpolator::BinarySubdivide(float aX, float aA, float aB) const
{
  float currentX;
  float currentT;
  int i = 0;

  do
  {
    currentT = aA + (aB - aA) / 2.0;
    currentX = CalcBezier(currentT, mX1, mX2) - aX;

    if (currentX > 0.0) {
      aB = currentT;
    } else {
      aA = currentT;
    }
  } while (fabs(currentX) > SUBDIVISION_PRECISION
           && ++i < SUBDIVISION_MAX_ITERATIONS);

  return currentT;
}