"""
地形物理属性系统
提供完整的地形物理属性定义、刚体动力学、碰撞检测、约束系统等功能
"""

from panda3d.core import NodePath, Vec3, Point3, BitMask32, TransformState
from panda3d.core import Material, MaterialAttrib, ClockObject
from panda3d.bullet import BulletWorld, BulletRigidBodyNode, BulletPlaneShape
from panda3d.bullet import BulletTriangleMesh, BulletTriangleMeshShape, BulletHeightfieldShape
from panda3d.bullet import BulletConstraint, BulletPoint2PointConstraint, BulletHingeConstraint
from panda3d.bullet import BulletSliderConstraint, BulletConeTwistConstraint, BulletGeneric6DofConstraint
from panda3d.bullet import BulletVehicle, BulletWheel, BulletBoxShape, BulletSphereShape
from panda3d.bullet import BulletSoftBodyNode, BulletSoftBodyConfig
import math
import json
import os
import random
from collections import deque

class TerrainPhysicsSystem:
    """
    地形物理属性系统类
    提供完整的刚体动力学、碰撞检测、约束系统、车辆模拟等功能
    """
    
    def __init__(self, world):
        self.world = world
        self.physics_world = None
        self.terrain_physics = {}  # 存储地形物理属性
        self.materials = {}  # 存储物理材质
        self.rigid_bodies = []  # 存储刚体
        self.constraints = []  # 存储约束
        self.vehicles = []  # 存储车辆
        self.particles = []  # 存储粒子系统
        self.soft_bodies = []  # 存储软体
        self.physics_enabled = True
        self.substeps = 10  # 物理子步数
        self.internal_time_step = 1.0/60.0  # 内部时间步长
        self.time_accumulator = 0.0  # 时间累积器
        self.max_substeps = 10  # 最大子步数
        
        # 物理参数
        self.physics_params = {
            'gravity': Vec3(0, 0, -9.81),
            'air_density': 1.225,  # 空气密度
            'default_friction': 0.5,
            'default_restitution': 0.1,
            'default_rolling_friction': 0.1,
            'default_spinning_friction': 0.1
        }
        
        # 性能监控
        self.performance_stats = {
            'last_update_time': 0.0,
            'update_count': 0,
            'avg_update_time': 0.0,
            'max_update_time': 0.0
        }
        
        # 初始化物理世界
        self._init_physics_world()
        
    def _init_physics_world(self):
        """
        初始化物理世界
        """
        try:
            self.physics_world = BulletWorld()
            self.physics_world.setGravity(self.physics_params['gravity'])
            
            # 设置物理世界参数
            self.physics_world.setConstraintSolver(self.physics_world.getConstraintSolver())
            self.physics_world.getDispatchInfo().set_merpFactor(0.1)
            self.physics_world.getDispatchInfo().set_misERP(0.1)
            
            print("物理世界初始化完成")
            
        except Exception as e:
            print(f"初始化物理世界时出错: {e}")
            
    def define_material(self, name, properties):
        """
        定义物理材质
        properties: 材质属性字典
        """
        try:
            material = {
                'name': name,
                'friction': properties.get('friction', self.physics_params['default_friction']),
                'restitution': properties.get('restitution', self.physics_params['default_restitution']),
                'rolling_friction': properties.get('rolling_friction', self.physics_params['default_rolling_friction']),
                'spinning_friction': properties.get('spinning_friction', self.physics_params['default_spinning_friction']),
                'density': properties.get('density', 1000.0),
                'color': properties.get('color', (0.5, 0.5, 0.5, 1.0)),
                'static_friction': properties.get('static_friction', 0.6),
                'dynamic_friction': properties.get('dynamic_friction', 0.4),
                'surface_velocity': properties.get('surface_velocity', Vec3(0, 0, 0)),
                'anisotropic_friction': properties.get('anisotropic_friction', Vec3(1, 1, 1)),
                'contact_cfm': properties.get('contact_cfm', 0.0),
                'contact_erp': properties.get('contact_erp', 0.2)
            }
            
            self.materials[name] = material
            print(f"定义物理材质: {name}")
            return material
            
        except Exception as e:
            print(f"定义物理材质时出错: {e}")
            return None
            
    def assign_material_to_terrain(self, terrain_info, material_name):
        """
        为地形分配物理材质
        """
        try:
            if material_name not in self.materials:
                print(f"物理材质 {material_name} 未定义")
                return False
                
            # 保存材质信息
            self.terrain_physics[terrain_info['node']] = {
                'material': material_name,
                'material_properties': self.materials[material_name]
            }
            
            # 创建地形物理碰撞体
            self._create_terrain_collision(terrain_info, material_name)
            
            print(f"为地形分配物理材质: {material_name}")
            return True
            
        except Exception as e:
            print(f"为地形分配物理材质时出错: {e}")
            return False
            
    def _create_terrain_collision(self, terrain_info, material_name):
        """
        创建地形物理碰撞体（增强版）
        """
        try:
            terrain_node = terrain_info['node']
            heightfield = terrain_info['heightfield']
            
            if not heightfield:
                print("无法获取地形高度图数据")
                return False
                
            # 移除现有的物理碰撞体
            self._remove_terrain_physics(terrain_node)
            
            # 创建Bullet高度场形状
            width = heightfield.getXSize()
            height = heightfield.getYSize()
            
            # 提取高度数据
            height_data = []
            for y in range(height):
                for x in range(width):
                    height_value = heightfield.getRed(x, y)
                    # 转换高度值到物理坐标系
                    physical_height = height_value * terrain_node.getScale().getZ()
                    height_data.append(physical_height)
                    
            # 创建高度场形状
            hfield_shape = BulletHeightfieldShape(
                height_data, 
                width, 
                height, 
                terrain_node.getScale().getX(),  # X间距
                terrain_node.getScale().getY(),  # Y间距
                1  # up axis (Z轴)
            )
            
            # 创建刚体节点
            body_name = f'terrain_physics_{len(self.rigid_bodies)}'
            body_node = BulletRigidBodyNode(body_name)
            body_node.addShape(hfield_shape)
            
            # 设置材质属性
            material = self.materials[material_name]
            body_node.setFriction(material['friction'])
            body_node.setRestitution(material['restitution'])
            body_node.setRollingFriction(material['rolling_friction'])
            body_node.setSpinningFriction(material['spinning_friction'])
            body_node.setAnisotropicFriction(material['anisotropic_friction'])
            body_node.setContactProcessingThreshold(1e30)
            
            # 设置为静态刚体（地形通常不移动）
            body_node.setMass(0)
            
            # 创建物理节点
            physics_node = terrain_node.attachNewNode(body_node)
            physics_node.setPos(terrain_node.getPos())
            
            # 添加到物理世界
            self.physics_world.attachRigidBody(body_node)
            
            # 保存刚体信息
            body_info = {
                'node': physics_node,
                'body': body_node,
                'shape': hfield_shape,
                'terrain_info': terrain_info,
                'type': 'terrain',
                'material': material_name,
                'creation_time': self.world.globalClock.getFrameTime()
            }
            
            self.rigid_bodies.append(body_info)
            
            print(f"创建地形物理碰撞体: {material_name} (尺寸: {width}x{height})")
            return True
            
        except Exception as e:
            print(f"创建地形物理碰撞体时出错: {e}")
            return False
            
    def _remove_terrain_physics(self, terrain_node):
        """
        移除地形物理碰撞体
        """
        try:
            for body_info in self.rigid_bodies[:]:  # 使用切片复制列表
                if (body_info.get('type') == 'terrain' and 
                    body_info['terrain_info']['node'] == terrain_node):
                    # 从物理世界移除
                    self.physics_world.removeRigidBody(body_info['body'])
                    
                    # 移除节点
                    if body_info['node'] and not body_info['node'].isEmpty():
                        body_info['node'].removeNode()
                        
                    # 从列表移除
                    self.rigid_bodies.remove(body_info)
                    
            return True
            
        except Exception as e:
            print(f"移除地形物理碰撞体时出错: {e}")
            return False
            
    def create_rigid_body(self, node, mass=1.0, shape_type='box', material_name=None, 
                         kinematic=False, activation_state='active', collision_group=1, 
                         collision_mask=0xFFFFFFFF):
        """
        创建刚体（增强版）
        """
        try:
            if not self.physics_enabled:
                print("物理系统已禁用")
                return None
                
            # 创建碰撞形状
            shape = self._create_collision_shape(node, shape_type)
            if not shape:
                return None
                
            # 创建刚体节点
            body_name = f'rigid_body_{len(self.rigid_bodies)}'
            body_node = BulletRigidBodyNode(body_name)
            body_node.addShape(shape)
            body_node.setMass(mass)
            
            # 设置碰撞组和掩码
            body_node.setCollideFilterGroup(collision_group)
            body_node.setCollideFilterMask(collision_mask)
            
            # 设置是否为运动学刚体
            if kinematic:
                body_node.setKinematic(True)
                
            # 设置激活状态
            if activation_state == 'inactive':
                body_node.setActive(False)
            elif activation_state == 'always_active':
                body_node.setDeactivationEnabled(False)
                
            # 设置材质属性
            if material_name and material_name in self.materials:
                material = self.materials[material_name]
                body_node.setFriction(material['friction'])
                body_node.setRestitution(material['restitution'])
                body_node.setRollingFriction(material['rolling_friction'])
                body_node.setSpinningFriction(material['spinning_friction'])
                body_node.setAnisotropicFriction(material.get('anisotropic_friction', Vec3(1, 1, 1)))
                body_node.setContactProcessingThreshold(material.get('contact_threshold', 1e30))
            else:
                # 使用默认材质属性
                body_node.setFriction(self.physics_params['default_friction'])
                body_node.setRestitution(self.physics_params['default_restitution'])
                body_node.setRollingFriction(self.physics_params['default_rolling_friction'])
                body_node.setSpinningFriction(self.physics_params['default_spinning_friction'])
                
            # 创建物理节点
            physics_node = node.attachNewNode(body_node)
            physics_node.setPos(node.getPos())
            physics_node.setHpr(node.getHpr())
            
            # 添加到物理世界
            self.physics_world.attachRigidBody(body_node)
            
            # 保存刚体信息
            body_info = {
                'node': physics_node,
                'body': body_node,
                'shape': shape,
                'original_node': node,
                'mass': mass,
                'material': material_name,
                'type': 'rigid_body',
                'kinematic': kinematic,
                'activation_state': activation_state,
                'collision_group': collision_group,
                'collision_mask': collision_mask,
                'velocity': Vec3(0, 0, 0),
                'angular_velocity': Vec3(0, 0, 0),
                'forces': [],  # 应用力列表
                'torques': [],  # 应用扭矩列表
                'linear_damping': 0.0,
                'angular_damping': 0.0,
                'linear_factor': Vec3(1, 1, 1),
                'angular_factor': Vec3(1, 1, 1),
                'sleeping_threshold': 0.8,
                'deactivation_time': 0.0,
                'creation_time': self.world.globalClock.getFrameTime(),
                'last_update': self.world.globalClock.getFrameTime(),
                'collision_history': deque(maxlen=10)  # 碰撞历史
            }
            
            # 设置阻尼
            body_node.setLinearDamping(body_info['linear_damping'])
            body_node.setAngularDamping(body_info['angular_damping'])
            
            # 设置因子
            body_node.setLinearFactor(body_info['linear_factor'])
            body_node.setAngularFactor(body_info['angular_factor'])
            
            # 设置睡眠阈值
            body_node.setSleepingThresholds(
                body_info['sleeping_threshold'], 
                body_info['sleeping_threshold'] * 0.1
            )
            
            self.rigid_bodies.append(body_info)
            
            print(f"创建刚体: {node.getName()} (质量: {mass}, 类型: {shape_type})")
            return body_info
            
        except Exception as e:
            print(f"创建刚体时出错: {e}")
            return None
            
    def _create_collision_shape(self, node, shape_type):
        """
        创建碰撞形状（增强版）
        """
        try:
            from panda3d.bullet import BulletBoxShape, BulletSphereShape, BulletCapsuleShape
            from panda3d.bullet import BulletConeShape, BulletCylinderShape, BulletConvexHullShape
            from panda3d.bullet import BulletTriangleMesh, BulletTriangleMeshShape
            
            if shape_type == 'box':
                # 获取节点包围盒
                bounds = node.getBounds()
                min_point = bounds.getMin()
                max_point = bounds.getMax()
                extents = (max_point - min_point) * 0.5
                shape = BulletBoxShape(extents)
                
            elif shape_type == 'sphere':
                bounds = node.getBounds()
                radius = bounds.getRadius()
                shape = BulletSphereShape(radius)
                
            elif shape_type == 'capsule':
                bounds = node.getBounds()
                radius = bounds.getRadius()
                height = bounds.getMax().getZ() - bounds.getMin().getZ()
                shape = BulletCapsuleShape(radius, height, 2)  # Z-axis
                
            elif shape_type == 'cone':
                bounds = node.getBounds()
                radius = bounds.getRadius()
                height = bounds.getMax().getZ() - bounds.getMin().getZ()
                shape = BulletConeShape(radius, height, 2)  # Z-axis
                
            elif shape_type == 'cylinder':
                bounds = node.getBounds()
                radius = bounds.getRadius()
                height = bounds.getMax().getZ() - bounds.getMin().getZ()
                shape = BulletCylinderShape(radius, height, 2)  # Z-axis
                
            elif shape_type == 'convex_hull':
                # 创建凸包形状
                mesh = BulletTriangleMesh()
                # 这里需要从节点提取几何数据
                # 简化实现，使用包围盒
                bounds = node.getBounds()
                min_point = bounds.getMin()
                max_point = bounds.getMax()
                shape = BulletBoxShape((max_point - min_point) * 0.5)
                
            elif shape_type == 'mesh':
                # 创建三角网格形状（用于静态物体）
                mesh = BulletTriangleMesh()
                # 这里需要从节点提取几何数据
                # 简化实现，使用包围盒
                bounds = node.getBounds()
                min_point = bounds.getMin()
                max_point = bounds.getMax()
                shape = BulletBoxShape((max_point - min_point) * 0.5)
                
            else:
                # 默认使用包围盒
                bounds = node.getBounds()
                min_point = bounds.getMin()
                max_point = bounds.getMax()
                extents = (max_point - min_point) * 0.5
                shape = BulletBoxShape(extents)
                
            return shape
            
        except Exception as e:
            print(f"创建碰撞形状时出错: {e}")
            return None
            
    def remove_rigid_body(self, body_info):
        """
        移除刚体
        """
        try:
            if body_info in self.rigid_bodies:
                # 从物理世界移除
                self.physics_world.removeRigidBody(body_info['body'])
                
                # 移除节点
                if body_info['node'] and not body_info['node'].isEmpty():
                    body_info['node'].removeNode()
                    
                # 如果有原始节点，恢复其位置
                if body_info['original_node'] and not body_info['original_node'].isEmpty():
                    body_info['original_node'].show()
                    
                # 从列表移除
                self.rigid_bodies.remove(body_info)
                
                print("移除刚体")
                return True
                
        except Exception as e:
            print(f"移除刚体时出错: {e}")
            return False
            
    def create_constraint(self, body_a, body_b, constraint_type='point', 
                         pivot_a=None, pivot_b=None, axis=None, 
                         enable_collision=False, breaking_threshold=0.0):
        """
        创建约束（增强版）
        """
        try:
            constraint = None
            
            if constraint_type == 'point':
                # 点对点约束
                if pivot_a is None:
                    pivot_a = Point3(0, 0, 0)
                if pivot_b is None:
                    pivot_b = Point3(0, 0, 0)
                constraint = BulletPoint2PointConstraint(
                    body_a['body'], body_b['body'], pivot_a, pivot_b)
                    
            elif constraint_type == 'hinge':
                # 铰链约束
                if pivot_a is None:
                    pivot_a = Point3(0, 0, 0)
                if pivot_b is None:
                    pivot_b = Point3(0, 0, 0)
                if axis is None:
                    axis = Vec3(0, 0, 1)
                constraint = BulletHingeConstraint(
                    body_a['body'], body_b['body'], pivot_a, pivot_b, axis, axis)
                # 设置限制
                constraint.setLimit(-math.pi/2, math.pi/2)
                    
            elif constraint_type == 'slider':
                # 滑动约束
                if pivot_a is None:
                    pivot_a = Point3(0, 0, 0)
                if pivot_b is None:
                    pivot_b = Point3(0, 0, 0)
                if axis is None:
                    axis = Vec3(0, 0, 1)
                constraint = BulletSliderConstraint(
                    body_a['body'], body_b['body'], TransformState.makePos(pivot_a), 
                    TransformState.makePos(pivot_b), True)
                # 设置滑动限制
                constraint.setLowerLinLimit(-5.0)
                constraint.setUpperLinLimit(5.0)
                    
            elif constraint_type == 'cone_twist':
                # 锥形扭转约束
                if pivot_a is None:
                    pivot_a = Point3(0, 0, 0)
                if pivot_b is None:
                    pivot_b = Point3(0, 0, 0)
                constraint = BulletConeTwistConstraint(
                    body_a['body'], body_b['body'], TransformState.makePos(pivot_a), 
                    TransformState.makePos(pivot_b))
                # 设置锥形限制
                constraint.setLimit(math.pi/4, math.pi/4, math.pi/2)
                
            elif constraint_type == 'generic_6dof':
                # 通用6自由度约束
                if pivot_a is None:
                    pivot_a = Point3(0, 0, 0)
                if pivot_b is None:
                    pivot_b = Point3(0, 0, 0)
                constraint = BulletGeneric6DofConstraint(
                    body_a['body'], body_b['body'], TransformState.makePos(pivot_a), 
                    TransformState.makePos(pivot_b), True)
                # 设置线性限制
                constraint.setLinearLowerLimit(Vec3(-1, -1, -1))
                constraint.setLinearUpperLimit(Vec3(1, 1, 1))
                # 设置角度限制
                constraint.setAngularLowerLimit(Vec3(-math.pi/4, -math.pi/4, -math.pi/4))
                constraint.setAngularUpperLimit(Vec3(math.pi/4, math.pi/4, math.pi/4))
                
            if constraint:
                # 设置是否允许碰撞
                constraint.setOverrideEnabled(not enable_collision)
                
                # 设置断裂阈值
                if breaking_threshold > 0:
                    constraint.setBreakingImpulseThreshold(breaking_threshold)
                
                # 添加到物理世界
                self.physics_world.attachConstraint(constraint)
                
                # 保存约束信息
                constraint_info = {
                    'constraint': constraint,
                    'body_a': body_a,
                    'body_b': body_b,
                    'type': constraint_type,
                    'enabled': True,
                    'breaking_threshold': breaking_threshold,
                    'creation_time': self.world.globalClock.getFrameTime()
                }
                
                self.constraints.append(constraint_info)
                print(f"创建约束: {constraint_type}")
                return constraint_info
                
        except Exception as e:
            print(f"创建约束时出错: {e}")
            return None
            
    def remove_constraint(self, constraint_info):
        """
        移除约束
        """
        try:
            if constraint_info in self.constraints:
                # 从物理世界移除
                self.physics_world.removeConstraint(constraint_info['constraint'])
                
                # 从列表移除
                self.constraints.remove(constraint_info)
                
                print("移除约束")
                return True
                
        except Exception as e:
            print(f"移除约束时出错: {e}")
            return False
            
    def create_vehicle(self, chassis_body, wheel_config):
        """
        创建车辆（增强版）
        """
        try:
            # 创建车辆
            vehicle = BulletVehicle(self.physics_world.getBroadphase(), chassis_body['body'])
            vehicle.setCoordinateSystem(0, 1, 2)  # Right, Up, Forward
            
            # 添加车轮
            wheels = []
            for i, wheel_cfg in enumerate(wheel_config):
                wheel = vehicle.createWheel()
                wheel.setNode(wheel_cfg['node'])
                wheel.setChassisConnectionPointCs(wheel_cfg['connection_point'])
                wheel.setWheelDirectionCs(wheel_cfg['direction'])
                wheel.setWheelAxleCs(wheel_cfg['axle'])
                wheel.setWheelRadius(wheel_cfg['radius'])
                wheel.setMaxSuspensionTravelCm(wheel_cfg.get('suspension_travel', 500.0))
                wheel.setSuspensionStiffness(wheel_cfg.get('suspension_stiffness', 50.0))
                wheel.setWheelsDampingRelaxation(wheel_cfg.get('damping_relaxation', 2.3))
                wheel.setWheelsDampingCompression(wheel_cfg.get('damping_compression', 4.4))
                wheel.setFrictionSlip(wheel_cfg.get('friction_slip', 100.0))
                wheel.setRollInfluence(wheel_cfg.get('roll_influence', 0.1))
                wheel.setSteering(0.0)
                wheel.setEngineForce(0.0)
                wheel.setBrake(0.0)
                
                wheels.append(wheel)
                
            # 添加到物理世界
            self.physics_world.attachVehicle(vehicle)
            
            # 保存车辆信息
            vehicle_info = {
                'vehicle': vehicle,
                'chassis': chassis_body,
                'wheels': wheels,
                'wheel_config': wheel_config,
                'speed': 0.0,
                'steering': 0.0,
                'engine_force': 0.0,
                'brake_force': 0.0,
                'max_engine_force': 3000.0,
                'max_brake_force': 100.0,
                'max_steering': math.pi/4,
                'creation_time': self.world.globalClock.getFrameTime()
            }
            
            self.vehicles.append(vehicle_info)
            print("创建车辆")
            return vehicle_info
            
        except Exception as e:
            print(f"创建车辆时出错: {e}")
            return None
            
    def update_physics(self, time_delta):
        """
        更新物理模拟（增强版）
        """
        try:
            if not self.physics_enabled or not self.physics_world:
                return
                
            # 性能监控开始
            start_time = self.world.globalClock.getRealTime()
            
            # 累积时间
            self.time_accumulator += time_delta
            
            # 限制最大子步数以防止螺旋式死亡
            num_substeps = min(
                int(self.time_accumulator / self.internal_time_step),
                self.max_substeps
            )
            
            # 确保至少执行一次更新
            if num_substeps == 0 and self.time_accumulator > 0:
                num_substeps = 1
                
            # 执行子步长模拟
            for i in range(num_substeps):
                # 更新物理世界
                self.physics_world.doPhysics(self.internal_time_step)
                
                # 更新力和扭矩
                self._update_forces_and_torques(self.internal_time_step)
                
                # 更新粒子系统
                self._update_particles(self.internal_time_step)
                
                # 更新软体
                self._update_soft_bodies(self.internal_time_step)
                
            # 减少累积时间
            if num_substeps > 0:
                self.time_accumulator -= num_substeps * self.internal_time_step
                
            # 更新可视化节点位置
            self._update_visualization()
            
            # 更新车辆
            self._update_vehicles(time_delta)
            
            # 更新性能统计
            end_time = self.world.globalClock.getRealTime()
            update_time = end_time - start_time
            self.performance_stats['last_update_time'] = update_time
            self.performance_stats['update_count'] += 1
            
            # 计算平均更新时间
            if self.performance_stats['update_count'] > 1:
                alpha = 0.1
                self.performance_stats['avg_update_time'] = (
                    alpha * update_time + 
                    (1 - alpha) * self.performance_stats['avg_update_time']
                )
            else:
                self.performance_stats['avg_update_time'] = update_time
                
            # 更新最大更新时间
            if update_time > self.performance_stats['max_update_time']:
                self.performance_stats['max_update_time'] = update_time
                
        except Exception as e:
            print(f"更新物理模拟时出错: {e}")
            
    def _update_forces_and_torques(self, time_delta):
        """
        更新力和扭矩
        """
        try:
            current_time = self.world.globalClock.getFrameTime()
            
            for body_info in self.rigid_bodies:
                body_node = body_info['body']
                
                # 应用重力
                if body_info['mass'] > 0 and not body_node.isKinematic():
                    gravity_force = self.physics_params['gravity'] * body_info['mass']
                    body_node.applyCentralForce(gravity_force)
                    
                # 应用空气阻力
                velocity = body_node.getLinearVelocity()
                air_resistance = -velocity * self.physics_params['air_density'] * 0.1
                body_node.applyCentralForce(air_resistance)
                
                # 应用自定义力
                for force_info in body_info.get('forces', []):
                    if force_info['duration'] > 0:
                        body_node.applyCentralForce(force_info['force'])
                        force_info['duration'] -= time_delta
                    else:
                        body_info['forces'].remove(force_info)
                        
                # 应用自定义扭矩
                for torque_info in body_info.get('torques', []):
                    if torque_info['duration'] > 0:
                        body_node.applyTorque(torque_info['torque'])
                        torque_info['duration'] -= time_delta
                    else:
                        body_info['torques'].remove(torque_info)
                        
                # 更新状态
                body_info['velocity'] = body_node.getLinearVelocity()
                body_info['angular_velocity'] = body_node.getAngularVelocity()
                body_info['last_update'] = current_time
                
        except Exception as e:
            print(f"更新力和扭矩时出错: {e}")
            
    def _update_visualization(self):
        """
        更新可视化节点
        """
        try:
            for body_info in self.rigid_bodies:
                if body_info['original_node'] and not body_info['original_node'].isEmpty():
                    # 同步物理节点和可视化节点
                    body_node = body_info['body']
                    original_node = body_info['original_node']
                    
                    # 更新位置和旋转
                    original_node.setPos(body_node.getTransform().getPos())
                    original_node.setHpr(body_node.getTransform().getHpr())
                    
        except Exception as e:
            print(f"更新可视化时出错: {e}")
            
    def _update_particles(self, time_delta):
        """
        更新粒子系统
        """
        try:
            for particle_info in self.particles[:]:  # 使用切片复制
                particle_body = particle_info['body']
                
                # 更新粒子生命周期
                particle_info['age'] += time_delta
                if particle_info['age'] > particle_info['lifetime']:
                    self.remove_rigid_body(particle_info)
                    if particle_info in self.particles:
                        self.particles.remove(particle_info)
                    continue
                    
                # 应用粒子力（如重力、风力等）
                if particle_info.get('affected_by_gravity', True):
                    gravity_force = self.physics_params['gravity'] * particle_info['mass']
                    particle_body.applyCentralForce(gravity_force)
                    
                # 应用阻尼
                velocity = particle_body.getLinearVelocity()
                damping_force = -velocity * particle_info.get('damping', 0.1)
                particle_body.applyCentralForce(damping_force)
                
        except Exception as e:
            print(f"更新粒子时出错: {e}")
            
    def _update_soft_bodies(self, time_delta):
        """
        更新软体
        """
        try:
            for soft_body_info in self.soft_bodies:
                # 软体由Bullet物理引擎自动更新
                pass
                
        except Exception as e:
            print(f"更新软体时出错: {e}")
            
    def _update_vehicles(self, time_delta):
        """
        更新车辆
        """
        try:
            for vehicle_info in self.vehicles:
                vehicle = vehicle_info['vehicle']
                # 车辆更新由BulletVehicle自动处理
                
                # 更新车辆状态
                vehicle_info['speed'] = vehicle.getCurrentSpeedKmHour()
                
        except Exception as e:
            print(f"更新车辆时出错: {e}")
            
    def apply_force(self, body_info, force, duration=0.0, position=None):
        """
        对刚体施加力（增强版）
        """
        try:
            if duration <= 0:
                # 瞬时力
                if position is not None:
                    body_info['body'].applyForce(force, position)
                else:
                    body_info['body'].applyCentralForce(force)
            else:
                # 持续力
                force_info = {
                    'force': force,
                    'duration': duration,
                    'position': position
                }
                body_info.setdefault('forces', []).append(force_info)
                
            print(f"对刚体施加力: {force}")
            return True
            
        except Exception as e:
            print(f"施加力时出错: {e}")
            return False
            
    def apply_torque(self, body_info, torque, duration=0.0):
        """
        对刚体施加扭矩（增强版）
        """
        try:
            if duration <= 0:
                # 瞬时扭矩
                body_info['body'].applyTorque(torque)
            else:
                # 持续扭矩
                torque_info = {
                    'torque': torque,
                    'duration': duration
                }
                body_info.setdefault('torques', []).append(torque_info)
                
            print(f"对刚体施加扭矩: {torque}")
            return True
            
        except Exception as e:
            print(f"施加扭矩时出错: {e}")
            return False
            
    def ray_test(self, from_pos, to_pos):
        """
        射线检测（增强版）
        """
        try:
            if not self.physics_enabled or not self.physics_world:
                return None
                
            # 执行射线检测
            result = self.physics_world.rayTestClosest(from_pos, to_pos)
            
            if result.hasHit():
                hit_info = {
                    'hit_point': result.getHitPos(),
                    'hit_normal': result.getHitNormal(),
                    'hit_fraction': result.getHitFraction(),
                    'node': result.getNode(),
                    'body': result.getRigidBody(),
                    'distance': (to_pos - from_pos).length() * result.getHitFraction()
                }
                return hit_info
                
        except Exception as e:
            print(f"射线检测时出错: {e}")
            return None
            
    def sphere_test(self, position, radius):
        """
        球体检测（增强版）
        """
        try:
            if not self.physics_enabled or not self.physics_world:
                return []
                
            # 执行球体检测
            results = self.physics_world.sphereTest(position, radius)
            
            hit_list = []
            for result in results:
                hit_info = {
                    'hit_point': result.getHitPos(),
                    'hit_normal': result.getHitNormal(),
                    'node': result.getNode(),
                    'body': result.getRigidBody(),
                    'distance': (result.getHitPos() - position).length()
                }
                hit_list.append(hit_info)
                
            return hit_list
            
        except Exception as e:
            print(f"球体检测时出错: {e}")
            return []
            
    def set_gravity(self, gravity_vector):
        """
        设置重力
        """
        try:
            if self.physics_world:
                self.physics_world.setGravity(gravity_vector)
                self.physics_params['gravity'] = gravity_vector
                print(f"设置重力: {gravity_vector}")
                return True
                
        except Exception as e:
            print(f"设置重力时出错: {e}")
            return False
            
    def set_physics_enabled(self, enabled):
        """
        启用或禁用物理系统
        """
        self.physics_enabled = enabled
        print(f"物理系统已{'启用' if enabled else '禁用'}")
        
    def get_physics_stats(self):
        """
        获取物理统计信息（增强版）
        """
        stats = {
            'enabled': self.physics_enabled,
            'rigid_bodies': len(self.rigid_bodies),
            'constraints': len(self.constraints),
            'vehicles': len(self.vehicles),
            'particles': len(self.particles),
            'soft_bodies': len(self.soft_bodies),
            'materials': len(self.materials),
            'gravity': self.physics_params['gravity'],
            'substeps': self.substeps,
            'internal_time_step': self.internal_time_step,
            'time_accumulator': self.time_accumulator,
            'performance': {
                'last_update_time': self.performance_stats['last_update_time'],
                'avg_update_time': self.performance_stats['avg_update_time'],
                'max_update_time': self.performance_stats['max_update_time'],
                'update_count': self.performance_stats['update_count']
            }
        }
        
        # 统计材质分布
        material_stats = {}
        for body_info in self.rigid_bodies:
            material = body_info.get('material', 'default')
            if material in material_stats:
                material_stats[material] += 1
            else:
                material_stats[material] = 1
                
        stats['material_distribution'] = material_stats
        
        # 统计刚体类型分布
        body_type_stats = {}
        for body_info in self.rigid_bodies:
            body_type = body_info.get('type', 'unknown')
            if body_type in body_type_stats:
                body_type_stats[body_type] += 1
            else:
                body_type_stats[body_type] = 1
                
        stats['body_type_distribution'] = body_type_stats
        
        # 统计约束类型分布
        constraint_stats = {}
        for constraint_info in self.constraints:
            constraint_type = constraint_info.get('type', 'unknown')
            if constraint_type in constraint_stats:
                constraint_stats[constraint_type] += 1
            else:
                constraint_stats[constraint_type] = 1
                
        stats['constraint_distribution'] = constraint_stats
        
        return stats
        
    def save_physics_data(self, output_path):
        """
        保存物理数据（增强版）
        """
        try:
            # 收集物理数据
            physics_data = {
                'materials': {},
                'terrain_properties': {},
                'settings': {
                    'gravity': [self.physics_params['gravity'].x,
                              self.physics_params['gravity'].y,
                              self.physics_params['gravity'].z],
                    'substeps': self.substeps,
                    'internal_time_step': self.internal_time_step,
                    'air_density': self.physics_params['air_density']
                }
            }
            
            # 保存材质
            for name, material in self.materials.items():
                physics_data['materials'][name] = {
                    'friction': material['friction'],
                    'restitution': material['restitution'],
                    'rolling_friction': material['rolling_friction'],
                    'spinning_friction': material['spinning_friction'],
                    'density': material['density'],
                    'static_friction': material.get('static_friction', 0.6),
                    'dynamic_friction': material.get('dynamic_friction', 0.4),
                    'contact_cfm': material.get('contact_cfm', 0.0),
                    'contact_erp': material.get('contact_erp', 0.2)
                }
                
            # 保存地形物理属性
            for terrain_node, props in self.terrain_physics.items():
                # 这里需要序列化节点引用
                node_name = terrain_node.getName() if not terrain_node.isEmpty() else "unknown"
                physics_data['terrain_properties'][node_name] = {
                    'material': props['material']
                }
                
            # 确保输出目录存在
            output_dir = os.path.dirname(output_path)
            if not os.path.exists(output_dir):
                os.makedirs(output_dir)
                
            # 写入JSON文件
            with open(output_path, 'w', encoding='utf-8') as f:
                json.dump(physics_data, f, indent=2, ensure_ascii=False)
                
            print(f"物理数据保存成功: {output_path}")
            return True
            
        except Exception as e:
            print(f"保存物理数据时出错: {e}")
            return False
            
    def load_physics_data(self, input_path):
        """
        加载物理数据（增强版）
        """
        try:
            if not os.path.exists(input_path):
                print(f"物理数据文件不存在: {input_path}")
                return False
                
            # 读取JSON文件
            with open(input_path, 'r', encoding='utf-8') as f:
                physics_data = json.load(f)
                
            # 恢复设置
            if 'settings' in physics_data:
                settings = physics_data['settings']
                if 'gravity' in settings:
                    gravity = settings['gravity']
                    self.physics_params['gravity'] = Vec3(gravity[0], gravity[1], gravity[2])
                    self.set_gravity(self.physics_params['gravity'])
                if 'substeps' in settings:
                    self.substeps = settings['substeps']
                if 'internal_time_step' in settings:
                    self.internal_time_step = settings['internal_time_step']
                if 'air_density' in settings:
                    self.physics_params['air_density'] = settings['air_density']
                    
            # 加载材质
            for name, properties in physics_data.get('materials', {}).items():
                self.define_material(name, properties)
                
            # 应用地形物理属性（需要在地形加载后执行）
            self._pending_terrain_properties = physics_data.get('terrain_properties', {})
            
            print(f"物理数据加载成功: {input_path}")
            return True
            
        except Exception as e:
            print(f"加载物理数据时出错: {e}")
            return False
            
    def apply_pending_terrain_properties(self, terrain_manager):
        """
        应用待处理的地形物理属性
        """
        try:
            if not hasattr(self, '_pending_terrain_properties'):
                return
                
            for terrain_info in terrain_manager.terrains:
                terrain_node = terrain_info['node']
                node_name = terrain_node.getName()
                
                if node_name in self._pending_terrain_properties:
                    props = self._pending_terrain_properties[node_name]
                    material_name = props['material']
                    self.assign_material_to_terrain(terrain_info, material_name)
                    
            # 清除待处理属性
            delattr(self, '_pending_terrain_properties')
            
        except Exception as e:
            print(f"应用待处理地形物理属性时出错: {e}")
            
    def clear_all_physics(self):
        """
        清除所有物理对象
        """
        try:
            # 移除所有约束
            for constraint_info in self.constraints[:]:
                self.remove_constraint(constraint_info)
                
            # 移除所有刚体
            for body_info in self.rigid_bodies[:]:  # 使用切片复制列表
                self.remove_rigid_body(body_info)
                
            # 移除所有车辆
            self.vehicles = []
            
            # 移除所有粒子
            self.particles = []
            
            # 移除所有软体
            self.soft_bodies = []
            
            # 清空数据结构
            self.rigid_bodies = []
            self.constraints = []
            self.terrain_physics = {}
            
            print("清除所有物理对象")
            return True
            
        except Exception as e:
            print(f"清除所有物理对象时出错: {e}")
            return False
            
    def create_debris_effect(self, position, count=10, material_name=None, 
                           velocity_range=(5, 15), size_range=(0.05, 0.2)):
        """
        创建碎片效果（增强版）
        """
        try:
            debris_bodies = []
            
            for i in range(count):
                # 创建碎片节点
                from panda3d.core import CardMaker
                size = random.uniform(size_range[0], size_range[1])
                cm = CardMaker(f'debris_{i}')
                cm.setFrame(-size, size, -size, size)
                debris_node = self.world.render.attachNewNode(cm.generate())
                debris_node.setPos(position)
                
                # 添加随机偏移
                offset = Vec3(
                    random.uniform(-1, 1),
                    random.uniform(-1, 1),
                    random.uniform(0, 2)
                )
                debris_node.setPos(position + offset)
                
                # 创建碎片刚体
                body_info = self.create_rigid_body(
                    debris_node, 
                    mass=random.uniform(0.1, 1.0),
                    shape_type='box',
                    material_name=material_name
                )
                
                if body_info:
                    # 应加初始力
                    initial_force = Vec3(
                        random.uniform(-1, 1) * random.uniform(velocity_range[0], velocity_range[1]) * 100,
                        random.uniform(-1, 1) * random.uniform(velocity_range[0], velocity_range[1]) * 100,
                        random.uniform(0, 1) * random.uniform(velocity_range[0], velocity_range[1]) * 100
                    )
                    self.apply_force(body_info, initial_force, duration=0.0)
                    
                    # 设置角速度
                    body_info['body'].setAngularVelocity(Vec3(
                        random.uniform(-10, 10),
                        random.uniform(-10, 10),
                        random.uniform(-10, 10)
                    ))
                    
                    # 设置生命周期
                    body_info['lifetime'] = random.uniform(2.0, 5.0)
                    body_info['age'] = 0.0
                    body_info['affected_by_gravity'] = True
                    body_info['damping'] = random.uniform(0.01, 0.1)
                    
                    debris_bodies.append(body_info)
                    self.particles.append(body_info)  # 添加到粒子系统
                    
            print(f"创建碎片效果: {count} 个碎片")
            return debris_bodies
            
        except Exception as e:
            print(f"创建碎片效果时出错: {e}")
            return []
            
    def simulate_earthquake(self, intensity=1.0, duration=5.0, epicenter=None):
        """
        模拟地震效果（增强版）
        """
        try:
            if epicenter is None:
                epicenter = Point3(0, 0, 0)
                
            # 创建地震力
            earthquake_force = Vec3(
                random.uniform(-1, 1) * intensity * 1000,
                random.uniform(-1, 1) * intensity * 1000,
                random.uniform(0, 1) * intensity * 500
            )
            
            # 对所有地形相关的刚体施加力
            for body_info in self.rigid_bodies:
                if body_info.get('type') == 'terrain':
                    # 对地形施加震动
                    self.apply_force(body_info, earthquake_force, duration=0.1)
                    
            # 对附近的物体施加力
            for body_info in self.rigid_bodies:
                if body_info.get('type') == 'rigid_body' and body_info['mass'] > 0:
                    # 计算距离
                    body_pos = body_info['node'].getPos()
                    distance = (body_pos - epicenter).length()
                    
                    # 距离衰减
                    if distance < 50.0:  # 50米范围内受影响
                        force_multiplier = max(0.1, 1.0 - distance / 50.0)
                        local_force = earthquake_force * force_multiplier
                        self.apply_force(body_info, local_force, duration=0.1)
                        
            print(f"模拟地震效果: 强度 {intensity}, 持续时间 {duration}秒")
            return True
            
        except Exception as e:
            print(f"模拟地震效果时出错: {e}")
            return False
            
    def set_physics_substeps(self, substeps, internal_time_step=None):
        """
        设置物理子步长
        """
        self.substeps = max(1, substeps)
        if internal_time_step:
            self.internal_time_step = internal_time_step
        print(f"物理子步长设置: {self.substeps} 步, 时间步长: {self.internal_time_step}")
        
    def create_soft_body(self, node, mass=1.0, material_name=None):
        """
        创建软体（实验性）
        """
        try:
            if not self.physics_enabled:
                print("物理系统已禁用")
                return None
                
            # 创建软体节点
            soft_body_name = f'soft_body_{len(self.soft_bodies)}'
            soft_body_node = BulletSoftBodyNode(soft_body_name)
            
            # 设置软体属性
            cfg = soft_body_node.get_cfg()
            cfg.set_kVC(0.5)  # 体积保持系数
            cfg.set_kDF(0.5)  # 动态摩擦系数
            cfg.set_kDP(0.01)  # 压力系数
            cfg.set_kCHR(1.0)  # 完全保持形状
            cfg.set_kKHR(0.1)  # 运动学保持形状
            cfg.set_kSHR(1.0)  # 静态保持形状
            cfg.set_kAHR(0.7)  # 锚点保持形状
            
            # 设置材质属性
            if material_name and material_name in self.materials:
                material = self.materials[material_name]
                soft_body_node.setFriction(material['friction'])
                soft_body_node.setRestitution(material['restitution'])
                
            # 创建软体节点
            soft_body_np = node.attachNewNode(soft_body_node)
            soft_body_np.setPos(node.getPos())
            soft_body_np.setHpr(node.getHpr())
            
            # 添加到物理世界
            self.physics_world.attachSoftBody(soft_body_node)
            
            # 保存软体信息
            soft_body_info = {
                'node': soft_body_np,
                'body': soft_body_node,
                'original_node': node,
                'mass': mass,
                'material': material_name,
                'type': 'soft_body',
                'creation_time': self.world.globalClock.getFrameTime()
            }
            
            self.soft_bodies.append(soft_body_info)
            
            print(f"创建软体: {node.getName()} (质量: {mass})")
            return soft_body_info
            
        except Exception as e:
            print(f"创建软体时出错: {e}")
            return None
            
    def remove_soft_body(self, soft_body_info):
        """
        移除软体
        """
        try:
            if soft_body_info in self.soft_bodies:
                # 从物理世界移除
                self.physics_world.removeSoftBody(soft_body_info['body'])
                
                # 移除节点
                if soft_body_info['node'] and not soft_body_info['node'].isEmpty():
                    soft_body_info['node'].removeNode()
                    
                # 如果有原始节点，恢复其位置
                if soft_body_info['original_node'] and not soft_body_info['original_node'].isEmpty():
                    soft_body_info['original_node'].show()
                    
                # 从列表移除
                self.soft_bodies.remove(soft_body_info)
                
                print("移除软体")
                return True
                
        except Exception as e:
            print(f"移除软体时出错: {e}")
            return False
            
    def set_vehicle_control(self, vehicle_info, steering=0.0, engine_force=0.0, brake_force=0.0):
        """
        设置车辆控制（增强版）
        """
        try:
            vehicle = vehicle_info['vehicle']
            wheels = vehicle_info['wheels']
            
            # 限制控制值
            steering = max(-1.0, min(1.0, steering))
            engine_force = max(-1.0, min(1.0, engine_force))
            brake_force = max(0.0, min(1.0, brake_force))
            
            # 应用转向
            max_steering = vehicle_info['max_steering']
            for i, wheel in enumerate(wheels):
                # 前轮转向
                if i < 2:  # 假设前两个轮子是前轮
                    wheel.setSteering(steering * max_steering)
                    
            # 应用引擎力和刹车力
            max_engine_force = vehicle_info['max_engine_force']
            max_brake_force = vehicle_info['max_brake_force']
            
            for i, wheel in enumerate(wheels):
                # 后轮驱动
                if i >= 2:  # 假设后两个轮子是后轮
                    wheel.setEngineForce(engine_force * max_engine_force)
                wheel.setBrake(brake_force * max_brake_force)
                
            # 更新车辆信息
            vehicle_info['steering'] = steering
            vehicle_info['engine_force'] = engine_force
            vehicle_info['brake_force'] = brake_force
            
            return True
            
        except Exception as e:
            print(f"设置车辆控制时出错: {e}")
            return False