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)