ベクトル計算



```python

'''
クラスにすることでコードの再利用性、拡張性、構造化が向上します。以下に、あなたのコードをクラスとして構造化した例を示します。
このクラスでは、各関数は @staticmethod デコレータを用いて静的メソッドとして定義されています。これにより、クラスのインスタンス化なしにメソッドを呼び出すことができます。
'''

import numpy as np
from scipy.linalg import lstsq

class VectorCalc:
    """さまざまなベクトル計算を行うためのクラス。"""

    def __init__(self):
        pass

    @staticmethod
    def vector_length(vector):
        """ベクトルの長さ（ノルム）を計算する。

        Args:
            vector (numpy.ndarray): 長さを計算する対象のベクトル。

        Returns:
            float: ベクトルの長さ。
        """
        norm = np.linalg.norm(vector)
        return norm

    @staticmethod
    def dot_product(vector1, vector2):
        """2つのベクトルの内積を計算する。

        Args:
            vector1 (numpy.ndarray): 1つ目のベクトル。
            vector2 (numpy.ndarray): 2つ目のベクトル。

        Returns:
            float: ベクトルの内積。
        """
        return np.dot(vector1, vector2)

    @staticmethod
    def cross_product(vector1, vector2):
        """2つのベクトルの外積を計算する。

        Args:
            vector1 (numpy.ndarray): 1つ目のベクトル。
            vector2 (numpy.ndarray): 2つ目のベクトル。

        Returns:
            numpy.ndarray: ベクトルの外積。
        """
        return np.cross(vector1, vector2)

    @staticmethod
    def angle_between_vectors(vector1, vector2):
        """2つのベクトルの成す角度を度単位で計算する。

        Args:
            vector1 (numpy.ndarray): 1つ目のベクトル。
            vector2 (numpy.ndarray): 2つ目のベクトル。

        Returns:
            float: 2つのベクトルの成す角度（度）。
        """
        dot_product = np.dot(vector1, vector2)
        norm2 = np.linalg.norm(vector2)
        norm1 = np.linalg.norm(vector1)
        cos_theta = dot_product / (norm1 * norm2)
        theta = np.arccos(np.clip(cos_theta, -1.0, 1.0))
        return np.degrees(theta)

    @staticmethod
    def distance_to_plane(plane_points, point):
        """点から平面までの距離と垂線が交わる点を計算する。

        Args:
            plane_points (list): 平面を定義する3つの点のリスト。
            point (numpy.ndarray): 距離を計算する対象の点。

        Returns:
            tuple: 平面までの距離と垂線が交わる点を含むタプル。
        """
        p1, p2, p3 = plane_points
        v1 = np.array(p2) - np.array(p1)
        v2 = np.array(p3) - np.array(p1)
        normal_vector = np.cross(v1, v2)
        norm_n = np.linalg.norm(normal_vector)
        distance = np.abs(np.dot(normal_vector, np.array(point) - np.array(p1))) / norm_n
        perpendicular_vector = np.array(point) - np.array(p1)
        dot_product = np.dot(normal_vector, perpendicular_vector)
        q = np.array(point) - (dot_product / norm_n**2) * normal_vector
        return distance, q

    @staticmethod
    def cross_product_vector(points):
        """与えられた3つの点から外積を計算し、法線ベクトルを得る。

        Args:
            points (list): 3つの点を含むリスト。

        Returns:
            numpy.ndarray: 外積によって得られた法線ベクトル。
        """
        vec1 = np.array(points[1]) - np.array(points[0])
        vec2 = np.array(points[2]) - np.array(points[0])
        normal_vector = np.cross(vec1, vec2)
        return normal_vector

    @staticmethod
    def fit_plane_normal_vector(points):
        """複数の点に対して最小二乗法を用いて平面をフィットし、その法線ベクトルを返す。

        Args:
            points (numpy.ndarray): 各点が[x, y, z]の配列である点の配列。

        Returns:
            numpy.ndarray: フィットした平面の法線ベクトル。
        """
        num_points = len(points)
        A = np.c_[[p[:2] for p in points], np.ones(num_points)]
        b = np.array([p[2] for p in points])
        coeffs, _, _, _ = lstsq(A, b)
        normal_vector = coeffs[:2]
        normal_vector = np.append(normal_vector, -1)
        return normal_vector

    @staticmethod
    def rodrigues_rotation_matrix(theta, rot_axis):
        """ロドリゲスの回転公式を使用して、指定された軸と角度での回転行列を返します。

        Args:
            rot_axis (array_like): 回転軸を表すベクトル。
            theta (float): 回転角（ラジアン）。

        Returns:
            numpy.ndarray: 回転行列。
        """
        axis = np.asarray(rot_axis)
        axis = axis / np.linalg.norm(axis)
        cos = np.cos(theta)
        sin = np.sin(theta)
        n1, n2, n3 = axis

        R = np.array([[n1**2 * (1 - cos) + cos, n1 * n2 * (1 - cos) - n3 * sin, n1 * n3 * (1 - cos) + n2 * sin],
                      [n1 * n2 * (1 - cos) + n3 * sin, n2**2 * (1 - cos) + cos, n2 * n3 * (1 - cos) - n1 * sin],
                      [n1 * n3 * (1 - cos) - n2 * sin, n2 * n3 * (1 - cos) + n1 * sin, n3**2 * (1 - cos) + cos]])
        return R

    @staticmethod
    def rotation_matrix_to_euler_xyz(R):
        """XYZオイラー角を使用して、回転行列からオイラー角を計算します。

        Args:
            R (numpy.ndarray): 3x3の回転行列。

        Returns:
            tuple: XYZオイラー角（ラジアン）。
        """
        roll = np.arctan2(R[2, 1], R[2, 2])
        pitch = -np.arcsin(R[2, 0])
        yaw = np.arctan2(R[1, 0], R[0, 0])
        return roll, pitch, yaw


```


