EG/plugins/user/rigid_body_physics/utils/physics_utils.py

"""
实用工具模块
提供物理系统相关的实用函数和工具类
"""

from panda3d.core import Vec3, Point3, LVector3f
from panda3d.core import TransformState
import math
import json


class PhysicsUtils:
    """
    物理实用工具类
    提供常用的物理计算和转换函数
    """
    
    @staticmethod
    def calculate_mass_from_density(shape, density):
        """
        根据密度计算质量
        
        Args:
            shape: 碰撞形状对象
            density (float): 密度（kg/m³）
            
        Returns:
            float: 质量（kg）
        """
        # 获取形状体积（简化实现）
        volume = PhysicsUtils.calculate_volume(shape)
        return volume * density
        
    @staticmethod
    def calculate_volume(shape):
        """
        计算形状体积（简化实现）
        
        Args:
            shape: 碰撞形状对象
            
        Returns:
            float: 体积（m³）
        """
        # 这里只是一个简化的实现，实际应该根据形状类型计算
        try:
            # 尝试获取形状的AABB
            aabb = shape.getAabb()
            min_point = aabb.getMin()
            max_point = aabb.getMax()
            
            # 计算包围盒体积
            dimensions = max_point - min_point
            volume = dimensions.getX() * dimensions.getY() * dimensions.getZ()
            
            # 简化：假设实际体积是包围盒体积的一半
            return volume * 0.5
        except:
            # 默认返回1立方米
            return 1.0
            
    @staticmethod
    def apply_impulse_at_point(body, impulse, point, relative_to_world=True):
        """
        在指定点应用冲量
        
        Args:
            body: 刚体对象
            impulse (Vec3): 冲量向量
            point (Point3): 作用点
            relative_to_world (bool): 点坐标是否相对于世界坐标系
        """
        if relative_to_world:
            # 转换到局部坐标系
            local_point = body.physics_node.getTransform().getInverse().xformPoint(point)
        else:
            local_point = point
            
        body.bullet_body.applyImpulse(impulse, local_point)
        
    @staticmethod
    def calculate_torque_from_force(force, point, center_of_mass=Point3(0, 0, 0)):
        """
        根据力和作用点计算扭矩
        
        Args:
            force (Vec3): 力向量
            point (Point3): 作用点
            center_of_mass (Point3): 质心位置
            
        Returns:
            Vec3: 扭矩向量
        """
        r = point - center_of_mass
        return r.cross(force)
        
    @staticmethod
    def interpolate_transforms(transform_a, transform_b, t):
        """
        插值两个变换
        
        Args:
            transform_a (TransformState): 起始变换
            transform_b (TransformState): 结束变换
            t (float): 插值因子 (0.0-1.0)
            
        Returns:
            TransformState: 插值后的变换
        """
        # 插值位置
        pos_a = transform_a.getPos()
        pos_b = transform_b.getPos()
        interpolated_pos = pos_a + (pos_b - pos_a) * t
        
        # 插值旋转（简化实现）
        hpr_a = transform_a.getHpr()
        hpr_b = transform_b.getHpr()
        interpolated_hpr = hpr_a + (hpr_b - hpr_a) * t
        
        # 插值缩放
        scale_a = transform_a.getScale()
        scale_b = transform_b.getScale()
        interpolated_scale = scale_a + (scale_b - scale_a) * t
        
        return TransformState.makePosHprScale(interpolated_pos, interpolated_hpr, interpolated_scale)
        
    @staticmethod
    def clamp_vector(vector, max_length):
        """
        限制向量长度
        
        Args:
            vector (Vec3): 输入向量
            max_length (float): 最大长度
            
        Returns:
            Vec3: 限制后的向量
        """
        length = vector.length()
        if length > max_length and length > 0:
            return vector * (max_length / length)
        return vector
        
    @staticmethod
    def reflect_vector(vector, normal):
        """
        计算向量在法线上的反射
        
        Args:
            vector (Vec3): 入射向量
            normal (Vec3): 法线向量（应为单位向量）
            
        Returns:
            Vec3: 反射向量
        """
        # R = V - 2 * (V · N) * N
        dot_product = vector.dot(normal)
        reflection = vector - normal * (2.0 * dot_product)
        return reflection
        
    @staticmethod
    def calculate_angular_velocity_from_rotation(start_rotation, end_rotation, time_delta):
        """
        根据旋转变化计算角速度
        
        Args:
            start_rotation (Vec3): 起始旋转
            end_rotation (Vec3): 结束旋转
            time_delta (float): 时间差
            
        Returns:
            Vec3: 角速度
        """
        if time_delta <= 0:
            return Vec3(0, 0, 0)
            
        # 计算旋转差
        rotation_delta = end_rotation - start_rotation
        
        # 处理角度环绕
        for i in range(3):  # H, P, R
            while rotation_delta[i] > 180:
                rotation_delta[i] -= 360
            while rotation_delta[i] < -180:
                rotation_delta[i] += 360
                
        # 计算角速度
        return rotation_delta / time_delta
        
    @staticmethod
    def create_transform_from_position_rotation(position, rotation):
        """
        根据位置和旋转创建变换
        
        Args:
            position (Point3): 位置
            rotation (Vec3): 旋转（HPR）
            
        Returns:
            TransformState: 变换状态
        """
        return TransformState.makePosHpr(position, rotation)
        
    @staticmethod
    def get_transform_matrix(transform):
        """
        获取变换的矩阵表示
        
        Args:
            transform (TransformState): 变换状态
            
        Returns:
            LMatrix4f: 变换矩阵
        """
        return transform.getMat()
        
    @staticmethod
    def transform_point(transform, point):
        """
        变换点
        
        Args:
            transform (TransformState): 变换状态
            point (Point3): 点
            
        Returns:
            Point3: 变换后的点
        """
        return transform.getMat().xformPoint(point)
        
    @staticmethod
    def transform_vector(transform, vector):
        """
        变换向量
        
        Args:
            transform (TransformState): 变换状态
            vector (Vec3): 向量
            
        Returns:
            Vec3: 变换后的向量
        """
        return transform.getMat().xformVec(vector)


