[iregeo.l :difference-rotation] use let, and fix 0 norm condition

euslisp · May 25, 2020 · 815f8b4 · 815f8b4
1 parent 85c060d
commit 815f8b4
Showing 1 changed file with 14 additions and 11 deletions.
diff --git a/irteus/irtgeo.l b/irteus/irtgeo.l
@@ -89,18 +89,21 @@
               (transform (transpose (send self :worldrot))
                          (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1))))))
        ((:xy :yx :yz :zy :zx :xz)
-        (setq a0 (send self :axis (case rotation-axis ((:xy :yx) :z) ((:yz :zy) :x) ((:zx :xz) :y)))
-              a1 (send coords :axis (case rotation-axis ((:xy :yx) :z) ((:yz :zy) :x) ((:zx :xz) :y))))
-        (setq dif-rot
-              (transform (transpose (send self :worldrot))
-                         (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1)))))
-        (let ((self-coords (send (send self :copy-worldcoords) :rotate (norm dif-rot) dif-rot)))
-          (setq a0 (send self-coords :axis (case rotation-axis ((:xy :yx) :x) ((:yz :zy) :y) ((:zx :xz) :z)))
-                a1 (send coords :axis (case rotation-axis ((:xy :yx) :x) ((:yz :zy) :y) ((:zx :xz) :z))))
+        (let ((ax1 (case rotation-axis ((:xy :yx) :z) ((:yz :zy) :x) ((:zx :xz) :y)))
+              (ax2 (case rotation-axis ((:xy :yx) :x) ((:yz :zy) :y) ((:zx :xz) :z))))
+          (setq a0 (send self :axis ax1)
+                a1 (send coords :axis ax1))
           (setq dif-rot
-                (transform (transpose (send self-coords :worldrot))
-                           (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1))))))
-        )
+                (transform (transpose (send self :worldrot))
+                           (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1)))))
+          (let ((self-coords (if (= 0.0 (norm dif-rot))
+                                 self
+                                 (send (send self :copy-worldcoords) :rotate (norm dif-rot) (normalize-vector dif-rot)))))
+            (setq a0 (send self-coords :axis ax2)
+                  a1 (send coords :axis ax2))
+            (setq dif-rot
+                  (transform (transpose (send self-coords :worldrot))
+                             (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1))))))))
        ((:xx :yy :zz)
         (let ((axis (case rotation-axis (:xx :x) (:yy :y) (:zz :z))) a0 a2)
           (setq a0 (send self :axis axis))