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
|
/* -*- Mode: C++; tab-width: 8; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
/* vim: set ts=8 sts=2 et sw=2 tw=80: */
/* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/. */
#include "vector_vinterpolator.h"
#include <cmath>
V_BEGIN_NAMESPACE
#define NEWTON_ITERATIONS 4
#define NEWTON_MIN_SLOPE 0.02
#define SUBDIVISION_PRECISION 0.0000001
#define SUBDIVISION_MAX_ITERATIONS 10
const float VInterpolator::kSampleStepSize =
1.0f / float(VInterpolator::kSplineTableSize - 1);
void VInterpolator::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 VInterpolator::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 VInterpolator::CalcSampleValues()
{
for (int i = 0; i < kSplineTableSize; ++i) {
mSampleValues[i] = CalcBezier(float(i) * kSampleStepSize, mX1, mX2);
}
}
float VInterpolator::GetSlope(float aT, float aA1, float aA2)
{
return 3.0f * A(aA1, aA2) * aT * aT + 2.0f * B(aA1, aA2) * aT + C(aA1);
}
float VInterpolator::value(float aX) const
{
if (mX1 == mY1 && mX2 == mY2) return aX;
return CalcBezier(GetTForX(aX), mY1, mY2);
}
float VInterpolator::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 VInterpolator::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 VInterpolator::BinarySubdivide(float aX, float aA, float aB) const
{
float currentX;
float currentT;
int i = 0;
do {
currentT = aA + (aB - aA) / 2.0f;
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;
}
V_END_NAMESPACE
|