```python
import numpy as np

# クラスをインスタンス化
vector_calc = VectorCalc()

# ベクトルの長さを計算
vector = np.array([3, 4, 0])
length = vector_calc.vector_length(vector)
print(f"ベクトルの長さ: {length}")

# 2つのベクトルの成す角度を計算
vector1 = np.array([1, 0, 0])
vector2 = np.array([0, 1, 0])
angle = vector_calc.angle_between_vectors(vector1, vector2)
print(f"2つのベクトルの成す角度: {angle} 度")

# 平面へのフィットと法線ベクトルを計算
points = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 1]])
normal_vector = vector_calc.fit_plane_normal_vector(points)
print(f"フィットした平面の法線ベクトル: {normal_vector}")

# 点から平面までの距離を計算
plane_points = [np.array([0, 0, 0]), np.array([1, 0, 0]), np.array([0, 1, 0])]
point = np.array([0, 0, 1])
distance, intersect_point = vector_calc.distance_to_plane(plane_points, point)
print(f"点から平面までの距離: {distance}")
print(f"平面との交点: {intersect_point}")

# 3つの点から外積を計算し、法線ベクトルを得る
cross_product_normal = vector_calc.cross_product_vector(plane_points)
print(f"外積による法線ベクトル: {cross_product_normal}")


vec1 = np.array([1, 2, 3])
vec2 = np.array([4, 5, 6])
# 内積
dot = VectorCalc.dot_product(vec1, vec2)
print(f"内積: {dot}")

# 外積
cross = VectorCalc.cross_product(vec1, vec2)
print(f"外積: {cross}")



# 回転軸と角度
axis = [8.2, 2.9, 2.1]  # 回転軸
theta = 0.5  # 回転角

# ロドリゲスの回転行列を計算
R = VectorCalc.rodrigues_rotation_matrix(theta, axis)
print("ロドリゲスの回転行列:")
print(R)

# 回転行列からXYZオイラー角を計算
roll, pitch, yaw = VectorCalc.rotation_matrix_to_euler_xyz(R)
print("XYZオイラー角:")
print(f"Roll: {roll} rad")
print(f"Pitch: {pitch} rad")
print(f"Yaw: {yaw} rad")



```


```python

```