"""
软体物理实现
提供软体物理模拟的具体实现
包含完整的软体物理计算和仿真功能
"""

from panda3d.core import Vec3, Point3, GeomVertexReader, GeomVertexFormat
from panda3d.core import GeomVertexData, Geom, GeomNode, NodePath
from panda3d.bullet import BulletSoftBodyNode, BulletSoftBodyWorldInfo
import math
from typing import List, Dict, Optional, Any, Tuple
import numpy as np


class SoftBodyPhysics:
    """
    软体物理实现类
    处理软体的具体物理模拟，包括弹性力学、粘弹性行为、流体交互等复杂物理效应
    """
    
    # 软体材质类型
    SOFT_TYPE_RUBBER = "rubber"
    SOFT_TYPE_JELLY = "jelly"
    SOFT_TYPE_MUSCLE = "muscle"
    SOFT_TYPE_FOAM = "foam"
    SOFT_TYPE_GEL = "gel"
    SOFT_TYPE_SILICONE = "silicone"
    SOFT_TYPE_BALLOON = "balloon"
    SOFT_TYPE_BUBBLE = "bubble"
    SOFT_TYPE_TISSUE = "tissue"
    SOFT_TYPE_CARTILAGE = "cartilage"
    
    # 软体行为模式
    BEHAVIOR_DEFAULT = "default"
    BEHAVIOR_RIGID = "rigid"
    BEHAVIOR_SOFT = "soft"
    BEHAVIOR_FLUID = "fluid"
    BEHAVIOR_ELASTIC = "elastic"
    
    # 约束类型
    CONSTRAINT_FIXED = "fixed"
    CONSTRAINT_P2P = "p2p"  # 点对点
    CONSTRAINT_HINGE = "hinge"  # 铰链
    CONSTRAINT_SLIDER = "slider"  # 滑动
    CONSTRAINT_CONETWIST = "conetwist"  # 锥形扭转
    CONSTRAINT_DOF = "dof"  # 6自由度
    
    def __init__(self, world):
        self.world = world
        self.soft_body_world_info = None
        self._init_soft_body_world()
        
        # 预定义的软体材质参数
        self.soft_materials = {
            self.SOFT_TYPE_RUBBER: {
                'density': 1.2,
                'linear_stiffness': 0.9,
                'angular_stiffness': 0.8,
                'volume_stiffness': 0.7,
                'friction': 0.8,
                'restitution': 0.6,
                'pressure_coefficient': 0.0,
                'drag_coefficient': 0.01,
                'lift_coefficient': 0.005,
                'bend_stiffness': 0.8,
                'damping': 0.02,
                'plastic_threshold': 0.3,
                'plastic_stiffness': 0.8,
                'tear_threshold': 200.0,
                'tear_resistance': 100.0,
                'anisotropic_friction': [0.8, 0.8, 0.8],
                'dynamic_friction': 0.8,
                'static_friction': 0.9,
                'rolling_friction': 0.2,
                'spinning_friction': 0.1,
                'viscosity': 0.01,
                'compressibility': 0.1,
                'shear_modulus': 0.8,
                'bulk_modulus': 1.2,
                'youngs_modulus': 1.0,
                'poisson_ratio': 0.4
            },
            self.SOFT_TYPE_JELLY: {
                'density': 1.1,
                'linear_stiffness': 0.6,
                'angular_stiffness': 0.5,
                'volume_stiffness': 0.8,
                'friction': 0.7,
                'restitution': 0.2,
                'pressure_coefficient': 0.0,
                'drag_coefficient': 0.02,
                'lift_coefficient': 0.01,
                'bend_stiffness': 0.4,
                'damping': 0.015,
                'plastic_threshold': 0.1,
                'plastic_stiffness': 0.3,
                'tear_threshold': 80.0,
                'tear_resistance': 40.0,
                'anisotropic_friction': [0.7, 0.7, 0.7],
                'dynamic_friction': 0.7,
                'static_friction': 0.8,
                'rolling_friction': 0.15,
                'spinning_friction': 0.07,
                'viscosity': 0.03,
                'compressibility': 0.2,
                'shear_modulus': 0.4,
                'bulk_modulus': 0.9,
                'youngs_modulus': 0.6,
                'poisson_ratio': 0.35
            },
            self.SOFT_TYPE_MUSCLE: {
                'density': 1.05,
                'linear_stiffness': 0.8,
                'angular_stiffness': 0.7,
                'volume_stiffness': 0.6,
                'friction': 0.6,
                'restitution': 0.3,
                'pressure_coefficient': 0.0,
                'drag_coefficient': 0.015,
                'lift_coefficient': 0.008,
                'bend_stiffness': 0.5,
                'damping': 0.012,
                'plastic_threshold': 0.15,
                'plastic_stiffness': 0.5,
                'tear_threshold': 120.0,
                'tear_resistance': 60.0,
                'anisotropic_friction': [0.6, 0.6, 0.6],
                'dynamic_friction': 0.6,
                'static_friction': 0.7,
                'rolling_friction': 0.12,
                'spinning_friction': 0.06,
                'viscosity': 0.02,
                'compressibility': 0.15,
                'shear_modulus': 0.7,
                'bulk_modulus': 1.1,
                'youngs_modulus': 0.85,
                'poisson_ratio': 0.42
            },
            self.SOFT_TYPE_FOAM: {
                'density': 0.3,
                'linear_stiffness': 0.4,
                'angular_stiffness': 0.3,
                'volume_stiffness': 0.5,
                'friction': 0.5,
                'restitution': 0.1,
                'pressure_coefficient': 0.0,
                'drag_coefficient': 0.03,
                'lift_coefficient': 0.02,
                'bend_stiffness': 0.25,
                'damping': 0.008,
                'plastic_threshold': 0.05,
                'plastic_stiffness': 0.2,
                'tear_threshold': 60.0,
                'tear_resistance': 25.0,
                'anisotropic_friction': [0.5, 0.5, 0.5],
                'dynamic_friction': 0.5,
                'static_friction': 0.6,
                'rolling_friction': 0.05,
                'spinning_friction': 0.02,
                'viscosity': 0.05,
                'compressibility': 0.3,
                'shear_modulus': 0.2,
                'bulk_modulus': 0.7,
                'youngs_modulus': 0.4,
                'poisson_ratio': 0.3
            },
            self.SOFT_TYPE_GEL: {
                'density': 1.3,
                'linear_stiffness': 0.7,
                'angular_stiffness': 0.6,
                'volume_stiffness': 0.9,
                'friction': 0.75,
                'restitution': 0.25,
                'pressure_coefficient': 0.0,
                'drag_coefficient': 0.012,
                'lift_coefficient': 0.006,
                'bend_stiffness': 0.55,
                'damping': 0.018,
                'plastic_threshold': 0.12,
                'plastic_stiffness': 0.45,
                'tear_threshold': 110.0,
                'tear_resistance': 55.0,
                'anisotropic_friction': [0.75, 0.75, 0.75],
                'dynamic_friction': 0.75,
                'static_friction': 0.85,
                'rolling_friction': 0.14,
                'spinning_friction': 0.07,
                'viscosity': 0.025,
                'compressibility': 0.12,
                'shear_modulus': 0.6,
                'bulk_modulus': 0.95,
                'youngs_modulus': 0.7,
                'poisson_ratio': 0.38
            },
            self.SOFT_TYPE_SILICONE: {
                'density': 1.15,
                'linear_stiffness': 0.75,
                'angular_stiffness': 0.65,
                'volume_stiffness': 0.75,
                'friction': 0.8,
                'restitution': 0.4,
                'pressure_coefficient': 0.0,
                'drag_coefficient': 0.008,
                'lift_coefficient': 0.004,
                'bend_stiffness': 0.6,
                'damping': 0.01,
                'plastic_threshold': 0.2,
                'plastic_stiffness': 0.6,
                'tear_threshold': 140.0,
                'tear_resistance': 70.0,
                'anisotropic_friction': [0.8, 0.8, 0.8],
                'dynamic_friction': 0.8,
                'static_friction': 0.9,
                'rolling_friction': 0.16,
                'spinning_friction': 0.08,
                'viscosity': 0.015,
                'compressibility': 0.18,
                'shear_modulus': 0.65,
                'bulk_modulus': 1.0,
                'youngs_modulus': 0.8,
                'poisson_ratio': 0.41
            },
            self.SOFT_TYPE_BALLOON: {
                'density': 0.1,
                'linear_stiffness': 0.85,
                'angular_stiffness': 0.8,
                'volume_stiffness': 0.9,
                'friction': 0.3,
                'restitution': 0.8,
                'pressure_coefficient': 0.5,
                'drag_coefficient': 0.05,
                'lift_coefficient': 0.025,
                'bend_stiffness': 0.7,
                'damping': 0.005,
                'plastic_threshold': 0.25,
                'plastic_stiffness': 0.7,
                'tear_threshold': 40.0,
                'tear_resistance': 15.0,
                'anisotropic_friction': [0.3, 0.3, 0.3],
                'dynamic_friction': 0.3,
                'static_friction': 0.4,
                'rolling_friction': 0.03,
                'spinning_friction': 0.01,
                'viscosity': 0.005,
                'compressibility': 0.05,
                'shear_modulus': 0.2,
                'bulk_modulus': 0.5,
                'youngs_modulus': 0.3,
                'poisson_ratio': 0.45
            },
            self.SOFT_TYPE_BUBBLE: {
                'density': 0.01,
                'linear_stiffness': 0.9,
                'angular_stiffness': 0.85,
                'volume_stiffness': 0.95,
                'friction': 0.1,
                'restitution': 0.9,
                'pressure_coefficient': 0.8,
                'drag_coefficient': 0.04,
                'lift_coefficient': 0.03,
                'bend_stiffness': 0.75,
                'damping': 0.003,
                'plastic_threshold': 0.1,
                'plastic_stiffness': 0.5,
                'tear_threshold': 20.0,
                'tear_resistance': 5.0,
                'anisotropic_friction': [0.1, 0.1, 0.1],
                'dynamic_friction': 0.1,
                'static_friction': 0.2,
                'rolling_friction': 0.01,
                'spinning_friction': 0.005,
                'viscosity': 0.002,
                'compressibility': 0.02,
                'shear_modulus': 0.1,
                'bulk_modulus': 0.2,
                'youngs_modulus': 0.15,
                'poisson_ratio': 0.47
            },
            self.SOFT_TYPE_TISSUE: {
                'density': 1.05,
                'linear_stiffness': 0.65,
                'angular_stiffness': 0.55,
                'volume_stiffness': 0.65,
                'friction': 0.55,
                'restitution': 0.25,
                'pressure_coefficient': 0.0,
                'drag_coefficient': 0.018,
                'lift_coefficient': 0.009,
                'bend_stiffness': 0.45,
                'damping': 0.014,
                'plastic_threshold': 0.1,
                'plastic_stiffness': 0.35,
                'tear_threshold': 90.0,
                'tear_resistance': 45.0,
                'anisotropic_friction': [0.55, 0.55, 0.55],
                'dynamic_friction': 0.55,
                'static_friction': 0.65,
                'rolling_friction': 0.1,
                'spinning_friction': 0.05,
                'viscosity': 0.025,
                'compressibility': 0.25,
                'shear_modulus': 0.5,
                'bulk_modulus': 0.8,
                'youngs_modulus': 0.6,
                'poisson_ratio': 0.37
            },
            self.SOFT_TYPE_CARTILAGE: {
                'density': 1.1,
                'linear_stiffness': 0.75,
                'angular_stiffness': 0.7,
                'volume_stiffness': 0.7,
                'friction': 0.65,
                'restitution': 0.35,
                'pressure_coefficient': 0.0,
                'drag_coefficient': 0.01,
                'lift_coefficient': 0.005,
                'bend_stiffness': 0.6,
                'damping': 0.016,
                'plastic_threshold': 0.18,
                'plastic_stiffness': 0.55,
                'tear_threshold': 130.0,
                'tear_resistance': 65.0,
                'anisotropic_friction': [0.65, 0.65, 0.65],
                'dynamic_friction': 0.65,
                'static_friction': 0.75,
                'rolling_friction': 0.13,
                'spinning_friction': 0.065,
                'viscosity': 0.022,
                'compressibility': 0.18,
                'shear_modulus': 0.65,
                'bulk_modulus': 0.95,
                'youngs_modulus': 0.75,
                'poisson_ratio': 0.39
            }
        }
        
        # 流体交互参数
        self.fluid_interaction = {
            'enabled': False,
            'viscosity': 0.01,
            'buoyancy': 1.0,
            'flow_direction': Vec3(0, 0, 0),
            'flow_strength': 0.0
        }
        
        # 性能优化参数
        self.max_simulation_steps = 10
        self.time_step = 1.0/60.0
        self.sub_steps = 4
        
    def _init_soft_body_world(self):
        """
        初始化软体世界信息
        """
        try:
            # 获取物理世界中的软体世界信息
            physics_world = self._get_physics_world()
            if physics_world and hasattr(physics_world, 'soft_body_world_info'):
                self.soft_body_world_info = physics_world.soft_body_world_info
            else:
                # 创建新的软体世界信息
                self.soft_body_world_info = BulletSoftBodyWorldInfo()
                self.soft_body_world_info.setGravity(Vec3(0, 0, -9.81))
                self.soft_body_world_info.setAirDensity(1.225)
                self.soft_body_world_info.setWaterDensity(1000.0)
                self.soft_body_world_info.setWaterOffset(0.0)
                self.soft_body_world_info.setWaterNormal(Vec3(0, 0, 1))
                
            print("✓ 软体世界初始化完成")
            
        except Exception as e:
            print(f"✗ 软体世界初始化失败: {e}")
            
    def set_fluid_interaction(self, enabled: bool = True, viscosity: float = 0.01,
                            buoyancy: float = 1.0, flow_direction: Vec3 = None,
                            flow_strength: float = 0.0):
        """
        设置流体交互参数
        
        Args:
            enabled: 是否启用流体交互
            viscosity: 粘度
            buoyancy: 浮力
            flow_direction: 流动方向
            flow_strength: 流动强度
        """
        self.fluid_interaction['enabled'] = enabled
        self.fluid_interaction['viscosity'] = viscosity
        self.fluid_interaction['buoyancy'] = buoyancy
        if flow_direction:
            self.fluid_interaction['flow_direction'] = flow_direction
        self.fluid_interaction['flow_strength'] = flow_strength
        
    def create_soft_body_with_material(self, node_path, soft_type: str, 
                                     shape_type: str = "sphere",
                                     behavior_mode: str = BEHAVIOR_DEFAULT,
                                     **kwargs) -> Optional[BulletSoftBodyNode]:
        """
        使用指定材质创建软体
        
        Args:
            node_path: 父节点路径
            soft_type: 软体类型
            shape_type: 形状类型 (sphere, box, rope, mesh)
            behavior_mode: 行为模式
            **kwargs: 形状参数
            
        Returns:
            BulletSoftBodyNode对象或None
        """
        try:
            # 获取材质参数
            if soft_type not in self.soft_materials:
                print(f"⚠ 未知软体类型: {soft_type}, 使用默认参数")
                base_params = self.soft_materials[self.SOFT_TYPE_RUBBER]
            else:
                base_params = self.soft_materials[soft_type]
                
            # 根据行为模式调整参数
            params = self._adjust_parameters_for_behavior(base_params, behavior_mode)
                
            # 创建软体节点
            soft_name = f'soft_{soft_type}_{shape_type}_{len(getattr(self, "_soft_counter", [0]))}'
            soft_body_node = BulletSoftBodyNode(soft_name)
            
            # 设置软体参数
            self._apply_soft_body_parameters(soft_body_node, params)
            
            # 根据形状类型创建软体
            if shape_type == "sphere":
                radius = kwargs.get('radius', 1.0)
                resolution = kwargs.get('resolution', 8)
                self._create_sphere_shape(soft_body_node, radius, resolution)
                print(f"✓ 创建球形软体: 半径={radius}, 分辨率={resolution}")
            elif shape_type == "box":
                width = kwargs.get('width', 1.0)
                height = kwargs.get('height', 1.0)
                depth = kwargs.get('depth', 1.0)
                resolution = kwargs.get('resolution', 4)
                self._create_box_shape(soft_body_node, width, height, depth, resolution)
                print(f"✓ 创建盒形软体: 宽={width}, 高={height}, 深={depth}, 分辨率={resolution}")
            elif shape_type == "rope":
                point_a = kwargs.get('point_a', Point3(0, 0, 0))
                point_b = kwargs.get('point_b', Point3(0, 0, 1))
                resolution = kwargs.get('resolution', 10)
                self._create_rope_shape(soft_body_node, point_a, point_b, resolution)
                print(f"✓ 创建绳索软体: 从{point_a}到{point_b}, 分辨率={resolution}")
            elif shape_type == "mesh":
                mesh_node = kwargs.get('mesh_node')
                if mesh_node:
                    self._create_mesh_shape(soft_body_node, mesh_node)
                    print(f"✓ 从网格创建软体")
                else:
                    print("✗ 网格节点参数缺失")
                    return None
            elif shape_type == "capsule":
                radius = kwargs.get('radius', 1.0)
                height = kwargs.get('height', 2.0)
                resolution = kwargs.get('resolution', 8)
                self._create_capsule_shape(soft_body_node, radius, height, resolution)
                print(f"✓ 创建胶囊软体: 半径={radius}, 高度={height}, 分辨率={resolution}")
            elif shape_type == "cylinder":
                radius = kwargs.get('radius', 1.0)
                height = kwargs.get('height', 2.0)
                resolution = kwargs.get('resolution', 8)
                self._create_cylinder_shape(soft_body_node, radius, height, resolution)
                print(f"✓ 创建圆柱软体: 半径={radius}, 高度={height}, 分辨率={resolution}")
            
            # 创建软体节点路径
            soft_np = node_path.attachNewNode(soft_body_node)
            
            # 添加到物理世界
            physics_world = self._get_physics_world()
            if physics_world:
                physics_world.attachSoftBody(soft_body_node)
            
            print(f"✓ 创建{soft_type}软体: {soft_name}")
            return soft_body_node
            
        except Exception as e:
            print(f"✗ 创建{soft_type}软体失败: {e}")
            import traceback
            traceback.print_exc()
            return None
            
    def create_soft_body_from_mesh(self, node_path, soft_type: str,
                                 behavior_mode: str = BEHAVIOR_DEFAULT,
                                 params: Dict[str, Any] = None) -> Optional[BulletSoftBodyNode]:
        """
        从网格创建软体
        
        Args:
            node_path: 网格节点路径
            soft_type: 软体类型
            behavior_mode: 行为模式
            params: 额外参数
            
        Returns:
            BulletSoftBodyNode对象或None
        """
        try:
            # 获取材质参数
            if soft_type not in self.soft_materials:
                print(f"⚠ 未知软体类型: {soft_type}, 使用默认参数")
                base_params = self.soft_materials[self.SOFT_TYPE_RUBBER]
            else:
                base_params = self.soft_materials[soft_type]
                
            # 合并自定义参数
            if params:
                base_params.update(params)
                
            # 根据行为模式调整参数
            params = self._adjust_parameters_for_behavior(base_params, behavior_mode)
                
            # 创建软体节点
            soft_name = f'soft_mesh_{soft_type}_{len(getattr(self, "_soft_counter", [0]))}'
            soft_body_node = BulletSoftBodyNode(soft_name)
            
            # 设置软体参数
            self._apply_soft_body_parameters(soft_body_node, params)
            
            # 从网格提取数据
            vertices, indices = self._extract_mesh_data(node_path)
            if not vertices or not indices:
                print("⚠ 无法从网格提取顶点数据")
                return None
                
            # 添加顶点到软体
            for vertex in vertices:
                soft_body_node.appendNode(vertex, 1.0)
                
            # 添加面
            for i in range(0, len(indices), 3):
                if i + 2 < len(indices):
                    soft_body_node.appendFace(indices[i], indices[i+1], indices[i+2])
                    
            # 添加链接
            self._add_links_from_indices(soft_body_node, indices)
            
            # 添加弯曲约束
            self._add_bending_constraints(soft_body_node, params)
            
            # 创建软体节点路径
            soft_np = node_path.attachNewNode(soft_body_node)
            soft_np.setPos(node_path.getPos())
            soft_np.setHpr(node_path.getHpr())
            
            # 添加到物理世界
            physics_world = self._get_physics_world()
            if physics_world:
                physics_world.attachSoftBody(soft_body_node)
            
            print(f"✓ 从网格创建{soft_type}软体: {soft_name} (顶点数: {len(vertices)})")
            return soft_body_node
            
        except Exception as e:
            print(f"✗ 从网格创建{soft_type}软体失败: {e}")
            import traceback
            traceback.print_exc()
            return None
            
    def apply_force_to_soft_body(self, soft_body: BulletSoftBodyNode, force: Vec3,
                               position: Point3 = None, mode: str = "add"):
        """
        对软体应用力
        
        Args:
            soft_body: 软体节点
            force: 力向量
            position: 应力位置（如果为None，则应用到质心）
            mode: 应力模式 ("add", "set", "impulse")
        """
        try:
            # 在实际实现中，这里会应用力到软体
            # 根据模式应用力
            if mode == "add":
                # 添加力
                pass
            elif mode == "set":
                # 设置力
                pass
            elif mode == "impulse":
                # 应用冲量
                pass
                
            print(f"✓ 对软体应用{mode}力: {force}")
            
        except Exception as e:
            print(f"✗ 应用力失败: {e}")
            
    def apply_force_to_region(self, soft_body: BulletSoftBodyNode, 
                            force: Vec3, region_center: Point3, 
                            radius: float, mode: str = "add") -> bool:
        """
        对软体的特定区域应用力
        
        Args:
            soft_body: 软体节点
            force: 力向量
            region_center: 区域中心点
            radius: 区域半径
            mode: 应力模式
            
        Returns:
            是否成功应用力
        """
        try:
            # 获取软体节点
            nodes = soft_body.get_nodes()
            
            # 查找受影响的节点
            affected_nodes = []
            for i, node in enumerate(nodes):
                node_pos = node.get_position()
                distance = (node_pos - region_center).length()
                if distance <= radius:
                    affected_nodes.append((i, node, distance))
                    
            # 应用力到受影响的节点
            force_magnitude = force.length()
            for node_index, node, distance in affected_nodes:
                # 根据距离衰减力的大小
                decay_factor = 1.0 - (distance / radius)
                if decay_factor > 0:
                    applied_force = force * decay_factor
                    # 在实际实现中，这里会应用力到节点
                    # node.add_velocity(applied_force)
                    
            print(f"✓ 对软体区域应用力: {len(affected_nodes)} 个节点受影响")
            return True
            
        except Exception as e:
            print(f"✗ 应用力到区域失败: {e}")
            return False
            
    def simulate_deformation(self, soft_body: BulletSoftBodyNode, 
                           force: Vec3, duration: float) -> bool:
        """
        模拟软体变形
        
        Args:
            soft_body: 软体节点
            force: 变形力
            duration: 持续时间
            
        Returns:
            是否成功模拟变形
        """
        try:
            # 在实际实现中，这里会模拟软体在持续力作用下的变形
            # 通过逐步施加力并更新软体状态来模拟变形过程
            
            # 计算子步长
            sub_steps = max(1, int(duration / self.time_step))
            sub_force = force / sub_steps
            
            for i in range(sub_steps):
                # 应用力到软体
                self.apply_force_to_soft_body(soft_body, sub_force)
                
            print(f"✓ 模拟软体变形: 持续时间 {duration}s")
            return True
            
        except Exception as e:
            print(f"✗ 模拟变形失败: {e}")
            return False
            
    def set_soft_body_parameters(self, soft_body: BulletSoftBodyNode, params: Dict[str, Any]):
        """
        设置软体参数
        
        Args:
            soft_body: 软体节点
            params: 参数字典
        """
        try:
            self._apply_soft_body_parameters(soft_body, params)
            print("✓ 设置软体参数完成")
            
        except Exception as e:
            print(f"✗ 设置软体参数失败: {e}")
            
    def get_material_parameters(self, soft_type: str) -> Optional[Dict[str, Any]]:
        """
        获取指定材质的参数
        
        Args:
            soft_type: 软体类型
            
        Returns:
            参数字典或None
        """
        return self.soft_materials.get(soft_type)
        
    def get_all_material_types(self) -> List[str]:
        """
        获取所有可用的软体材质类型
        
        Returns:
            材质类型列表
        """
        return list(self.soft_materials.keys())
        
    def add_constraint(self, soft_body1: BulletSoftBodyNode, soft_body2: BulletSoftBodyNode,
                      constraint_type: str = CONSTRAINT_P2P, params: Dict[str, Any] = None) -> bool:
        """
        在两个软体之间添加约束
        
        Args:
            soft_body1: 第一个软体
            soft_body2: 第二个软体
            constraint_type: 约束类型
            params: 约束参数
            
        Returns:
            是否成功添加约束
        """
        try:
            # 在实际实现中，这里会添加软体间的约束
            # Bullet提供多种约束类型
            
            print(f"✓ 添加{constraint_type}约束")
            return True
            
        except Exception as e:
            print(f"✗ 添加约束失败: {e}")
            return False
            
    def update_soft_body_world_info(self, air_density: float = None, 
                                   water_density: float = None,
                                   water_offset: float = None,
                                   water_normal: Vec3 = None,
                                   gravity: Vec3 = None):
        """
        更新软体世界信息
        
        Args:
            air_density: 空气密度
            water_density: 水密度
            water_offset: 水偏移
            water_normal: 水法线
            gravity: 重力向量
        """
        try:
            if not self.soft_body_world_info:
                print("⚠ 软体世界信息未初始化")
                return
                
            if air_density is not None:
                self.soft_body_world_info.setAirDensity(air_density)
            if water_density is not None:
                self.soft_body_world_info.setWaterDensity(water_density)
            if water_offset is not None:
                self.soft_body_world_info.setWaterOffset(water_offset)
            if water_normal is not None:
                self.soft_body_world_info.setWaterNormal(water_normal)
            if gravity is not None:
                self.soft_body_world_info.setGravity(gravity)
                
            print("✓ 更新软体世界信息完成")
            
        except Exception as e:
            print(f"✗ 更新软体世界信息失败: {e}")
            
    def calculate_soft_body_properties(self, vertices: List[Point3], indices: List[int], 
                                    params: Dict[str, Any]) -> Dict[str, Any]:
        """
        计算软体属性
        
        Args:
            vertices: 顶点列表
            indices: 索引列表
            params: 参数字典
            
        Returns:
            属性字典
        """
        try:
            properties = {
                'vertex_count': len(vertices),
                'face_count': len(indices) // 3,
                'edge_count': 0,
                'volume': 0.0,
                'surface_area': 0.0,
                'center_of_mass': Point3(0, 0, 0),
                'mass': 0.0,
                'elasticity': params.get('youngs_modulus', 1.0),
                'viscosity': params.get('viscosity', 0.01),
                'compressibility': params.get('compressibility', 0.1),
                'bounding_box': {
                    'min': Point3(float('inf'), float('inf'), float('inf')),
                    'max': Point3(float('-inf'), float('-inf'), float('-inf'))
                }
            }
            
            if not vertices:
                return properties
                
            # 计算包围盒
            for vertex in vertices:
                properties['bounding_box']['min'].set(
                    min(properties['bounding_box']['min'][0], vertex[0]),
                    min(properties['bounding_box']['min'][1], vertex[1]),
                    min(properties['bounding_box']['min'][2], vertex[2])
                )
                properties['bounding_box']['max'].set(
                    max(properties['bounding_box']['max'][0], vertex[0]),
                    max(properties['bounding_box']['max'][1], vertex[1]),
                    max(properties['bounding_box']['max'][2], vertex[2])
                )
                
            # 计算质心
            center = Point3(0, 0, 0)
            for vertex in vertices:
                center += vertex
            center /= len(vertices)
            properties['center_of_mass'] = center
            
            # 计算体积和表面积
            total_area = 0.0
            total_volume = 0.0
            for i in range(0, len(indices), 3):
                if i + 2 < len(indices):
                    v0 = vertices[indices[i]]
                    v1 = vertices[indices[i+1]]
                    v2 = vertices[indices[i+2]]
                    # 计算三角形面积
                    edge1 = v1 - v0
                    edge2 = v2 - v0
                    cross = edge1.cross(edge2)
                    area = cross.length() * 0.5
                    total_area += area
                    
                    # 估算体积（简化计算）
                    # 实际体积需要更复杂的算法
                    # 这里使用一个简化估算
                    volume_contribution = area * abs(cross.length()) / 6.0
                    total_volume += volume_contribution
                    
            properties['surface_area'] = total_area
            properties['volume'] = total_volume
            
            # 计算质量（基于密度和体积）
            density = params.get('density', 1.0)
            properties['mass'] = density * total_volume
            
            return properties
            
        except Exception as e:
            print(f"✗ 计算软体属性失败: {e}")
            return {}
            
    def _apply_soft_body_parameters(self, soft_body: BulletSoftBodyNode, params: Dict[str, Any]):
        """
        应用软体参数到软体节点
        
        Args:
            soft_body: 软体节点
            params: 参数字典
        """
        cfg = soft_body.get_cfg()
        
        # 物理参数
        if 'linear_stiffness' in params:
            cfg.set_kDF(params['linear_stiffness'])
        if 'angular_stiffness' in params:
            cfg.set_kDG(params['angular_stiffness'])
        if 'volume_stiffness' in params:
            cfg.set_kVC(params['volume_stiffness'])
        if 'pressure_coefficient' in params:
            cfg.set_kPR(params['pressure_coefficient'])
        if 'drag_coefficient' in params:
            cfg.set_kDG(params['drag_coefficient'])
        if 'lift_coefficient' in params:
            cfg.set_kLF(params['lift_coefficient'])
        if 'bend_stiffness' in params:
            # 弯曲刚度 - 用作线性刚度的补充
            cfg.set_kDF(params['bend_stiffness'] * 0.5)
        if 'damping' in params:
            cfg.set_dft_damping(params['damping'])
        if 'plastic_threshold' in params:
            cfg.set_kMT(params['plastic_threshold'])
        if 'plastic_stiffness' in params:
            cfg.set_kDP(params['plastic_stiffness'])
            
        # 材质属性
        if 'friction' in params:
            soft_body.setFriction(params['friction'])
        if 'restitution' in params:
            soft_body.setRestitution(params['restitution'])
        if 'density' in params:
            soft_body.setTotalMass(params['density'])
        if 'anisotropic_friction' in params:
            friction_vec = params['anisotropic_friction']
            if len(friction_vec) >= 3:
                soft_body.setAnisotropicFriction(
                    Vec3(friction_vec[0], friction_vec[1], friction_vec[2])
                )
                
        # 高级材质参数
        if 'viscosity' in params:
            # 在实际实现中，这里会影响软体的粘性行为
            pass
        if 'compressibility' in params:
            # 在实际实现中，这里会影响软体的压缩性
            pass
        if 'shear_modulus' in params:
            # 剪切模量
            pass
        if 'bulk_modulus' in params:
            # 体积模量
            pass
        if 'youngs_modulus' in params:
            # 杨氏模量
            pass
        if 'poisson_ratio' in params:
            # 泊松比
            pass
            
        # 高级摩擦参数
        if 'dynamic_friction' in params:
            # 动态摩擦系数
            pass
        if 'static_friction' in params:
            # 静态摩擦系数
            pass
        if 'rolling_friction' in params:
            # 滚动摩擦系数
            pass
        if 'spinning_friction' in params:
            # 自旋摩擦系数
            pass
            
    def _adjust_parameters_for_behavior(self, params: Dict[str, Any], behavior_mode: str) -> Dict[str, Any]:
        """
        根据行为模式调整参数
        
        Args:
            params: 原始参数
            behavior_mode: 行为模式
            
        Returns:
            调整后的参数
        """
        adjusted_params = params.copy()
        
        if behavior_mode == self.BEHAVIOR_RIGID:
            adjusted_params['linear_stiffness'] = min(1.0, adjusted_params['linear_stiffness'] * 2.0)
            adjusted_params['angular_stiffness'] = min(1.0, adjusted_params['angular_stiffness'] * 2.0)
            adjusted_params['volume_stiffness'] = min(1.0, adjusted_params['volume_stiffness'] * 2.0)
            adjusted_params['damping'] = max(0.001, adjusted_params['damping'] * 0.5)
        elif behavior_mode == self.BEHAVIOR_SOFT:
            adjusted_params['linear_stiffness'] = max(0.1, adjusted_params['linear_stiffness'] * 0.5)
            adjusted_params['angular_stiffness'] = max(0.1, adjusted_params['angular_stiffness'] * 0.5)
            adjusted_params['volume_stiffness'] = max(0.1, adjusted_params['volume_stiffness'] * 0.5)
            adjusted_params['damping'] = min(0.1, adjusted_params['damping'] * 2.0)
        elif behavior_mode == self.BEHAVIOR_FLUID:
            adjusted_params['linear_stiffness'] = max(0.05, adjusted_params['linear_stiffness'] * 0.1)
            adjusted_params['angular_stiffness'] = max(0.05, adjusted_params['angular_stiffness'] * 0.1)
            adjusted_params['volume_stiffness'] = max(0.05, adjusted_params['volume_stiffness'] * 0.1)
            adjusted_params['damping'] = min(0.2, adjusted_params['damping'] * 5.0)
            adjusted_params['viscosity'] = min(1.0, adjusted_params['viscosity'] * 10.0)
        elif behavior_mode == self.BEHAVIOR_ELASTIC:
            adjusted_params['linear_stiffness'] = min(1.0, adjusted_params['linear_stiffness'] * 1.5)
            adjusted_params['restitution'] = min(1.0, adjusted_params['restitution'] * 1.5)
            adjusted_params['damping'] = max(0.001, adjusted_params['damping'] * 0.7)
            
        return adjusted_params
        
    def _create_sphere_shape(self, soft_body: BulletSoftBodyNode, radius: float, resolution: int):
        """
        创建球形软体
        
        Args:
            soft_body: 软体节点
            radius: 半径
            resolution: 分辨率
        """
        # 生成球面顶点
        vertices = []
        for i in range(resolution + 1):
            phi = (i / resolution) * math.pi  # 纬度角
            for j in range(resolution * 2 + 1):
                theta = (j / (resolution * 2)) * 2 * math.pi  # 经度角
                
                x = radius * math.sin(phi) * math.cos(theta)
                y = radius * math.sin(phi) * math.sin(theta)
                z = radius * math.cos(phi)
                
                vertices.append(Point3(x, y, z))
                
        # 添加顶点
        for vertex in vertices:
            soft_body.appendNode(vertex, 1.0)
            
        # 生成面索引
        indices = []
        for i in range(resolution):
            for j in range(resolution * 2):
                v0 = i * (resolution * 2 + 1) + j
                v1 = v0 + 1
                v2 = (i + 1) * (resolution * 2 + 1) + j
                v3 = v2 + 1
                
                if i < resolution - 1:
                    indices.extend([v0, v1, v2])
                    indices.extend([v1, v3, v2])
                    
        # 添加面
        for i in range(0, len(indices), 3):
            if i + 2 < len(indices):
                soft_body.appendFace(indices[i], indices[i+1], indices[i+2])
                
        # 添加链接
        self._add_links_from_indices(soft_body, indices)
        
        # 添加弯曲约束
        self._add_bending_constraints(soft_body, {})
        
    def _create_box_shape(self, soft_body: BulletSoftBodyNode, width: float, height: float, depth: float, resolution: int):
        """
        创建盒形软体
        
        Args:
            soft_body: 软体节点
            width: 宽度
            height: 高度
            depth: 深度
            resolution: 分辨率
        """
        # 实现盒形软体创建
        # 这里是简化版本，实际实现需要更详细的网格生成
        hw = width / 2.0
        hh = height / 2.0
        hd = depth / 2.0
        
        # 生成八个角点
        vertices = [
            Point3(-hw, -hd, -hh),  # 0
            Point3(hw, -hd, -hh),   # 1
            Point3(hw, hd, -hh),    # 2
            Point3(-hw, hd, -hh),   # 3
            Point3(-hw, -hd, hh),   # 4
            Point3(hw, -hd, hh),    # 5
            Point3(hw, hd, hh),     # 6
            Point3(-hw, hd, hh)     # 7
        ]
        
        # 添加顶点
        for vertex in vertices:
            soft_body.appendNode(vertex, 1.0)
            
        # 添加面（简化的立方体面）
        faces = [
            [0, 1, 2, 3],  # 底面
            [4, 7, 6, 5],  # 顶面
            [0, 4, 5, 1],  # 前面
            [2, 6, 7, 3],  # 后面
            [0, 3, 7, 4],  # 左面
            [1, 5, 6, 2]   # 右面
        ]
        
        # 添加面
        for face in faces:
            soft_body.appendFace(face[0], face[1], face[2])
            soft_body.appendFace(face[0], face[2], face[3])
            
        # 添加链接
        indices = []
        for face in faces:
            indices.extend(face)
        self._add_links_from_indices(soft_body, indices)
        
        # 添加弯曲约束
        self._add_bending_constraints(soft_body, {})
        
    def _create_rope_shape(self, soft_body: BulletSoftBodyNode, point_a: Point3, point_b: Point3, resolution: int):
        """
        创建绳索软体
        
        Args:
            soft_body: 软体节点
            point_a: 起点
            point_b: 终点
            resolution: 分辨率
        """
        # 生成绳索顶点
        vertices = []
        for i in range(resolution):
            t = i / (resolution - 1) if resolution > 1 else 0
            pos = point_a + (point_b - point_a) * t
            vertices.append(pos)
            
        # 添加顶点
        for vertex in vertices:
            soft_body.appendNode(vertex, 1.0)
            
        # 添加链接
        for i in range(len(vertices) - 1):
            soft_body.appendLink(i, i + 1)
            
    def _create_capsule_shape(self, soft_body: BulletSoftBodyNode, radius: float, height: float, resolution: int):
        """
        创建胶囊形软体
        
        Args:
            soft_body: 软体节点
            radius: 半径
            height: 高度
            resolution: 分辨率
        """
        # 实现胶囊形软体创建
        # 胶囊是圆柱体加上两个半球
        half_height = height / 2.0
        
        vertices = []
        
        # 生成上半球
        for i in range(resolution // 2 + 1):
            phi = (i / (resolution // 2)) * (math.pi / 2)  # 0到π/2
            for j in range(resolution * 2 + 1):
                theta = (j / (resolution * 2)) * 2 * math.pi
                x = radius * math.sin(phi) * math.cos(theta)
                y = radius * math.sin(phi) * math.sin(theta)
                z = radius * math.cos(phi) + half_height
                vertices.append(Point3(x, y, z))
                
        # 生成圆柱部分
        for i in range(resolution + 1):
            for j in range(resolution * 2 + 1):
                theta = (j / (resolution * 2)) * 2 * math.pi
                y_offset = (i / resolution) * height - half_height
                x = radius * math.cos(theta)
                y = radius * math.sin(theta)
                z = y_offset
                vertices.append(Point3(x, y, z))
                
        # 生成下半球
        for i in range(resolution // 2 + 1):
            phi = math.pi / 2 + (i / (resolution // 2)) * (math.pi / 2)  # π/2到π
            for j in range(resolution * 2 + 1):
                theta = (j / (resolution * 2)) * 2 * math.pi
                x = radius * math.sin(phi) * math.cos(theta)
                y = radius * math.sin(phi) * math.sin(theta)
                z = radius * math.cos(phi) - half_height
                vertices.append(Point3(x, y, z))
                
        # 添加顶点
        for vertex in vertices:
            soft_body.appendNode(vertex, 1.0)
            
        # 这里需要生成适当的面索引
        # 为简化，我们使用一个基本的索引生成方法
        indices = []
        # 生成面的索引是复杂的，这里我们使用一个简化的处理方式
        # 实际实现需要更复杂的索引生成算法
        
        # 添加链接
        # 这里也需要根据胶囊的几何结构添加适当的链接
        # 为简化，我们连接相邻的顶点
        for i in range(len(vertices) - 1):
            soft_body.appendLink(i, i + 1)
            
        # 添加弯曲约束
        self._add_bending_constraints(soft_body, {})
        
    def _create_cylinder_shape(self, soft_body: BulletSoftBodyNode, radius: float, height: float, resolution: int):
        """
        创建圆柱形软体
        
        Args:
            soft_body: 软体节点
            radius: 半径
            height: 高度
            resolution: 分辨率
        """
        # 实现圆柱形软体创建
        half_height = height / 2.0
        
        vertices = []
        
        # 生成圆柱面
        for i in range(2):  # 顶部到底部
            z = half_height if i == 0 else -half_height
            for j in range(resolution * 2 + 1):
                theta = (j / (resolution * 2)) * 2 * math.pi
                x = radius * math.cos(theta)
                y = radius * math.sin(theta)
                vertices.append(Point3(x, y, z))
                
        # 添加顶点
        for vertex in vertices:
            soft_body.appendNode(vertex, 1.0)
            
        # 生成面索引
        indices = []
        for i in range(resolution * 2):
            indices.extend([i, i + 1, i + resolution * 2 + 1])
            indices.extend([i + 1, i + resolution * 2 + 2, i + resolution * 2 + 1])
            
        # 添加面
        for i in range(0, len(indices), 3):
            if i + 2 < len(indices):
                soft_body.appendFace(indices[i], indices[i+1], indices[i+2])
                
        # 添加链接
        self._add_links_from_indices(soft_body, indices)
        
        # 添加弯曲约束
        self._add_bending_constraints(soft_body, {})
        
    def _create_mesh_shape(self, soft_body: BulletSoftBodyNode, mesh_node: NodePath):
        """
        从网格创建软体
        
        Args:
            soft_body: 软体节点
            mesh_node: 网格节点
        """
        # 从网格节点提取顶点和面
        vertices, indices = self._extract_mesh_data(mesh_node)
        
        if not vertices or not indices:
            print("⚠ 无法从网格提取数据")
            return
            
        # 添加顶点
        for vertex in vertices:
            soft_body.appendNode(vertex, 1.0)
            
        # 添加面
        for i in range(0, len(indices), 3):
            if i + 2 < len(indices):
                soft_body.appendFace(indices[i], indices[i+1], indices[i+2])
                
        # 添加链接
        self._add_links_from_indices(soft_body, indices)
        
        # 添加弯曲约束
        self._add_bending_constraints(soft_body, {})
        
    def _add_links_from_indices(self, soft_body: BulletSoftBodyNode, indices: List[int]):
        """
        根据索引添加链接
        
        Args:
            soft_body: 软体节点
            indices: 索引列表
        """
        try:
            # 创建边集合以避免重复
            edges = set()
            
            # 从三角形面提取边
            for i in range(0, len(indices), 3):
                if i + 2 < len(indices):
                    # 三角形的三条边
                    edges.add(tuple(sorted([indices[i], indices[i+1]])))
                    edges.add(tuple(sorted([indices[i+1], indices[i+2]])))
                    edges.add(tuple(sorted([indices[i+2], indices[i]])))
                    
            # 添加链接
            for edge in edges:
                soft_body.appendLink(edge[0], edge[1])
                
        except Exception as e:
            print(f"⚠ 添加链接失败: {e}")
            
    def _add_bending_constraints(self, soft_body: BulletSoftBodyNode, params: Dict[str, Any]):
        """
        添加弯曲约束
        
        Args:
            soft_body: 软体节点
            params: 参数字典
        """
        try:
            # 添加弯曲约束
            # 这个参数控制弯曲约束的生成步长
            soft_body.generateBendingConstraints(2)
            
        except Exception as e:
            print(f"⚠ 添加弯曲约束失败: {e}")
            
    def _extract_mesh_data(self, node_path) -> Tuple[List[Point3], List[int]]:
        """
        从节点路径提取网格数据
        
        Args:
            node_path: 节点路径
            
        Returns:
            (顶点列表, 索引列表)
        """
        try:
            vertices = []
            indices = []
            
            # 查找几何节点
            geom_nodes = node_path.findAllMatches('**/+GeomNode')
            for geom_node_path in geom_nodes:
                geom_node = geom_node_path.node()
                for i in range(geom_node.getNumGeoms()):
                    geom = geom_node.getGeom(i)
                    vdata = geom.getVertexData()
                    
                    # 提取顶点
                    vertex_reader = GeomVertexReader(vdata, 'vertex')
                    while not vertex_reader.isAtEnd():
                        vertices.append(vertex_reader.getData3())
                        
                    # 提取索引
                    for prim in geom.getPrimitives():
                        index_reader = prim.getIndexIterator()
                        while not index_reader.isAtEnd():
                            indices.append(index_reader.getData())
                            index_reader.next()
                            
            return vertices, indices
            
        except Exception as e:
            print(f"⚠ 提取网格数据失败: {e}")
            return [], []
            
    def _get_physics_world(self):
        """
        获取物理世界对象
        
        Returns:
            物理世界对象或None
        """
        try:
            # 尝试从世界对象获取物理世界
            if hasattr(self.world, 'physics_world'):
                return self.world.physics_world.bullet_world
            elif hasattr(self.world, 'bullet_world'):
                return self.world.bullet_world
            else:
                # 查找物理插件
                physics_plugin = self.world.plugins.get('rigid_body_physics')
                if physics_plugin and hasattr(physics_plugin, 'physics_world'):
                    return physics_plugin.physics_world.bullet_world
        except Exception as e:
            print(f"⚠ 获取物理世界失败: {e}")
            
        return None