# axis_angle.py
# copyright 2022- https://robot-creation-diary.com/
import numpy as np
import quaternion
import array
class AxisAngle():
def __init__(self, arg1=None, arg2=None):
self.__deg_to_rad_param = np.pi / 180.0
if isinstance(arg1, type(None)):
self.__quaternion = np.quaternion(1.0, 0, 0, 0)
elif isinstance(arg1, list):
if isinstance(arg2, type(None)):
self.__quaternion = quaternion.as_quat_array(
np.array(arg1, np.float64))
else:
self.set_axis_angle(arg1, arg2)
elif isinstance(arg1, np.ndarray):
if isinstance(arg2, type(None)):
self.__quaternion = quaternion.as_quat_array(
np.array(arg1, np.float64))
else:
self.set_axis_angle(arg1, arg2)
elif isinstance(arg1, quaternion.quaternion):
self.__quaternion = arg1
elif isinstance(arg1, array.array):
self.__quaternion = quaternion.as_quat_array(
np.array(arg1, np.float64))
else:
self.__quaternion = arg1.__quaternion
def set_quaternion(self, quat):
"""
クォータニオンを設定します。
"""
self.__quaternion = quat
def get_quaternion(self):
"""
クォータニオンを取得します。
"""
return self.__quaternion
def set_axis_angle(self, axis, rad):
"""
回転軸と角度で設定します。
"""
axis = axis / np.sqrt(np.dot(axis, axis))
self.__quaternion = np.quaternion(
np.cos(rad * 0.5),
np.float64(axis[0]) * np.sin(rad * 0.5),
np.float64(axis[1]) * np.sin(rad * 0.5),
np.float64(axis[2]) * np.sin(rad * 0.5))
self.__quaternion = self.__quaternion.normalized()
def set_axis_angle_deg(self, axis, deg):
"""
回転角と角度（deg）で設定します。
"""
self.set_axis_angle(axis, self.__deg_to_rad_param * deg)
def get_rotate_quaternion(self, quat):
"""
指定のクォータニオンで開店後のクォータニオンを得ます
"""
return (self.__quaternion * quat)
def rotate_axis_angle(self, axis_angle):
"""
指定の軸角度で回転させます。
"""
return self.__quaternion * axis_angle.__quaternion
def get_rotate_vector(self, vector):
"""
指定のベクトルを自身のデータで回転させます。
"""
return quaternion.rotate_vectors(self.__quaternion, vector)
def inverse_axis(self):
"""
軸の向きを反転させます。
"""
self.__quaternion[1] *= -1
self.__quaternion[2] *= -1
self.__quaternion[3] *= -1
def get_angle(self):
return self.__quaternion.angle()
def get_axis(self):
"""
回転軸を取得します。
"""
return quaternion.as_rotation_vector(self.__quaternion)
def get_axis_list(self):
"""
回転軸をpythonのlistで取得します。
"""
return self.get_axis().tolist()
def nomalize(self):
"""
クォータニオンをノーマライズします
"""
self.__quaternion = self.__quaternion.nomalized()
def get_nomalized_quaternion(self):
"""
ノーマライズされたクォータニオンを取得します。
"""
return self.__quaternion.nomalized()
def get_add_quaternion(self, axis_angle):
"""
指定の軸角度で回転させた軸角度を得ます。
"""
return axis_angle.get_rotate_quaternion(self.__quaternion)
def __add__(self, axis_angle):
"""
指定の軸角度で回転させた軸角度を得ます。
"""
self.result = AxisAngle(
axis_angle.get_rotate_quaternion(self.__quaternion))
return self.result
def __iadd__(self, axis_angle):
"""
指定の軸角度で回転させます。
"""
# axis_angleによって自分が回転させられる
self.__quaternion = axis_angle.get_rotate_quaternion(self.__quaternion)
return self
def get_sub_quaternion(self, axis_angle):
"""
指定の軸角度で逆回転させた軸角度を得ます。
"""
return self.__quaternion * axis_angle.__quaternion.inverse()
def __sub__(self, axis_angle):
"""
指定の軸角度で逆回転させた軸角度を得ます。
"""
self.result = AxisAngle(self.get_sub_quaternion(axis_angle))
return self.result
def __isub__(self, axis_angle):
"""
指定の軸角度で逆回転させます。
"""
self.__quaternion = self.__quaternion \
* axis_angle.__quaternion.inverse()
return self
def __str__(self):
"""
軸角度をテキスト出力します。printで指定した時様です。
"""
return (str(self.get_axis()) + "," +
str(self.get_angle() / self.__deg_to_rad_param))
if __name__ == '__main__':
aa = AxisAngle()
