Skip to content

Commit e83f30e

Browse files
committed
use existing angle wrap func
1 parent 6664937 commit e83f30e

File tree

1 file changed

+2
-6
lines changed

1 file changed

+2
-6
lines changed

spatialmath/base/transforms2d.py

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -920,9 +920,7 @@ def trinterp2(start, end, s, shortest: bool = True):
920920
th0 = math.atan2(start[1, 0], start[0, 0])
921921
th1 = math.atan2(end[1, 0], end[0, 0])
922922
if shortest:
923-
delta = (th1 - th0 + math.pi) % (math.pi * 2) - math.pi
924-
delta = delta + math.pi * 2 if delta < -math.pi else delta
925-
th1 = th0 + delta
923+
th1 = th0 + smb.wrap_mpi_pi(th1 - th0)
926924

927925
th = th0 * (1 - s) + s * th1
928926

@@ -944,9 +942,7 @@ def trinterp2(start, end, s, shortest: bool = True):
944942
th0 = math.atan2(start[1, 0], start[0, 0])
945943
th1 = math.atan2(end[1, 0], end[0, 0])
946944
if shortest:
947-
delta = (th1 - th0 + math.pi) % (math.pi * 2) - math.pi
948-
delta = delta + math.pi * 2 if delta < -math.pi else delta
949-
th1 = th0 + delta
945+
th1 = th0 + smb.wrap_mpi_pi(th1 - th0)
950946

951947
p0 = transl2(start)
952948
p1 = transl2(end)

0 commit comments

Comments
 (0)