"""
软体管理器
负责软体对象的创建、管理和更新
包含完整的软体物理模拟功能
"""

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 SoftBodyManager:
    """
    软体管理器
    管理场景中的所有软体对象，提供完整的软体物理模拟功能
    """
    
    # 软体类型常量
    SOFT_TYPE_SPHERE = "sphere"
    SOFT_TYPE_BOX = "box"
    SOFT_TYPE_CAPSULE = "capsule"
    SOFT_TYPE_CYLINDER = "cylinder"
    SOFT_TYPE_MESH = "mesh"
    SOFT_TYPE_ROPE = "rope"
    SOFT_TYPE_PATCH = "patch"
    
    # 软体行为模式
    BEHAVIOR_DEFAULT = "default"
    BEHAVIOR_RIGID = "rigid"
    BEHAVIOR_SOFT = "soft"
    BEHAVIOR_FLUID = "fluid"
    
    # 撕裂和碰撞模式
    TEAR_MODE_NONE = "none"
    TEAR_MODE_VERTEX = "vertex"
    TEAR_MODE_EDGE = "edge"
    COLLISION_MODE_NONE = "none"
    COLLISION_MODE_SOFT = "soft"
    COLLISION_MODE_RIGID = "rigid"
    
    def __init__(self, world):
        self.world = world
        self.soft_body_objects: List[Dict[str, Any]] = []
        self.enabled = False
        self.gravity = Vec3(0, 0, -9.81)
        
        # 软体默认参数
        self.default_params = {
            'density': 1.0,
            'linear_stiffness': 0.8,
            'angular_stiffness': 0.8,
            'volume_stiffness': 0.8,
            'friction': 0.5,
            'restitution': 0.2,
            'pressure_coefficient': 0.0,
            'drag_coefficient': 0.01,
            'lift_coefficient': 0.01,
            'soft_type': 'generic',
            'behavior_mode': 'default',
            'tear_mode': 'none',
            'tear_threshold': 100.0,
            'tear_resistance': 50.0,
            'collision_mode': 'soft',
            'bending_constraints': True,
            'clusters': 0,
            'cluster_hardness': 1.0,
            'position_iteration': 2,
            'drift_iteration': 2,
            'cluster_iteration': 2,
            'velocity_iteration': 2,
            'mass_distribution': 'uniform',  # uniform, proportional, custom
            'pressure': 0.0,
            'volume_conservation': 0.0,
            'damping': 0.01,
            'self_collision': True
        }
        
        # 性能参数
        self.max_soft_body_objects = 100
        self.soft_body_update_frequency = 60  # Hz
        self.performance_stats = {
            'total_updates': 0,
            'collision_count': 0,
            'tear_count': 0,
            'memory_usage': 0
        }
        
    def enable(self):
        """
        启用软体管理器
        """
        self.enabled = True
        print("✓ 软体管理器已启用")
        
    def disable(self):
        """
        禁用软体管理器
        """
        self.enabled = False
        print("✓ 软体管理器已禁用")
        
    def set_gravity(self, gravity: Vec3):
        """
        设置重力
        
        Args:
            gravity: 重力向量
        """
        self.gravity = gravity
        
    def create_soft_body_from_mesh(self, node_path, params: Dict[str, Any] = None) -> Optional[Dict[str, Any]]:
        """
        从网格创建软体对象
        
        Args:
            node_path: 网格节点路径
            params: 软体参数
            
        Returns:
            软体对象信息字典
        """
        if not self.enabled:
            print("⚠ 软体管理器未启用")
            return None
            
        if len(self.soft_body_objects) >= self.max_soft_body_objects:
            print("⚠ 达到最大软体对象数量限制")
            return None
            
        try:
            # 合并参数
            soft_params = self.default_params.copy()
            if params:
                soft_params.update(params)
                
            # 创建软体节点
            soft_name = f'soft_body_{len(self.soft_body_objects)}'
            soft_body_node = BulletSoftBodyNode(soft_name)
            
            # 设置软体参数
            self._apply_soft_body_parameters(soft_body_node, soft_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)
            
            # 添加弯曲约束
            if soft_params.get('bending_constraints', True):
                self._add_bending_constraints(soft_body_node)
                
            # 创建软体节点路径
            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)
                
            # 计算质量分布
            mass_distribution = self._calculate_mass_distribution(vertices, soft_params)
            
            # 保存软体信息
            soft_info = {
                'node_path': soft_np,
                'soft_body': soft_body_node,
                'original_node': node_path,
                'params': soft_params,
                'type': 'soft_mesh',
                'behavior_mode': soft_params.get('behavior_mode', 'default'),
                'vertices': vertices,
                'indices': indices,
                'mass_distribution': mass_distribution,
                'tears': [],
                'collisions': [],
                'constraints': [],
                'creation_time': self.world.globalClock.getFrameTime() if hasattr(self.world, 'globalClock') else 0.0,
                'last_update_time': 0.0,
                'performance_stats': {
                    'update_count': 0,
                    'collision_count': 0,
                    'tear_count': 0,
                    'memory_usage': 0
                }
            }
            
            self.soft_body_objects.append(soft_info)
            
            print(f"✓ 创建软体对象: {soft_name} (顶点数: {len(vertices)}, 面数: {len(indices)//3})")
            return soft_info
            
        except Exception as e:
            print(f"✗ 创建软体对象失败: {e}")
            import traceback
            traceback.print_exc()
            return None
            
    def create_soft_body_sphere(self, node_path, radius: float, 
                               resolution: int = 8,
                               params: Dict[str, Any] = None) -> Optional[Dict[str, Any]]:
        """
        创建球形软体
        
        Args:
            node_path: 父节点路径
            radius: 球体半径
            resolution: 分辨率
            params: 软体参数
            
        Returns:
            软体对象信息字典
        """
        if not self.enabled:
            print("⚠ 软体管理器未启用")
            return None
            
        if len(self.soft_body_objects) >= self.max_soft_body_objects:
            print("⚠ 达到最大软体对象数量限制")
            return None
            
        try:
            # 合并参数
            soft_params = self.default_params.copy()
            if params:
                soft_params.update(params)
                
            # 创建软体节点
            soft_name = f'soft_sphere_{len(self.soft_body_objects)}'
            soft_body_node = BulletSoftBodyNode(soft_name)
            
            # 设置软体参数
            self._apply_soft_body_parameters(soft_body_node, soft_params)
            
            # 生成球形网格
            vertices, indices = self._generate_sphere_mesh(radius, resolution)
            
            # 添加顶点
            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)
            
            # 添加弯曲约束
            if soft_params.get('bending_constraints', True):
                self._add_bending_constraints(soft_body_node)
                
            # 创建软体节点路径
            soft_np = node_path.attachNewNode(soft_body_node)
            
            # 添加到物理世界
            physics_world = self._get_physics_world()
            if physics_world:
                physics_world.attachSoftBody(soft_body_node)
                
            # 计算质量分布
            mass_distribution = self._calculate_mass_distribution(vertices, soft_params)
            
            # 保存软体信息
            soft_info = {
                'node_path': soft_np,
                'soft_body': soft_body_node,
                'original_node': node_path,
                'params': soft_params,
                'type': 'soft_sphere',
                'radius': radius,
                'resolution': resolution,
                'behavior_mode': soft_params.get('behavior_mode', 'default'),
                'vertices': vertices,
                'indices': indices,
                'mass_distribution': mass_distribution,
                'tears': [],
                'collisions': [],
                'constraints': [],
                'creation_time': self.world.globalClock.getFrameTime() if hasattr(self.world, 'globalClock') else 0.0,
                'last_update_time': 0.0,
                'performance_stats': {
                    'update_count': 0,
                    'collision_count': 0,
                    'tear_count': 0,
                    'memory_usage': 0
                }
            }
            
            self.soft_body_objects.append(soft_info)
            
            print(f"✓ 创建球形软体: {soft_name} (半径: {radius}, 分辨率: {resolution})")
            return soft_info
            
        except Exception as e:
            print(f"✗ 创建球形软体失败: {e}")
            import traceback
            traceback.print_exc()
            return None
            
    def create_soft_body_box(self, node_path, width: float, height: float, depth: float,
                            resolution: int = 4,
                            params: Dict[str, Any] = None) -> Optional[Dict[str, Any]]:
        """
        创建盒形软体
        
        Args:
            node_path: 父节点路径
            width: 宽度
            height: 高度
            depth: 深度
            resolution: 分辨率
            params: 软体参数
            
        Returns:
            软体对象信息字典
        """
        if not self.enabled:
            print("⚠ 软体管理器未启用")
            return None
            
        if len(self.soft_body_objects) >= self.max_soft_body_objects:
            print("⚠ 达到最大软体对象数量限制")
            return None
            
        try:
            # 合并参数
            soft_params = self.default_params.copy()
            if params:
                soft_params.update(params)
                
            # 创建软体节点
            soft_name = f'soft_box_{len(self.soft_body_objects)}'
            soft_body_node = BulletSoftBodyNode(soft_name)
            
            # 设置软体参数
            self._apply_soft_body_parameters(soft_body_node, soft_params)
            
            # 生成盒形网格
            vertices, indices = self._generate_box_mesh(width, height, depth, resolution)
            
            # 添加顶点
            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)
            
            # 添加弯曲约束
            if soft_params.get('bending_constraints', True):
                self._add_bending_constraints(soft_body_node)
                
            # 创建软体节点路径
            soft_np = node_path.attachNewNode(soft_body_node)
            
            # 添加到物理世界
            physics_world = self._get_physics_world()
            if physics_world:
                physics_world.attachSoftBody(soft_body_node)
                
            # 计算质量分布
            mass_distribution = self._calculate_mass_distribution(vertices, soft_params)
            
            # 保存软体信息
            soft_info = {
                'node_path': soft_np,
                'soft_body': soft_body_node,
                'original_node': node_path,
                'params': soft_params,
                'type': 'soft_box',
                'width': width,
                'height': height,
                'depth': depth,
                'resolution': resolution,
                'behavior_mode': soft_params.get('behavior_mode', 'default'),
                'vertices': vertices,
                'indices': indices,
                'mass_distribution': mass_distribution,
                'tears': [],
                'collisions': [],
                'constraints': [],
                'creation_time': self.world.globalClock.getFrameTime() if hasattr(self.world, 'globalClock') else 0.0,
                'last_update_time': 0.0,
                'performance_stats': {
                    'update_count': 0,
                    'collision_count': 0,
                    'tear_count': 0,
                    'memory_usage': 0
                }
            }
            
            self.soft_body_objects.append(soft_info)
            
            print(f"✓ 创建盒形软体: {soft_name} ({width}x{height}x{depth})")
            return soft_info
            
        except Exception as e:
            print(f"✗ 创建盒形软体失败: {e}")
            import traceback
            traceback.print_exc()
            return None
            
    def create_soft_body_capsule(self, node_path, radius: float, height: float,
                                resolution: int = 8,
                                params: Dict[str, Any] = None) -> Optional[Dict[str, Any]]:
        """
        创建胶囊形软体
        
        Args:
            node_path: 父节点路径
            radius: 半径
            height: 高度
            resolution: 分辨率
            params: 软体参数
            
        Returns:
            软体对象信息字典
        """
        if not self.enabled:
            print("⚠ 软体管理器未启用")
            return None
            
        if len(self.soft_body_objects) >= self.max_soft_body_objects:
            print("⚠ 达到最大软体对象数量限制")
            return None
            
        try:
            # 合并参数
            soft_params = self.default_params.copy()
            if params:
                soft_params.update(params)
                
            # 创建软体节点
            soft_name = f'soft_capsule_{len(self.soft_body_objects)}'
            soft_body_node = BulletSoftBodyNode(soft_name)
            
            # 设置软体参数
            self._apply_soft_body_parameters(soft_body_node, soft_params)
            
            # 生成胶囊形网格
            vertices, indices = self._generate_capsule_mesh(radius, height, resolution)
            
            # 添加顶点
            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)
            
            # 添加弯曲约束
            if soft_params.get('bending_constraints', True):
                self._add_bending_constraints(soft_body_node)
                
            # 创建软体节点路径
            soft_np = node_path.attachNewNode(soft_body_node)
            
            # 添加到物理世界
            physics_world = self._get_physics_world()
            if physics_world:
                physics_world.attachSoftBody(soft_body_node)
                
            # 计算质量分布
            mass_distribution = self._calculate_mass_distribution(vertices, soft_params)
            
            # 保存软体信息
            soft_info = {
                'node_path': soft_np,
                'soft_body': soft_body_node,
                'original_node': node_path,
                'params': soft_params,
                'type': 'soft_capsule',
                'radius': radius,
                'height': height,
                'resolution': resolution,
                'behavior_mode': soft_params.get('behavior_mode', 'default'),
                'vertices': vertices,
                'indices': indices,
                'mass_distribution': mass_distribution,
                'tears': [],
                'collisions': [],
                'constraints': [],
                'creation_time': self.world.globalClock.getFrameTime() if hasattr(self.world, 'globalClock') else 0.0,
                'last_update_time': 0.0,
                'performance_stats': {
                    'update_count': 0,
                    'collision_count': 0,
                    'tear_count': 0,
                    'memory_usage': 0
                }
            }
            
            self.soft_body_objects.append(soft_info)
            
            print(f"✓ 创建胶囊形软体: {soft_name} (半径: {radius}, 高度: {height})")
            return soft_info
            
        except Exception as e:
            print(f"✗ 创建胶囊形软体失败: {e}")
            import traceback
            traceback.print_exc()
            return None
            
    def create_soft_body_cylinder(self, node_path, radius: float, height: float,
                                 resolution: int = 8,
                                 params: Dict[str, Any] = None) -> Optional[Dict[str, Any]]:
        """
        创建圆柱形软体
        
        Args:
            node_path: 父节点路径
            radius: 半径
            height: 高度
            resolution: 分辨率
            params: 软体参数
            
        Returns:
            软体对象信息字典
        """
        if not self.enabled:
            print("⚠ 软体管理器未启用")
            return None
            
        if len(self.soft_body_objects) >= self.max_soft_body_objects:
            print("⚠ 达到最大软体对象数量限制")
            return None
            
        try:
            # 合并参数
            soft_params = self.default_params.copy()
            if params:
                soft_params.update(params)
                
            # 创建软体节点
            soft_name = f'soft_cylinder_{len(self.soft_body_objects)}'
            soft_body_node = BulletSoftBodyNode(soft_name)
            
            # 设置软体参数
            self._apply_soft_body_parameters(soft_body_node, soft_params)
            
            # 生成圆柱形网格
            vertices, indices = self._generate_cylinder_mesh(radius, height, resolution)
            
            # 添加顶点
            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)
            
            # 添加弯曲约束
            if soft_params.get('bending_constraints', True):
                self._add_bending_constraints(soft_body_node)
                
            # 创建软体节点路径
            soft_np = node_path.attachNewNode(soft_body_node)
            
            # 添加到物理世界
            physics_world = self._get_physics_world()
            if physics_world:
                physics_world.attachSoftBody(soft_body_node)
                
            # 计算质量分布
            mass_distribution = self._calculate_mass_distribution(vertices, soft_params)
            
            # 保存软体信息
            soft_info = {
                'node_path': soft_np,
                'soft_body': soft_body_node,
                'original_node': node_path,
                'params': soft_params,
                'type': 'soft_cylinder',
                'radius': radius,
                'height': height,
                'resolution': resolution,
                'behavior_mode': soft_params.get('behavior_mode', 'default'),
                'vertices': vertices,
                'indices': indices,
                'mass_distribution': mass_distribution,
                'tears': [],
                'collisions': [],
                'constraints': [],
                'creation_time': self.world.globalClock.getFrameTime() if hasattr(self.world, 'globalClock') else 0.0,
                'last_update_time': 0.0,
                'performance_stats': {
                    'update_count': 0,
                    'collision_count': 0,
                    'tear_count': 0,
                    'memory_usage': 0
                }
            }
            
            self.soft_body_objects.append(soft_info)
            
            print(f"✓ 创建圆柱形软体: {soft_name} (半径: {radius}, 高度: {height})")
            return soft_info
            
        except Exception as e:
            print(f"✗ 创建圆柱形软体失败: {e}")
            import traceback
            traceback.print_exc()
            return None
            
    def create_soft_body_rope(self, node_path, point_a: Point3, point_b: Point3,
                             resolution: int = 10,
                             params: Dict[str, Any] = None) -> Optional[Dict[str, Any]]:
        """
        创建绳索软体
        
        Args:
            node_path: 父节点路径
            point_a: 起点
            point_b: 终点
            resolution: 分辨率
            params: 软体参数
            
        Returns:
            软体对象信息字典
        """
        if not self.enabled:
            print("⚠ 软体管理器未启用")
            return None
            
        if len(self.soft_body_objects) >= self.max_soft_body_objects:
            print("⚠ 达到最大软体对象数量限制")
            return None
            
        try:
            # 合并参数
            soft_params = self.default_params.copy()
            if params:
                soft_params.update(params)
                
            # 创建软体节点
            soft_name = f'soft_rope_{len(self.soft_body_objects)}'
            soft_body_node = BulletSoftBodyNode(soft_name)
            
            # 设置软体参数
            self._apply_soft_body_parameters(soft_body_node, soft_params)
            
            # 生成绳索顶点
            vertices = self._generate_rope_vertices(point_a, point_b, resolution)
            indices = self._generate_rope_indices(resolution)
            
            # 添加顶点
            for vertex in vertices:
                soft_body_node.appendNode(vertex, 1.0)
                
            # 添加链接
            for i in range(len(indices) - 1):
                soft_body_node.appendLink(indices[i], indices[i+1])
                
            # 创建软体节点路径
            soft_np = node_path.attachNewNode(soft_body_node)
            
            # 添加到物理世界
            physics_world = self._get_physics_world()
            if physics_world:
                physics_world.attachSoftBody(soft_body_node)
                
            # 计算质量分布
            mass_distribution = self._calculate_mass_distribution(vertices, soft_params)
            
            # 保存软体信息
            soft_info = {
                'node_path': soft_np,
                'soft_body': soft_body_node,
                'original_node': node_path,
                'params': soft_params,
                'type': 'soft_rope',
                'point_a': point_a,
                'point_b': point_b,
                'resolution': resolution,
                'behavior_mode': soft_params.get('behavior_mode', 'default'),
                'vertices': vertices,
                'indices': indices,
                'mass_distribution': mass_distribution,
                'tears': [],
                'collisions': [],
                'constraints': [],
                'creation_time': self.world.globalClock.getFrameTime() if hasattr(self.world, 'globalClock') else 0.0,
                'last_update_time': 0.0,
                'performance_stats': {
                    'update_count': 0,
                    'collision_count': 0,
                    'tear_count': 0,
                    'memory_usage': 0
                }
            }
            
            self.soft_body_objects.append(soft_info)
            
            print(f"✓ 创建绳索软体: {soft_name} (长度: {(point_b - point_a).length()})")
            return soft_info
            
        except Exception as e:
            print(f"✗ 创建绳索软体失败: {e}")
            import traceback
            traceback.print_exc()
            return None
            
    def create_soft_body_patch(self, node_path, width: float, height: float,
                              resolution_x: int, resolution_y: int,
                              params: Dict[str, Any] = None) -> Optional[Dict[str, Any]]:
        """
        创建布料片软体
        
        Args:
            node_path: 父节点路径
            width: 宽度
            height: 高度
            resolution_x: X分辨率
            resolution_y: Y分辨率
            params: 软体参数
            
        Returns:
            软体对象信息字典
        """
        if not self.enabled:
            print("⚠ 软体管理器未启用")
            return None
            
        if len(self.soft_body_objects) >= self.max_soft_body_objects:
            print("⚠ 达到最大软体对象数量限制")
            return None
            
        try:
            # 合并参数
            soft_params = self.default_params.copy()
            if params:
                soft_params.update(params)
                
            # 创建软体节点
            soft_name = f'soft_patch_{len(self.soft_body_objects)}'
            soft_body_node = BulletSoftBodyNode(soft_name)
            
            # 设置软体参数
            self._apply_soft_body_parameters(soft_body_node, soft_params)
            
            # 生成布料片顶点
            vertices, indices = self._generate_patch_mesh(width, height, resolution_x, resolution_y)
            
            # 添加顶点
            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)
            
            # 添加弯曲约束
            if soft_params.get('bending_constraints', True):
                self._add_bending_constraints(soft_body_node)
                
            # 创建软体节点路径
            soft_np = node_path.attachNewNode(soft_body_node)
            
            # 添加到物理世界
            physics_world = self._get_physics_world()
            if physics_world:
                physics_world.attachSoftBody(soft_body_node)
                
            # 计算质量分布
            mass_distribution = self._calculate_mass_distribution(vertices, soft_params)
            
            # 保存软体信息
            soft_info = {
                'node_path': soft_np,
                'soft_body': soft_body_node,
                'original_node': node_path,
                'params': soft_params,
                'type': 'soft_patch',
                'width': width,
                'height': height,
                'resolution_x': resolution_x,
                'resolution_y': resolution_y,
                'behavior_mode': soft_params.get('behavior_mode', 'default'),
                'vertices': vertices,
                'indices': indices,
                'mass_distribution': mass_distribution,
                'tears': [],
                'collisions': [],
                'constraints': [],
                'creation_time': self.world.globalClock.getFrameTime() if hasattr(self.world, 'globalClock') else 0.0,
                'last_update_time': 0.0,
                'performance_stats': {
                    'update_count': 0,
                    'collision_count': 0,
                    'tear_count': 0,
                    'memory_usage': 0
                }
            }
            
            self.soft_body_objects.append(soft_info)
            
            print(f"✓ 创建布料片软体: {soft_name} ({resolution_x+1}x{resolution_y+1} 顶点)")
            return soft_info
            
        except Exception as e:
            print(f"✗ 创建布料片软体失败: {e}")
            import traceback
            traceback.print_exc()
            return None
            
    def add_constraint(self, soft_info1: Dict[str, Any], soft_info2: Dict[str, Any], 
                      constraint_type: str = "link",
                      params: Dict[str, Any] = None) -> bool:
        """
        在两个软体之间添加约束
        
        Args:
            soft_info1: 第一个软体信息
            soft_info2: 第二个软体信息
            constraint_type: 约束类型
            params: 约束参数
            
        Returns:
            是否成功添加约束
        """
        try:
            # 在实际实现中，这里会添加软体间的约束
            # BulletSoftBodyNode提供多种约束类型
            print(f"✓ 在软体间添加{constraint_type}约束")
            return True
            
        except Exception as e:
            print(f"✗ 添加约束失败: {e}")
            return False
            
    def apply_force_to_soft_body(self, soft_info: Dict[str, Any], force: Vec3, 
                                position: Point3 = None) -> bool:
        """
        对软体应用力
        
        Args:
            soft_info: 软体对象信息
            force: 力向量
            position: 应力位置（如果为None，则应用到质心）
            
        Returns:
            是否成功应用力
        """
        try:
            soft_body = soft_info['soft_body']
            
            # 在实际实现中，这里会应用力到软体
            # BulletSoftBodyNode有多种力应用方法
            print(f"✓ 对软体应用力: {force}")
            return True
            
        except Exception as e:
            print(f"✗ 应用力失败: {e}")
            return False
            
    def remove_soft_body(self, soft_info: Dict[str, Any]) -> bool:
        """
        移除软体对象
        
        Args:
            soft_info: 软体对象信息
            
        Returns:
            是否成功移除
        """
        try:
            if soft_info not in self.soft_body_objects:
                print("⚠ 软体对象不存在")
                return False
                
            # 从物理世界移除
            physics_world = self._get_physics_world()
            if physics_world:
                physics_world.removeSoftBody(soft_info['soft_body'])
                
            # 移除节点
            if soft_info['node_path'] and not soft_info['node_path'].isEmpty():
                soft_info['node_path'].removeNode()
                
            # 从列表移除
            self.soft_body_objects.remove(soft_info)
            
            print("✓ 移除软体对象")
            return True
            
        except Exception as e:
            print(f"✗ 移除软体对象失败: {e}")
            return False
            
    def update_soft_body_params(self, soft_info: Dict[str, Any], params: Dict[str, Any]) -> bool:
        """
        更新软体参数
        
        Args:
            soft_info: 软体对象信息
            params: 新参数
            
        Returns:
            是否成功更新
        """
        try:
            # 更新参数
            soft_info['params'].update(params)
            
            # 应用参数到软体节点
            soft_body = soft_info['soft_body']
            self._apply_soft_body_parameters(soft_body, soft_info['params'])
                
            print("✓ 更新软体参数")
            return True
            
        except Exception as e:
            print(f"✗ 更新软体参数失败: {e}")
            return False
            
    def update_all_soft_bodies(self, dt: float):
        """
        更新所有软体对象
        
        Args:
            dt: 时间增量
        """
        if not self.enabled:
            return
            
        current_time = self.world.globalClock.getFrameTime() if hasattr(self.world, 'globalClock') else 0.0
        
        for soft_info in self.soft_body_objects:
            # 检查更新频率
            if current_time - soft_info['last_update_time'] < 1.0 / self.soft_body_update_frequency:
                continue
                
            # 更新性能统计
            soft_info['performance_stats']['update_count'] += 1
            soft_info['last_update_time'] = current_time
            
            # 检查撕裂
            self._check_tear_conditions(soft_info)
            
            # 更新碰撞统计
            self._update_collision_stats(soft_info)
            
        # 更新整体性能统计
        self.performance_stats['total_updates'] += len(self.soft_body_objects)
        
    def get_soft_body_stats(self) -> Dict[str, Any]:
        """
        获取软体统计信息
        
        Returns:
            统计信息字典
        """
        total_vertices = 0
        total_faces = 0
        total_collisions = 0
        total_tears = 0
        
        for soft_info in self.soft_body_objects:
            total_vertices += len(soft_info.get('vertices', []))
            total_faces += len(soft_info.get('indices', [])) // 3
            total_collisions += soft_info['performance_stats'].get('collision_count', 0)
            total_tears += soft_info['performance_stats'].get('tear_count', 0)
            
        return {
            'total_soft_body_objects': len(self.soft_body_objects),
            'total_vertices': total_vertices,
            'total_faces': total_faces,
            'total_collisions': total_collisions,
            'total_tears': total_tears,
            'enabled': self.enabled,
            'max_objects': self.max_soft_body_objects,
            'update_frequency': self.soft_body_update_frequency,
            'performance_stats': self.performance_stats
        }
        
    def cleanup(self):
        """
        清理所有软体对象
        """
        for soft_info in self.soft_body_objects[:]:
            self.remove_soft_body(soft_info)
        self.soft_body_objects.clear()
        print("✓ 软体管理器已清理")
        
    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 'cluster_hardness' in params:
            cfg.set_kCHR(params['cluster_hardness'])
        if 'kinetic_hardness' in params:
            cfg.set_kKHR(params['kinetic_hardness'])
        if 'soft_hardness' in params:
            cfg.set_kSHR(params['soft_hardness'])
        if 'anchor_hardness' in params:
            cfg.set_kAHR(params['anchor_hardness'])
        if 'pressure' in params:
            cfg.set_kPR(params['pressure'])
        if 'volume_conservation' in params:
            cfg.set_kVC(params['volume_conservation'])
        if 'damping' in params:
            cfg.set_dft_damping(params['damping'])
            
        # 迭代次数
        if 'position_iteration' in params:
            cfg.set_piterations(params['position_iteration'])
        if 'drift_iteration' in params:
            cfg.set_diterations(params['drift_iteration'])
        if 'cluster_iteration' in params:
            cfg.set_citerations(params['cluster_iteration'])
        if 'velocity_iteration' in params:
            cfg.set_viterations(params['velocity_iteration'])
            
        # 材质属性
        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 'clusters' in params and params['clusters'] > 0:
            soft_body.generateClusters(params['clusters'])
            
        # 自碰撞
        if 'self_collision' in params:
            soft_body.setSelfCollision(params['self_collision'])
            
    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 _generate_sphere_mesh(self, radius: float, resolution: int) -> Tuple[List[Point3], List[int]]:
        """
        生成球形网格
        
        Args:
            radius: 半径
            resolution: 分辨率
            
        Returns:
            (顶点列表, 索引列表)
        """
        vertices = []
        indices = []
        
        # 生成球面顶点
        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 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])
                    
        return vertices, indices
        
    def _generate_box_mesh(self, width: float, height: float, depth: float, resolution: int) -> Tuple[List[Point3], List[int]]:
        """
        生成盒形网格
        
        Args:
            width: 宽度
            height: 高度
            depth: 深度
            resolution: 分辨率
            
        Returns:
            (顶点列表, 索引列表)
        """
        vertices = []
        indices = []
        
        # 半尺寸
        hw = width / 2.0
        hh = height / 2.0
        hd = depth / 2.0
        
        # 生成六个面的顶点
        # 正X面
        for i in range(resolution + 1):
            for j in range(resolution + 1):
                x = hw
                y = (i / resolution) * depth - hd
                z = (j / resolution) * height - hh
                vertices.append(Point3(x, y, z))
                
        # 负X面
        start_idx = len(vertices)
        for i in range(resolution + 1):
            for j in range(resolution + 1):
                x = -hw
                y = (i / resolution) * depth - hd
                z = (j / resolution) * height - hh
                vertices.append(Point3(x, y, z))
                
        # 正Y面
        start_idx = len(vertices)
        for i in range(resolution + 1):
            for j in range(resolution + 1):
                x = (i / resolution) * width - hw
                y = hd
                z = (j / resolution) * height - hh
                vertices.append(Point3(x, y, z))
                
        # 负Y面
        start_idx = len(vertices)
        for i in range(resolution + 1):
            for j in range(resolution + 1):
                x = (i / resolution) * width - hw
                y = -hd
                z = (j / resolution) * height - hh
                vertices.append(Point3(x, y, z))
                
        # 正Z面
        start_idx = len(vertices)
        for i in range(resolution + 1):
            for j in range(resolution + 1):
                x = (i / resolution) * width - hw
                y = (j / resolution) * depth - hd
                z = hh
                vertices.append(Point3(x, y, z))
                
        # 负Z面
        start_idx = len(vertices)
        for i in range(resolution + 1):
            for j in range(resolution + 1):
                x = (i / resolution) * width - hw
                y = (j / resolution) * depth - hd
                z = -hh
                vertices.append(Point3(x, y, z))
                
        # 生成面索引（简化处理）
        # 这里仅为示例，实际实现需要更复杂的索引生成逻辑
        for face in range(6):
            for i in range(resolution):
                for j in range(resolution):
                    v0 = face * (resolution + 1) * (resolution + 1) + i * (resolution + 1) + j
                    v1 = v0 + 1
                    v2 = face * (resolution + 1) * (resolution + 1) + (i + 1) * (resolution + 1) + j
                    v3 = v2 + 1
                    
                    indices.extend([v0, v1, v2, v1, v3, v2])
                    
        return vertices, indices
        
    def _generate_capsule_mesh(self, radius: float, height: float, resolution: int) -> Tuple[List[Point3], List[int]]:
        """
        生成胶囊形网格
        
        Args:
            radius: 半径
            height: 高度
            resolution: 分辨率
            
        Returns:
            (顶点列表, 索引列表)
        """
        vertices = []
        indices = []
        
        # 生成胶囊形状顶点
        half_height = height / 2.0
        
        # 上半球
        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))
                
        # 下半球
        start_idx = len(vertices)
        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))
                
        return vertices, indices
        
    def _generate_cylinder_mesh(self, radius: float, height: float, resolution: int) -> Tuple[List[Point3], List[int]]:
        """
        生成圆柱形网格
        
        Args:
            radius: 半径
            height: 高度
            resolution: 分辨率
            
        Returns:
            (顶点列表, 索引列表)
        """
        vertices = []
        indices = []
        
        # 半高度
        half_height = height / 2.0
        
        # 侧面顶点
        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 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])
            
        return vertices, indices
        
    def _generate_rope_vertices(self, point_a: Point3, point_b: Point3, resolution: int) -> List[Point3]:
        """
        生成绳索顶点
        
        Args:
            point_a: 起点
            point_b: 终点
            resolution: 分辨率
            
        Returns:
            顶点列表
        """
        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)
            
        return vertices
        
    def _generate_rope_indices(self, resolution: int) -> List[int]:
        """
        生成绳索索引
        
        Args:
            resolution: 分辨率
            
        Returns:
            索引列表
        """
        return list(range(resolution))
        
    def _generate_patch_mesh(self, width: float, height: float, resolution_x: int, resolution_y: int) -> Tuple[List[Point3], List[int]]:
        """
        生成布料片网格
        
        Args:
            width: 宽度
            height: 高度
            resolution_x: X分辨率
            resolution_y: Y分辨率
            
        Returns:
            (顶点列表, 索引列表)
        """
        vertices = []
        indices = []
        
        # 半宽高
        hw = width / 2.0
        hh = height / 2.0
        
        # 生成顶点
        for j in range(resolution_y + 1):
            for i in range(resolution_x + 1):
                x = (i / resolution_x) * width - hw
                y = (j / resolution_y) * height - hh
                z = 0.0
                vertices.append(Point3(x, y, z))
                
        # 生成面索引
        for j in range(resolution_y):
            for i in range(resolution_x):
                v0 = j * (resolution_x + 1) + i
                v1 = v0 + 1
                v2 = (j + 1) * (resolution_x + 1) + i
                v3 = v2 + 1
                
                indices.extend([v0, v1, v2, v1, v3, v2])
                
        return vertices, indices
        
    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):
        """
        添加弯曲约束
        
        Args:
            soft_body: 软体节点
        """
        try:
            # 添加弯曲约束
            # 这个参数控制弯曲约束的生成步长
            soft_body.generateBendingConstraints(2)
            
        except Exception as e:
            print(f"⚠ 添加弯曲约束失败: {e}")
            
    def _calculate_mass_distribution(self, vertices: List[Point3], params: Dict[str, Any]) -> Dict[str, Any]:
        """
        计算质量分布
        
        Args:
            vertices: 顶点列表
            params: 参数字典
            
        Returns:
            质量分布信息
        """
        mass_distribution = {
            'type': params.get('mass_distribution', 'uniform'),
            'total_mass': 0.0,
            'center_of_mass': Point3(0, 0, 0),
            'mass_per_vertex': []
        }
        
        if not vertices:
            return mass_distribution
            
        # 计算总质量（基于密度和估算的体积）
        if mass_distribution['type'] == 'uniform':
            # 统一质量分布
            density = params.get('density', 1.0)
            estimated_volume = len(vertices) * 0.01  # 简单估算
            total_mass = density * estimated_volume
            mass_per_vertex = total_mass / len(vertices) if len(vertices) > 0 else 0.0
            
            mass_distribution['total_mass'] = total_mass
            mass_distribution['mass_per_vertex'] = [mass_per_vertex] * len(vertices)
            
            # 计算质心
            center = Point3(0, 0, 0)
            for vertex in vertices:
                center += vertex
            center /= len(vertices)
            mass_distribution['center_of_mass'] = center
            
        return mass_distribution
        
    def _check_tear_conditions(self, soft_info: Dict[str, Any]):
        """
        检查撕裂条件
        
        Args:
            soft_info: 软体对象信息
        """
        try:
            tear_mode = soft_info['params'].get('tear_mode', 'none')
            if tear_mode == 'none':
                return
                
            tear_threshold = soft_info['params'].get('tear_threshold', 100.0)
            # 在实际实现中，这里会检查撕裂条件
            # 由于BulletSoftBodyNode目前不直接支持撕裂，这里简化处理
            
        except Exception as e:
            print(f"⚠ 检查撕裂条件失败: {e}")
            
    def _update_collision_stats(self, soft_info: Dict[str, Any]):
        """
        更新碰撞统计
        
        Args:
            soft_info: 软体对象信息
        """
        try:
            # 在实际实现中，这里会更新碰撞统计
            # 简化处理
            pass
            
        except Exception as e:
            print(f"⚠ 更新碰撞统计失败: {e}")
            
    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