已知 空间 正交 基 用世界坐标 系 表示 ，和在 正交 基下的 相对 坐标p ， 求在 base 坐标系下 的坐标 给出 python 代码

# -*- coding: utf-8 -*-
"""
Created on Sat Nov 15 20:56:35 2025

@author: luogan
"""

import numpy as np

def relative_to_world_coordinates(base_vectors, relative_coords):
    """
    将相对坐标转换为世界坐标系下的坐标
    
    参数:
    base_vectors: list of arrays, 正交基向量 [v1, v2, v3]
    relative_coords: array, 在正交基下的相对坐标 [x, y, z]
    
    返回:
    world_coords: array, 在世界坐标系下的坐标
    """
    
    # 将输入转换为numpy数组
    base_vectors = np.array(base_vectors, dtype=float)
    relative_coords = np.array(relative_coords, dtype=float)
    
    # 验证输入维度
    if base_vectors.shape != (3, 3):
        raise ValueError("基向量应该是3个三维向量")
    if relative_coords.shape != (3,):
        raise ValueError("相对坐标应该是三维向量")
    
    # 验证基向量是否正交
    tolerance = 1e-10
    for i in range(3):
        for j in range(i+1, 3):
            dot_product = np.dot(base_vectors[i], base_vectors[j])
            if abs(dot_product) > tolerance:
                print(f"警告: 基向量不是严格正交的，v{i}·v{j} = {dot_product}")
    
    # 计算世界坐标：世界坐标 = 基向量矩阵 × 相对坐标
    world_coords = base_vectors.T @ relative_coords
    
    return world_coords

def world_to_relative_coordinates(base_vectors, world_coords):
    """
    将世界坐标转换为相对坐标（逆变换）
    
    参数:
    base_vectors: list of arrays, 正交基向量 [v1, v2, v3]
    world_coords: array, 在世界坐标系下的坐标
    
    返回:
    relative_coords: array, 在正交基下的相对坐标
    """
    
    base_vectors = np.array(base_vectors, dtype=float)
    world_coords = np.array(world_coords, dtype=float)
    
    # 对于正交基，逆变换就是转置
    relative_coords = base_vectors @ world_coords
    
    return relative_coords

# 示例使用
if __name__ == "__main__":
    # 示例1: 标准正交基（世界坐标系本身）
    print("示例1: 标准正交基")
    base_vectors_std = [
        [1, 0, 0],  # x轴基向量
        [0, 1, 0],  # y轴基向量
        [0, 0, 1]   # z轴基向量
    ]
    relative_coords = [2, 3, 4]  # 在基坐标系下的坐标
    
    world_coords = relative_to_world_coordinates(base_vectors_std, relative_coords)
    print(f"相对坐标 {relative_coords} 在世界坐标系下为: {world_coords}")
    
    # 验证逆变换
    relative_back = world_to_relative_coordinates(base_vectors_std, world_coords)
    print(f"逆变换验证: {relative_back}")
    
    # 示例2: 旋转后的正交基
    print("\n示例2: 旋转45度的坐标系")
    import math
    theta = math.radians(45)  # 45度旋转
    
    base_vectors_rotated = [
        [math.cos(theta), -math.sin(theta), 0],  # 旋转后的x轴
        [math.sin(theta), math.cos(theta), 0],   # 旋转后的y轴  
        [0, 0, 1]                               # z轴不变
    ]
    
    relative_coords = [1, 0, 0]  # 在旋转坐标系下x方向的单位向量
    
    world_coords = relative_to_world_coordinates(base_vectors_rotated, relative_coords)
    print(f"相对坐标 {relative_coords} 在世界坐标系下为: {world_coords}")
    print(f"向量长度: {np.linalg.norm(world_coords):.6f}")
    
    # 示例3: 自定义正交基
    print("\n示例3: 自定义正交基")
    base_vectors_custom = [
        [1/math.sqrt(2), 1/math.sqrt(2), 0],
        [-1/math.sqrt(2), 1/math.sqrt(2), 0],
        [0, 0, 1]
    ]
    
    relative_coords = [2, 1, 3]
    world_coords = relative_to_world_coordinates(base_vectors_custom, relative_coords)
    print(f"相对坐标 {relative_coords} 在世界坐标系下为: {world_coords}")
    
    # 验证坐标转换的正确性
    relative_back = world_to_relative_coordinates(base_vectors_custom, world_coords)
    print(f"逆变换验证: {relative_back}")
    print(f"误差: {np.linalg.norm(relative_coords - relative_back):.10f}")

# 更完整的类版本
class CoordinateTransformer:
    """坐标系转换器类"""
    
    def __init__(self, base_vectors):
        """
        初始化坐标系转换器
        
        参数:
        base_vectors: 基向量矩阵
        """
        self.base_vectors = np.array(base_vectors, dtype=float)
        self._validate_base()
        
    def _validate_base(self):
        """验证基向量的正交性"""
        if self.base_vectors.shape != (3, 3):
            raise ValueError("基向量应该是3×3矩阵")
        
        # 检查正交性
        tolerance = 1e-10
        for i in range(3):
            for j in range(i+1, 3):
                dot_product = np.dot(self.base_vectors[i], self.base_vectors[j])
                if abs(dot_product) > tolerance:
                    raise ValueError(f"基向量不是正交的: v{i}·v{j} = {dot_product}")
    
    def to_world(self, relative_coords):
        """相对坐标转世界坐标"""
        return self.base_vectors.T @ np.array(relative_coords)
    
    def to_relative(self, world_coords):
        """世界坐标转相对坐标"""
        return self.base_vectors @ np.array(world_coords)
    
    def get_base_vectors(self):
        """获取基向量"""
        return self.base_vectors.copy()

# 使用类的示例
print("\n使用CoordinateTransformer类:")
transformer = CoordinateTransformer(base_vectors_custom)
relative_coords = [1, 2, 3]
world_coords = transformer.to_world(relative_coords)
print(f"相对坐标 {relative_coords} -> 世界坐标 {world_coords}")
print(f"逆变换: {transformer.to_relative(world_coords)}")

示例1: 标准正交基
相对坐标 [2, 3, 4] 在世界坐标系下为: [2. 3. 4.]
逆变换验证: [2. 3. 4.]

示例2: 旋转45度的坐标系
相对坐标 [1, 0, 0] 在世界坐标系下为: [ 0.70710678 -0.70710678  0.        ]
向量长度: 1.000000

示例3: 自定义正交基
相对坐标 [2, 1, 3] 在世界坐标系下为: [0.70710678 2.12132034 3.        ]
逆变换验证: [2. 1. 3.]
误差: 0.0000000000

使用CoordinateTransformer类:
相对坐标 [1, 2, 3] -> 世界坐标 [-0.70710678  2.12132034  3.        ]
逆变换: [1. 2. 3.]