From 13bfa4b51b88458cb00ad3015caebf2aa1bc4443 Mon Sep 17 00:00:00 2001 From: MasakiMurooka Date: Sun, 18 Feb 2018 14:35:46 +0900 Subject: [PATCH] [irteus/test/geo.l] add test-vector-angle. --- irteus/test/geo.l | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/irteus/test/geo.l b/irteus/test/geo.l index 5b8885f0d..aa03bf572 100644 --- a/irteus/test/geo.l +++ b/irteus/test/geo.l @@ -61,6 +61,22 @@ bottom-frame )) +(deftest test-vector-angle + (assert (and (eps= (vector-angle (float-vector 0 0 1) (float-vector 0 0 1)) 0.0) + (not (c-isnan (vector-angle (float-vector 0 0 1) (float-vector 0 0 1)))))) + (assert (and (eps= (vector-angle (float-vector 0 0 1) (float-vector 0 0 -1)) pi) + (not (c-isnan (vector-angle (float-vector 0 0 1) (float-vector 0 0 -1)))))) + (assert (eps= + (vector-angle (float-vector 0 0 1) + (send (make-coords :rpy (list 0 -0.2 0)) :rotate-vector (float-vector 0 0 1))) + 0.2)) + (assert (eps= + (vector-angle (float-vector 0 0 1) + (send (make-coords :rpy (list 0 -0.2 0)) :rotate-vector (float-vector 0 0 1)) + (float-vector 0 1 0)) + -0.2)) + ) + (run-all-tests) (exit)