"""
刚体系统核心模块
负责刚体的创建、配置和管理
"""

from panda3d.core import Vec3, Point3, TransformState, LVector3f
from panda3d.bullet import BulletRigidBodyNode, BulletBoxShape, BulletSphereShape
from panda3d.bullet import BulletCapsuleShape, BulletConeShape, BulletCylinderShape
from panda3d.bullet import BulletConvexHullShape, BulletTriangleMeshShape, BulletTriangleMesh
from panda3d.bullet import BulletCollisionShape, BulletCompoundShape
import math
import time
from collections import deque


class RigidBody:
    """
    刚体类
    管理单个刚体的物理属性和行为
    """
    
    # 刚体类型常量
    STATIC = 0      # 静态刚体
    DYNAMIC = 1     # 动态刚体
    KINEMATIC = 2   # 运动学刚体
    
    # 碰撞形状类型常量
    SHAPE_BOX = 'box'
    SHAPE_SPHERE = 'sphere'
    SHAPE_CAPSULE = 'capsule'
    SHAPE_CONE = 'cone'
    SHAPE_CYLINDER = 'cylinder'
    SHAPE_CONVEX_HULL = 'convex_hull'
    SHAPE_MESH = 'mesh'
    SHAPE_COMPOUND = 'compound'
    
    # 碰撞组常量
    GROUP_STATIC = 1
    GROUP_DYNAMIC = 2
    GROUP_KINEMATIC = 4
    GROUP_SENSOR = 8
    GROUP_VEHICLE = 16
    GROUP_CHARACTER = 32
    GROUP_PARTICLE = 64
    GROUP_CUSTOM1 = 128
    GROUP_CUSTOM2 = 256
    
    def __init__(self, node_path, shape_type='box', mass=1.0, body_type=DYNAMIC, 
                 shape_params=None, compound_shapes=None, collision_group=None, collision_mask=None):
        """
        初始化刚体
        
        Args:
            node_path: Panda3D节点路径
            shape_type (str): 碰撞形状类型
            mass (float): 质量
            body_type (int): 刚体类型
            shape_params (dict): 形状参数
            compound_shapes (list): 复合形状列表
            collision_group (int): 碰撞组
            collision_mask (int): 碰撞掩码
        """
        self.node_path = node_path
        self.mass = mass
        self.body_type = body_type
        self.shape_type = shape_type
        self.shape_params = shape_params or {}
        self.compound_shapes = compound_shapes or []
        self.collision_group = collision_group or self.GROUP_DYNAMIC
        self.collision_mask = collision_mask or 0xFFFFFFFF
        
        # 创建碰撞形状
        self.shape = self._create_collision_shape(shape_type, shape_params, compound_shapes)
        
        # 创建Bullet刚体节点
        self.bullet_body = BulletRigidBodyNode('RigidBody')
        self.bullet_body.addShape(self.shape)
        
        # 设置刚体属性
        if body_type == self.STATIC:
            self.bullet_body.setMass(0)
            self.collision_group = self.GROUP_STATIC
        else:
            self.bullet_body.setMass(mass)
            
        if body_type == self.KINEMATIC:
            self.bullet_body.setKinematic(True)
            self.collision_group = self.GROUP_KINEMATIC
            
        # 设置碰撞组和掩码
        self.bullet_body.setCollideFilterGroup(self.collision_group)
        self.bullet_body.setCollideFilterMask(self.collision_mask)
            
        # 创建物理节点并附加到场景
        self.physics_node = self.node_path.attachNewNode(self.bullet_body)
        self.physics_node.setTransform(self.node_path.getTransform())
        
        # 物理属性
        self.friction = 0.5
        self.restitution = 0.1
        self.rolling_friction = 0.1
        self.spinning_friction = 0.1
        self.linear_damping = 0.0
        self.angular_damping = 0.0
        self.linear_factor = Vec3(1, 1, 1)
        self.angular_factor = Vec3(1, 1, 1)
        self.ccd_motion_threshold = 0.0
        self.ccd_swept_sphere_radius = 0.0
        self.anisotropic_friction = Vec3(1, 1, 1)
        self.contact_processing_threshold = 1e30
        self.deactivation_time = 0.0
        self.linear_sleeping_threshold = 0.8
        self.angular_sleeping_threshold = 1.0
        
        # 高级物理属性
        self.air_resistance = 0.0
        self.buoyancy = 0.0
        self.drag_coefficient = 0.0
        self.lift_coefficient = 0.0
        self.wind_resistance = 0.0
        self.magnetic_field = Vec3(0, 0, 0)
        
        # 力和扭矩历史记录
        self.force_history = deque(maxlen=100)
        self.torque_history = deque(maxlen=100)
        self.impulse_history = deque(maxlen=100)
        
        # 碰撞历史记录
        self.collision_history = deque(maxlen=50)
        
        # 运动历史记录
        self.position_history = deque(maxlen=100)
        self.velocity_history = deque(maxlen=100)
        self.angular_velocity_history = deque(maxlen=100)
        self.acceleration_history = deque(maxlen=100)
        
        # 状态标志
        self.is_sleeping = False
        self.is_kinematic_controlled = False
        self.is_ghost = False
        self.is_frozen = False
        self.is_submerged = False
        
        # 自定义数据
        self.custom_data = {}
        
        # 时间戳
        self.creation_time = time.time()
        self.last_update_time = self.creation_time
        self.last_collision_time = 0.0
        
        # 碰撞回调
        self.collision_callbacks = []
        
        # 应用默认属性
        self._apply_physics_properties()
        
        # 记录初始位置
        self._record_state()
        
        print(f"刚体创建完成: {self.bullet_body.getName()}")
        
    def _create_collision_shape(self, shape_type, shape_params=None, compound_shapes=None):
        """
        创建碰撞形状
        
        Args:
            shape_type (str): 形状类型
            shape_params (dict): 形状参数
            compound_shapes (list): 复合形状列表
            
        Returns:
            BulletShape: 碰撞形状对象
        """
        shape_params = shape_params or {}
        
        if shape_type == self.SHAPE_COMPOUND and compound_shapes:
            # 创建复合形状
            compound_shape = BulletCompoundShape()
            
            for shape_info in compound_shapes:
                sub_shape_type = shape_info.get('type', 'box')
                sub_shape_params = shape_info.get('params', {})
                transform = shape_info.get('transform', TransformState.makeIdentity())
                
                sub_shape = self._create_single_shape(sub_shape_type, sub_shape_params)
                compound_shape.addChildShape(transform, sub_shape)
                
            return compound_shape
            
        else:
            # 创建单一形状
            return self._create_single_shape(shape_type, shape_params)
            
    def _create_single_shape(self, shape_type, shape_params):
        """
        创建单一碰撞形状
        
        Args:
            shape_type (str): 形状类型
            shape_params (dict): 形状参数
            
        Returns:
            BulletShape: 碰撞形状对象
        """
        bounds = self.node_path.getBounds()
        margin = shape_params.get('margin', 0.04)  # 默认边界
        
        if shape_type == self.SHAPE_BOX:
            # 盒体形状
            min_point = bounds.getMin()
            max_point = bounds.getMax()
            extents = (max_point - min_point) * 0.5
            extents.setX(max(extents.getX(), margin))
            extents.setY(max(extents.getY(), margin))
            extents.setZ(max(extents.getZ(), margin))
            shape = BulletBoxShape(extents)
            
        elif shape_type == self.SHAPE_SPHERE:
            # 球体形状
            radius = bounds.getRadius()
            radius = max(radius, margin)
            shape = BulletSphereShape(radius)
            
        elif shape_type == self.SHAPE_CAPSULE:
            # 胶囊形状
            radius = bounds.getRadius()
            height = bounds.getMax().getZ() - bounds.getMin().getZ()
            radius = max(radius, margin)
            height = max(height, margin * 2)
            shape = BulletCapsuleShape(radius, height, 2)  # Z轴
            
        elif shape_type == self.SHAPE_CONE:
            # 圆锥形状
            radius = bounds.getRadius()
            height = bounds.getMax().getZ() - bounds.getMin().getZ()
            radius = max(radius, margin)
            height = max(height, margin)
            shape = BulletConeShape(radius, height, 2)  # Z轴
            
        elif shape_type == self.SHAPE_CYLINDER:
            # 圆柱形状
            radius = bounds.getRadius()
            height = bounds.getMax().getZ() - bounds.getMin().getZ()
            radius = max(radius, margin)
            height = max(height, margin)
            shape = BulletCylinderShape(radius, height, 2)  # Z轴
            
        elif shape_type == self.SHAPE_CONVEX_HULL:
            # 凸包形状
            shape = BulletConvexHullShape()
            # 从节点几何体创建凸包
            geom_node = self.node_path.node()
            if hasattr(geom_node, 'getGeom'):
                geom = geom_node.getGeom()
                shape.addGeom(geom)
            else:
                # 简化实现，使用包围盒
                min_point = bounds.getMin()
                max_point = bounds.getMax()
                shape.addPoint(min_point)
                shape.addPoint(max_point)
                
        elif shape_type == self.SHAPE_MESH:
            # 三角网格形状（用于静态物体）
            mesh = BulletTriangleMesh()
            # 从节点几何体创建网格
            geom_node = self.node_path.node()
            if hasattr(geom_node, 'getGeom'):
                geom = geom_node.getGeom()
                mesh.addGeom(geom)
            
            dynamic = (self.body_type != self.STATIC)
            shape = BulletTriangleMeshShape(mesh, dynamic=dynamic)
            
        else:
            # 默认使用盒体形状
            min_point = bounds.getMin()
            max_point = bounds.getMax()
            extents = (max_point - min_point) * 0.5
            extents.setX(max(extents.getX(), margin))
            extents.setY(max(extents.getY(), margin))
            extents.setZ(max(extents.getZ(), margin))
            shape = BulletBoxShape(extents)
            
        # 设置形状边界
        shape.setMargin(margin)
        
        return shape
        
    def _apply_physics_properties(self):
        """
        应用物理属性到Bullet刚体
        """
        self.bullet_body.setFriction(self.friction)
        self.bullet_body.setRestitution(self.restitution)
        self.bullet_body.setRollingFriction(self.rolling_friction)
        self.bullet_body.setSpinningFriction(self.spinning_friction)
        self.bullet_body.setLinearDamping(self.linear_damping)
        self.bullet_body.setAngularDamping(self.angular_damping)
        self.bullet_body.setLinearFactor(self.linear_factor)
        self.bullet_body.setAngularFactor(self.angular_factor)
        self.bullet_body.setAnisotropicFriction(self.anisotropic_friction)
        self.bullet_body.setContactProcessingThreshold(self.contact_processing_threshold)
        self.bullet_body.setDeactivationTime(self.deactivation_time)
        self.bullet_body.setSleepingThresholds(
            self.linear_sleeping_threshold, 
            self.angular_sleeping_threshold
        )
        
        # 设置CCD参数
        if self.ccd_motion_threshold > 0:
            self.bullet_body.setCcdMotionThreshold(self.ccd_motion_threshold)
        if self.ccd_swept_sphere_radius > 0:
            self.bullet_body.setCcdSweptSphereRadius(self.ccd_swept_sphere_radius)
            
    def set_friction(self, friction, static_friction=None, dynamic_friction=None):
        """
        设置摩擦系数
        
        Args:
            friction (float): 摩擦系数
            static_friction (float): 静摩擦系数（可选）
            dynamic_friction (float): 动摩擦系数（可选）
        """
        self.friction = friction
        self.bullet_body.setFriction(friction)
        
        # 如果提供了静摩擦和动摩擦，可以使用更复杂的摩擦模型
        if static_friction is not None or dynamic_friction is not None:
            # 这里可以实现更复杂的摩擦模型
            pass
            
    def set_restitution(self, restitution):
        """
        设置恢复系数（弹性）
        
        Args:
            restitution (float): 恢复系数
        """
        self.restitution = restitution
        self.bullet_body.setRestitution(restitution)
        
    def set_damping(self, linear_damping, angular_damping):
        """
        设置阻尼
        
        Args:
            linear_damping (float): 线性阻尼
            angular_damping (float): 角阻尼
        """
        self.linear_damping = linear_damping
        self.angular_damping = angular_damping
        self.bullet_body.setLinearDamping(linear_damping)
        self.bullet_body.setAngularDamping(angular_damping)
        
    def set_factors(self, linear_factor, angular_factor):
        """
        设置线性和角因子
        
        Args:
            linear_factor (Vec3): 线性因子
            angular_factor (Vec3): 角因子
        """
        self.linear_factor = linear_factor
        self.angular_factor = angular_factor
        self.bullet_body.setLinearFactor(linear_factor)
        self.bullet_body.setAngularFactor(angular_factor)
        
    def set_activation_state(self, active=True, always_active=False):
        """
        设置激活状态
        
        Args:
            active (bool): 是否激活
            always_active (bool): 是否始终保持激活
        """
        if always_active:
            self.bullet_body.setDeactivationEnabled(False)
        else:
            self.bullet_body.setDeactivationEnabled(True)
            self.bullet_body.setActive(active)
            
    def set_sleeping_thresholds(self, linear_threshold, angular_threshold):
        """
        设置休眠阈值
        
        Args:
            linear_threshold (float): 线性休眠阈值
            angular_threshold (float): 角休眠阈值
        """
        self.linear_sleeping_threshold = linear_threshold
        self.angular_sleeping_threshold = angular_threshold
        self.bullet_body.setSleepingThresholds(linear_threshold, angular_threshold)
        
    def set_ccd_parameters(self, motion_threshold, swept_sphere_radius):
        """
        设置连续碰撞检测参数
        
        Args:
            motion_threshold (float): 运动阈值
            swept_sphere_radius (float): 扫掠球半径
        """
        self.ccd_motion_threshold = motion_threshold
        self.ccd_swept_sphere_radius = swept_sphere_radius
        self.bullet_body.setCcdMotionThreshold(motion_threshold)
        self.bullet_body.setCcdSweptSphereRadius(swept_sphere_radius)
        
    def set_anisotropic_friction(self, friction):
        """
        设置各向异性摩擦
        
        Args:
            friction (Vec3): 各向异性摩擦系数
        """
        self.anisotropic_friction = friction
        self.bullet_body.setAnisotropicFriction(friction)
        
    def set_contact_threshold(self, threshold):
        """
        设置接触处理阈值
        
        Args:
            threshold (float): 接触阈值
        """
        self.contact_processing_threshold = threshold
        self.bullet_body.setContactProcessingThreshold(threshold)
        
    def set_collision_filter(self, group=None, mask=None):
        """
        设置碰撞过滤器
        
        Args:
            group (int): 碰撞组
            mask (int): 碰撞掩码
        """
        if group is not None:
            self.collision_group = group
            self.bullet_body.setCollideFilterGroup(group)
        if mask is not None:
            self.collision_mask = mask
            self.bullet_body.setCollideFilterMask(mask)
            
    def apply_force(self, force, position=None, duration=0.0):
        """
        应用力
        
        Args:
            force (Vec3): 力向量
            position (Point3): 作用点（可选）
            duration (float): 持续时间（秒，0表示瞬时）
        """
        # 记录力历史
        force_record = {
            'force': force,
            'position': position,
            'duration': duration,
            'timestamp': time.time()
        }
        self.force_history.append(force_record)
        
        if position is not None:
            self.bullet_body.applyForce(force, position)
        else:
            self.bullet_body.applyCentralForce(force)
            
    def apply_torque(self, torque, duration=0.0):
        """
        应用扭矩
        
        Args:
            torque (Vec3): 扭矩向量
            duration (float): 持续时间（秒，0表示瞬时）
        """
        # 记录扭矩历史
        torque_record = {
            'torque': torque,
            'duration': duration,
            'timestamp': time.time()
        }
        self.torque_history.append(torque_record)
        
        self.bullet_body.applyTorque(torque)
        
    def apply_impulse(self, impulse, position=None, duration=0.0):
        """
        应用冲量
        
        Args:
            impulse (Vec3): 冲量向量
            position (Point3): 作用点（可选）
            duration (float): 持续时间（秒，0表示瞬时）
        """
        # 记录冲量历史
        impulse_record = {
            'impulse': impulse,
            'position': position,
            'duration': duration,
            'timestamp': time.time()
        }
        self.impulse_history.append(impulse_record)
        
        if position is not None:
            self.bullet_body.applyImpulse(impulse, position)
        else:
            self.bullet_body.applyCentralImpulse(impulse)
            
    def apply_torque_impulse(self, torque_impulse):
        """
        应用扭矩冲量
        
        Args:
            torque_impulse (Vec3): 扭矩冲量向量
        """
        # 记录扭矩冲量历史
        torque_record = {
            'torque': torque_impulse,
            'duration': 0.0,
            'is_impulse': True,
            'timestamp': time.time()
        }
        self.torque_history.append(torque_record)
        
        self.bullet_body.applyTorqueImpulse(torque_impulse)
        
    def apply_force_field(self, field_type, field_params):
        """
        应用力场效果
        
        Args:
            field_type (str): 力场类型 ('gravity', 'wind', 'magnetic', 'buoyancy')
            field_params (dict): 力场参数
        """
        if field_type == 'gravity':
            gravity = field_params.get('gravity', Vec3(0, 0, -9.81))
            self.bullet_body.applyCentralForce(gravity * self.mass)
        elif field_type == 'wind':
            wind_vector = field_params.get('wind_vector', Vec3(0, 0, 0))
            wind_coefficient = field_params.get('wind_coefficient', 1.0)
            self.bullet_body.applyCentralForce(wind_vector * wind_coefficient)
        elif field_type == 'magnetic':
            magnetic_field = field_params.get('magnetic_field', Vec3(0, 0, 0))
            magnetic_coefficient = field_params.get('magnetic_coefficient', 1.0)
            self.bullet_body.applyCentralForce(magnetic_field * magnetic_coefficient)
        elif field_type == 'buoyancy':
            fluid_density = field_params.get('fluid_density', 1000.0)
            volume = self.mass / 1000.0  # 简化计算
            buoyancy_force = Vec3(0, 0, fluid_density * 9.81 * volume)
            self.bullet_body.applyCentralForce(buoyancy_force)
            
    def get_linear_velocity(self):
        """
        获取线性速度
        
        Returns:
            Vec3: 线性速度
        """
        return self.bullet_body.getLinearVelocity()
        
    def set_linear_velocity(self, velocity):
        """
        设置线性速度
        
        Args:
            velocity (Vec3): 线性速度
        """
        self.bullet_body.setLinearVelocity(velocity)
        
    def get_angular_velocity(self):
        """
        获取角速度
        
        Returns:
            Vec3: 角速度
        """
        return self.bullet_body.getAngularVelocity()
        
    def set_angular_velocity(self, velocity):
        """
        设置角速度
        
        Args:
            velocity (Vec3): 角速度
        """
        self.bullet_body.setAngularVelocity(velocity)
        
    def get_position(self):
        """
        获取位置
        
        Returns:
            Point3: 位置
        """
        return self.physics_node.getPos()
        
    def set_position(self, position):
        """
        设置位置
        
        Args:
            position (Point3): 位置
        """
        self.physics_node.setPos(position)
        if self.body_type == self.KINEMATIC:
            self.bullet_body.setKinematicPos(position)
            
    def get_rotation(self):
        """
        获取旋转
        
        Returns:
            Vec3: 旋转（HPR）
        """
        return self.physics_node.getHpr()
        
    def set_rotation(self, rotation):
        """
        设置旋转
        
        Args:
            rotation (Vec3): 旋转（HPR）
        """
        self.physics_node.setHpr(rotation)
        if self.body_type == self.KINEMATIC:
            self.bullet_body.setKinematicHpr(rotation)
            
    def get_transform(self):
        """
        获取变换矩阵
        
        Returns:
            TransformState: 变换状态
        """
        return self.physics_node.getTransform()
        
    def set_transform(self, transform):
        """
        设置变换矩阵
        
        Args:
            transform (TransformState): 变换状态
        """
        self.physics_node.setTransform(transform)
        if self.body_type == self.KINEMATIC:
            self.bullet_body.setKinematicPos(transform.getPos())
            self.bullet_body.setKinematicHpr(transform.getHpr())
            
    def sync_visual_node(self):
        """
        同步物理节点和可视化节点的位置
        """
        if self.node_path and not self.node_path.isEmpty():
            self.node_path.setTransform(self.physics_node.getTransform())
            
    def get_mass(self):
        """
        获取质量
        
        Returns:
            float: 质量
        """
        return self.bullet_body.getMass()
        
    def set_mass(self, mass):
        """
        设置质量
        
        Args:
            mass (float): 质量
        """
        self.mass = mass
        if self.body_type != self.STATIC:
            self.bullet_body.setMass(mass)
            
    def get_inertia(self):
        """
        获取惯性张量
        
        Returns:
            Vec3: 惯性张量
        """
        return self.bullet_body.getInertia()
        
    def set_inertia(self, inertia):
        """
        设置惯性张量
        
        Args:
            inertia (Vec3): 惯性张量
        """
        # 注意：Bullet可能不直接支持设置惯性张量
        pass
        
    def is_active(self):
        """
        检查刚体是否激活
        
        Returns:
            bool: 是否激活
        """
        return self.bullet_body.isActive()
        
    def is_static(self):
        """
        检查刚体是否为静态
        
        Returns:
            bool: 是否为静态
        """
        return self.body_type == self.STATIC
        
    def is_dynamic(self):
        """
        检查刚体是否为动态
        
        Returns:
            bool: 是否为动态
        """
        return self.body_type == self.DYNAMIC
        
    def is_kinematic(self):
        """
        检查刚体是否为运动学
        
        Returns:
            bool: 是否为运动学
        """
        return self.body_type == self.KINEMATIC
        
    def get_aabb(self):
        """
        获取包围盒
        
        Returns:
            tuple: (min_point, max_point)
        """
        aabb = self.bullet_body.getAabb()
        return (aabb.getMin(), aabb.getMax())
        
    def get_center_of_mass(self):
        """
        获取质心位置
        
        Returns:
            Point3: 质心位置
        """
        return self.bullet_body.getCenterOfMassTransform().getPos()
        
    def set_center_of_mass(self, center):
        """
        设置质心位置
        
        Args:
            center (Point3): 质心位置
        """
        # 注意：Bullet可能不直接支持设置质心
        pass
        
    def add_collision_callback(self, callback):
        """
        添加碰撞回调
        
        Args:
            callback (function): 回调函数
        """
        if callback not in self.collision_callbacks:
            self.collision_callbacks.append(callback)
            
    def remove_collision_callback(self, callback):
        """
        移除碰撞回调
        
        Args:
            callback (function): 回调函数
        """
        if callback in self.collision_callbacks:
            self.collision_callbacks.remove(callback)
            
    def _record_state(self):
        """
        记录当前状态
        """
        current_time = time.time()
        self.last_update_time = current_time
        
        # 记录位置和速度
        pos = self.get_position()
        vel = self.get_linear_velocity()
        ang_vel = self.get_angular_velocity()
        
        self.position_history.append((pos, current_time))
        self.velocity_history.append((vel, current_time))
        self.angular_velocity_history.append((ang_vel, current_time))
        
        # 计算并记录加速度
        if len(self.velocity_history) >= 2:
            vel2, time2 = self.velocity_history[-1]
            vel1, time1 = self.velocity_history[-2]
            if time2 - time1 > 0:
                acceleration = (vel2 - vel1) / (time2 - time1)
                self.acceleration_history.append((acceleration, current_time))
        
        # 更新休眠状态
        self.is_sleeping = not self.bullet_body.isActive()
        
    def get_velocity_history(self, duration=1.0):
        """
        获取指定时间段内的速度历史
        
        Args:
            duration (float): 时间段（秒）
            
        Returns:
            list: 速度历史记录
        """
        current_time = time.time()
        start_time = current_time - duration
        
        history = []
        for vel, timestamp in self.velocity_history:
            if timestamp >= start_time:
                history.append((vel, timestamp))
                
        return history
        
    def get_acceleration(self, time_window=0.1):
        """
        计算加速度
        
        Args:
            time_window (float): 时间窗口（秒）
            
        Returns:
            Vec3: 加速度向量
        """
        history = self.get_velocity_history(time_window)
        if len(history) < 2:
            return Vec3(0, 0, 0)
            
        # 计算最近两个时间点的速度差
        vel2, time2 = history[-1]
        vel1, time1 = history[-2]
        
        if time2 - time1 <= 0:
            return Vec3(0, 0, 0)
            
        acceleration = (vel2 - vel1) / (time2 - time1)
        return acceleration
        
    def get_kinetic_energy(self):
        """
        计算动能
        
        Returns:
            float: 动能
        """
        velocity = self.get_linear_velocity()
        angular_velocity = self.get_angular_velocity()
        inertia = self.get_inertia()
        
        # 平移动能: 0.5 * m * v^2
        linear_ke = 0.5 * self.mass * velocity.lengthSquared()
        
        # 转动动能: 0.5 * I * ω^2
        angular_ke = 0.5 * inertia.length() * angular_velocity.lengthSquared()
        
        return linear_ke + angular_ke
        
    def get_potential_energy(self, gravity=Vec3(0, 0, -9.81)):
        """
        计算重力势能
        
        Args:
            gravity (Vec3): 重力向量
            
        Returns:
            float: 重力势能
        """
        height = self.get_position().getZ()
        # E = m * g * h
        return self.mass * abs(gravity.getZ()) * height
        
    def get_total_energy(self, gravity=Vec3(0, 0, -9.81)):
        """
        计算总能量（动能+势能）
        
        Args:
            gravity (Vec3): 重力向量
            
        Returns:
            float: 总能量
        """
        return self.get_kinetic_energy() + self.get_potential_energy(gravity)
        
    def get_momentum(self):
        """
        计算动量
        
        Returns:
            Vec3: 动量向量
        """
        velocity = self.get_linear_velocity()
        return velocity * self.mass
        
    def get_angular_momentum(self):
        """
        计算角动量
        
        Returns:
            Vec3: 角动量向量
        """
        angular_velocity = self.get_angular_velocity()
        inertia = self.get_inertia()
        return angular_velocity * inertia.length()
        
    def set_custom_data(self, key, value):
        """
        设置自定义数据
        
        Args:
            key (str): 键
            value: 值
        """
        self.custom_data[key] = value
        
    def get_custom_data(self, key, default=None):
        """
        获取自定义数据
        
        Args:
            key (str): 键
            default: 默认值
            
        Returns:
            自定义数据值
        """
        return self.custom_data.get(key, default)
        
    def freeze(self, frozen=True):
        """
        冻结或解冻刚体
        
        Args:
            frozen (bool): 是否冻结
        """
        self.is_frozen = frozen
        if frozen:
            self.bullet_body.setActivationState(0)  # 禁用激活
        else:
            self.bullet_body.setActivationState(1)  # 启用激活
            
    def set_ghost(self, is_ghost=True):
        """
        设置为幽灵对象（不参与碰撞检测）
        
        Args:
            is_ghost (bool): 是否为幽灵对象
        """
        self.is_ghost = is_ghost
        if is_ghost:
            self.bullet_body.setCollideFilterGroup(0)
            self.bullet_body.setCollideFilterMask(0)
        else:
            self.bullet_body.setCollideFilterGroup(self.collision_group)
            self.bullet_body.setCollideFilterMask(self.collision_mask)
            
    def get_collision_info(self):
        """
        获取碰撞信息
        
        Returns:
            dict: 碰撞信息
        """
        return {
            'collision_count': len(self.collision_history),
            'last_collision_time': self.last_collision_time,
            'is_colliding': len(self.collision_history) > 0 and \
                           time.time() - self.last_collision_time < 0.1
        }
        
    def add_collision_event(self, other_body, contact_points, impulse):
        """
        添加碰撞事件
        
        Args:
            other_body (RigidBody): 另一个刚体
            contact_points (list): 接触点列表
            impulse (float): 冲量
        """
        collision_info = {
            'other_body': other_body,
            'contact_points': contact_points,
            'impulse': impulse,
            'timestamp': time.time()
        }
        self.collision_history.append(collision_info)
        self.last_collision_time = time.time()
        
        # 调用碰撞回调
        for callback in self.collision_callbacks:
            try:
                callback(self, other_body, contact_points, impulse)
            except Exception as e:
                print(f"碰撞回调执行错误: {e}")
                
    def clear_collision_history(self):
        """
        清除碰撞历史
        """
        self.collision_history.clear()
        self.last_collision_time = 0.0
        
    def get_force_history(self, duration=1.0):
        """
        获取指定时间段内的力历史
        
        Args:
            duration (float): 时间段（秒）
            
        Returns:
            list: 力历史记录
        """
        current_time = time.time()
        start_time = current_time - duration
        
        history = []
        for force_record in self.force_history:
            if force_record['timestamp'] >= start_time:
                history.append(force_record)
                
        return history
        
    def get_torque_history(self, duration=1.0):
        """
        获取指定时间段内的扭矩历史
        
        Args:
            duration (float): 时间段（秒）
            
        Returns:
            list: 扭矩历史记录
        """
        current_time = time.time()
        start_time = current_time - duration
        
        history = []
        for torque_record in self.torque_history:
            if torque_record['timestamp'] >= start_time:
                history.append(torque_record)
                
        return history
        
    def get_impulse_history(self, duration=1.0):
        """
        获取指定时间段内的冲量历史
        
        Args:
            duration (float): 时间段（秒）
            
        Returns:
            list: 冲量历史记录
        """
        current_time = time.time()
        start_time = current_time - duration
        
        history = []
        for impulse_record in self.impulse_history:
            if impulse_record['timestamp'] >= start_time:
                history.append(impulse_record)
                
        return history
        
    def get_statistics(self):
        """
        获取刚体统计信息
        
        Returns:
            dict: 统计信息
        """
        return {
            'name': self.bullet_body.getName(),
            'body_type': self.body_type,
            'mass': self.get_mass(),
            'position': self.get_position(),
            'velocity': self.get_linear_velocity(),
            'angular_velocity': self.get_angular_velocity(),
            'kinetic_energy': self.get_kinetic_energy(),
            'potential_energy': self.get_potential_energy(),
            'total_energy': self.get_total_energy(),
            'is_active': self.is_active(),
            'is_sleeping': self.is_sleeping,
            'collision_count': len(self.collision_history),
            'force_count': len(self.force_history),
            'torque_count': len(self.torque_history),
            'impulse_count': len(self.impulse_history),
            'age': time.time() - self.creation_time
        }
        
    def reset_forces(self):
        """
        重置所有力和扭矩
        """
        self.force_history.clear()
        self.torque_history.clear()
        self.impulse_history.clear()
        
    def reset_motion(self):
        """
        重置运动状态
        """
        self.set_linear_velocity(Vec3(0, 0, 0))
        self.set_angular_velocity(Vec3(0, 0, 0))
        self.reset_forces()
        
    def destroy(self):
        """
        销毁刚体
        """
        # 清理历史记录
        self.force_history.clear()
        self.torque_history.clear()
        self.impulse_history.clear()
        self.collision_history.clear()
        self.position_history.clear()
        self.velocity_history.clear()
        self.angular_velocity_history.clear()
        self.acceleration_history.clear()
        
        # 清理回调
        self.collision_callbacks.clear()
        
        # 清理自定义数据
        self.custom_data.clear()
        
        # 移除物理节点
        if self.physics_node and not self.physics_node.isEmpty():
            self.physics_node.removeNode()
            
        print(f"刚体已销毁: {self.bullet_body.getName()}")