diff --git a/src/KeyFrame.cpp b/src/KeyFrame.cpp index 6b186d376..68ea1ef1a 100644 --- a/src/KeyFrame.cpp +++ b/src/KeyFrame.cpp @@ -39,6 +39,44 @@ namespace { bool IsPointBeforeX(Point const & p, double const x) { return p.co.X < x; } + + double InterpolateBezierCurve(Point const & left, Point const & right, bool const needY, double const target, double const allowed_error) { + double const X_diff = right.co.X - left.co.X; + double const Y_diff = right.co.Y - left.co.Y; + Coordinate const p0 = left.co; + Coordinate const p1 = Coordinate(p0.X + left.handle_right.X * X_diff, p0.Y + left.handle_right.Y * Y_diff); + Coordinate const p2 = Coordinate(p0.X + right.handle_left.X * X_diff, p0.Y + right.handle_left.Y * Y_diff); + Coordinate const p3 = right.co; + + double t = 0.5; + double t_step = 0.25; + do { + // Bernstein polynoms + double B[4] = {1, 3, 3, 1}; + double oneMinTExp = 1; + double tExp = 1; + for (int i = 0; i < 4; ++i, tExp *= t) { + B[i] *= tExp; + } + for (int i = 0; i < 4; ++i, oneMinTExp *= 1 - t) { + B[4 - i - 1] *= oneMinTExp; + } + double const x = p0.X * B[0] + p1.X * B[1] + p2.X * B[2] + p3.X * B[3]; + double const y = p0.Y * B[0] + p1.Y * B[1] + p2.Y * B[2] + p3.Y * B[3]; + bool const stop = abs(target - (needY ? x : y)) < allowed_error; + bool const move_left = (needY ? x : y) > target; + if (stop) { + return needY ? y : x; + } + if (move_left) { + t -= t_step; + } + else { + t += t_step; + } + t_step /= 2; + } while (true); + } } @@ -231,39 +269,7 @@ double Keyframe::GetValue(int64_t index) const { // BEZIER curve! assert(candidate->interpolation == BEZIER); - double const X_diff = candidate->co.X - predecessor->co.X; - double const Y_diff = candidate->co.Y - predecessor->co.Y; - Coordinate const p0 = predecessor->co; - Coordinate const p1 = Coordinate(p0.X + predecessor->handle_right.X * X_diff, p0.Y + predecessor->handle_right.Y * Y_diff); - Coordinate const p2 = Coordinate(p0.X + candidate->handle_left.X * X_diff, p0.Y + candidate->handle_left.Y * Y_diff); - Coordinate const p3 = candidate->co; - - double t = 0.5; - double t_step = 0.25; - do { - // Bernstein polynoms - double B[4] = {1, 3, 3, 1}; - double oneMinTExp = 1; - double tExp = 1; - for (int i = 0; i < 4; ++i, tExp *= t) { - B[i] *= tExp; - } - for (int i = 0; i < 4; ++i, oneMinTExp *= 1 - t) { - B[4 - i - 1] *= oneMinTExp; - } - double const x = p0.X * B[0] + p1.X * B[1] + p2.X * B[2] + p3.X * B[3]; - double const y = p0.Y * B[0] + p1.Y * B[1] + p2.Y * B[2] + p3.Y * B[3]; - if (abs(index - x) < 0.01) { - return y; - } - if (x > index) { - t -= t_step; - } - else { - t += t_step; - } - t_step /= 2; - } while (true); + return InterpolateBezierCurve(*predecessor, *candidate, true, index, 0.01); } // Get the rounded INT value at a specific index