def test_poly2_arc_length(self):
# Reference: WolframAlpha
# arclength of x(t) = 1 - 4*t + 0.1*t^2, y(t) = 2*t - 3*t^2 from t = 1.5 to t = 3
x_t = [1, -4, 0.1]
y_t = [0, 2, -3]
arclength = PolyArcLength.for_polys(x_t, y_t).value(1.5, 3)
str_arclength = '%.5f' % arclength
self.assertEquals(str_arclength, '18.10166')
def test_poly1_arc_length(self):
# Reference: WolframAlpha
# arclength of x(t) = 1 + 2*t, y(t) = 5 - 11.5*t from t = 1 to t = 2.5
x_t = [1, 2]
y_t = [5, -11.5]
arclength = PolyArcLength.for_polys(x_t, y_t).value(1, 2.5)
str_arclength = '%.4f' % arclength
self.assertEquals(str_arclength, '17.5089')
def _add_section(self, t0, t1, sec_x, sec_y):
# For every polynomial section of q_x and q_y whose start and end time
# match, this method expands the QSS polynomials into the form
# a_0 + a_1 * t + a_2 * t^2 and then computes the arc length of the
# resulting curve between the start time (t0) and the end time (t1).
# sec_x(t) = x_0 + x_1 * (t - t0) + x_2 * (t - t0)^2
# = x_0 + x_1 * t - x_1 * t0 + x_2 * [t^2 - 2*t*t0 + t0^2]
# = (x_0 - x_1 * t0 + x_2*t0^2) + (x_1 - 2*x_2*t0) * t + x_2 * t^2
t0sq = t0 * t0
x_0 = sec_x[0] - sec_x[1] * t0 + sec_x[2] * t0sq
x_1 = sec_x[1] - 2 * sec_x[2] * t0
x_2 = sec_x[2]
y_0 = sec_y[0] - sec_y[1] * t0 + sec_y[2] * t0sq
y_1 = sec_y[1] - 2 * sec_y[2] * t0
y_2 = sec_y[2]
x_t = [x_0, x_1, x_2]
y_t = [y_0, y_1, y_2]
return PolyArcLength.for_polys(x_t, y_t).value(t0, t1)