class PhysicsConfigManager:
    """
    物理配置管理器
    管理物理系统的配置文件
    """
    
    def __init__(self, config_file=None):
        """
        初始化配置管理器
        
        Args:
            config_file (str): 配置文件路径
        """
        self.config_file = config_file
        self.config_data = {}
        
        if config_file:
            self.load_config(config_file)
            
    def load_config(self, config_file):
        """
        加载配置文件
        
        Args:
            config_file (str): 配置文件路径
        """
        try:
            with open(config_file, 'r', encoding='utf-8') as f:
                self.config_data = json.load(f)
            self.config_file = config_file
            print(f"配置文件加载成功: {config_file}")
        except Exception as e:
            print(f"加载配置文件失败: {e}")
            
    def save_config(self, config_file=None):
        """
        保存配置文件
        
        Args:
            config_file (str): 配置文件路径（可选）
        """
        save_path = config_file or self.config_file
        if not save_path:
            print("未指定配置文件路径")
            return
            
        try:
            with open(save_path, 'w', encoding='utf-8') as f:
                json.dump(self.config_data, f, indent=2, ensure_ascii=False)
            print(f"配置文件保存成功: {save_path}")
        except Exception as e:
            print(f"保存配置文件失败: {e}")
            
    def get_setting(self, key, default=None):
        """
        获取配置项
        
        Args:
            key (str): 配置项键名
            default: 默认值
            
        Returns:
            配置项值
        """
        return self.config_data.get(key, default)
        
    def set_setting(self, key, value):
        """
        设置配置项
        
        Args:
            key (str): 配置项键名
            value: 配置项值
        """
        self.config_data[key] = value
        
    def get_physics_settings(self):
        """
        获取物理设置
        
        Returns:
            dict: 物理设置
        """
        return self.config_data.get('physics', {})
        
    def set_physics_settings(self, settings):
        """
        设置物理设置
        
        Args:
            settings (dict): 物理设置
        """
        self.config_data['physics'] = settings
        
    def get_material_settings(self, material_name):
        """
        获取材质设置
        
        Args:
            material_name (str): 材质名称
            
        Returns:
            dict: 材质设置
        """
        materials = self.config_data.get('materials', {})
        return materials.get(material_name, {})
        
    def set_material_settings(self, material_name, settings):
        """
        设置材质设置
        
        Args:
            material_name (str): 材质名称
            settings (dict): 材质设置
        """
        if 'materials' not in self.config_data:
            self.config_data['materials'] = {}
        self.config_data['materials'][material_name] = settings


class PhysicsCollisionGroups:
    """
    物理碰撞组定义
    定义常用的碰撞组和掩码
    """
    
    # 基础碰撞组
    GROUP_STATIC = 1        # 静态物体
    GROUP_DYNAMIC = 2       # 动态物体
    GROUP_KINEMATIC = 4     # 运动物体
    GROUP_SENSOR = 8        # 传感器
    GROUP_PARTICLE = 16     # 粒子
    GROUP_VEHICLE = 32      # 车辆
    GROUP_CHARACTER = 64    # 角色
    GROUP_TRIGGER = 128     # 触发器
    
    # 常用掩码组合
    MASK_ALL = 0xFFFFFFFF   # 所有组
    MASK_STATIC_DYNAMIC = GROUP_STATIC | GROUP_DYNAMIC  # 静态与动态
    MASK_DYNAMIC_ONLY = GROUP_DYNAMIC  # 仅动态
    MASK_VEHICLE_WORLD = GROUP_STATIC | GROUP_DYNAMIC | GROUP_VEHICLE  # 车辆与世界
    MASK_CHARACTER_WORLD = GROUP_STATIC | GROUP_DYNAMIC | GROUP_CHARACTER  # 角色与世界
    MASK_PARTICLE_WORLD = GROUP_STATIC | GROUP_DYNAMIC | GROUP_PARTICLE  # 粒子与世界
    
    @staticmethod
    def create_mask(*groups):
        """
        创建掩码
        
        Args:
            *groups: 碰撞组
            
        Returns:
            int: 掩码值
        """
        mask = 0
        for group in groups:
            mask |= group
        return mask
        
    @staticmethod
    def add_to_mask(mask, *groups):
        """
        添加组到掩码
        
        Args:
            mask (int): 原始掩码
            *groups: 要添加的碰撞组
            
        Returns:
            int: 新掩码
        """
        for group in groups:
            mask |= group
        return mask
        
    @staticmethod
    def remove_from_mask(mask, *groups):
        """
        从掩码移除组
        
        Args:
            mask (int): 原始掩码
            *groups: 要移除的碰撞组
            
        Returns:
            int: 新掩码
        """
        for group in groups:
            mask &= ~group
        return mask


# 预定义的物理常量
PHYSICS_CONSTANTS = {
    'GRAVITY_EARTH': Vec3(0, 0, -9.81),      # 地球重力
    'GRAVITY_MOON': Vec3(0, 0, -1.62),       # 月球重力
    'GRAVITY_MARS': Vec3(0, 0, -3.71),       # 火星重力
    'AIR_DENSITY_SEA_LEVEL': 1.225,          # 海平面空气密度
    'WATER_DENSITY': 1000.0,                 # 水密度
    'SPEED_OF_SOUND_AIR': 343.0,             # 空气中声速
    'SPEED_OF_LIGHT': 299792458.0           # 光速
}