def test_find_intersection(self):
# spline2 starts behind spline1, but ends in the same place.
spline1 = CubicSpline( # goes to position: 100, then stays forever
[
10, 13.333333333333334, 24.744343897926601, 85.255656102073402,
96.666666666666671, 100, 100
], [0, 5.0, 11.794494717703369, 11.794494717703369, 5.0, 0, 0],
[0, 5, 5, -5, -5, 0, 0], [2.5, 0, -2.5, 0, 2.5, 0], [
0, 2.0, 3.358898943540674, 7.358898943540674,
8.717797887081348, 10.717797887081348, inf
])
spline2 = CubicSpline(
[
0, 3.3333333333333335, 17.507576383774929, 82.492423616225054,
96.666666666666643, 100
], [0, 5.0, 12.912878474779198, 12.912878474779198, 5.0, 0],
[0, 5, 5, -5, -5, 0], [2.5, 0, -2.5, 0, 2.5], [
0, 2.0, 3.5825756949558398, 7.5825756949558398,
9.1651513899116797, 11.16515138991168
])
dist = (spline1.q[0] -
6) - spline2.q[0] # assume spline1's vehicle is 6 meters long
t = spline2.find_intersection(spline1, 0, spline2.t[-1], dist)
self.assertAlmostEqual(
spline1.evaluate(t).pos - 6,
spline2.evaluate(t).pos)
# Test that nothing is returned if past the point of intersection
dist = (spline1.evaluate(t + 0.1).pos - 6) - spline2.evaluate(t +
0.1).pos
t2 = spline2.find_intersection(spline1, t + 0.1, spline2.t[-1], dist)
self.assertEqual(t2, None)
def test_evaluate(self):
spline = CubicSpline(
[0, 1 / 6, 1], # pos
[0, 1 / 2, 1], # vel
[0, 1, 0], # accel
[1, -1], # jerk
[0, 1, 2]) # time
knot = spline.evaluate(0.5)
self.assertEquals(knot.pos, 1 / 48)
self.assertEquals(knot.vel, 0.125)
self.assertEquals(knot.accel, 0.5)
self.assertEquals(knot.time, 0.5)