diff --git a/irteus/irtgeo.l b/irteus/irtgeo.l index a2232b7c8..c43a28e11 100644 --- 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))