""" Boids算法实现 这是一个经过优化和扩展的Boids算法实现，包含空间分区优化和高级行为规则 """ from panda3d.core import Vec3, Point3 import random import math from typing import List, Dict, Tuple, Optional class SpatialPartition: """ 空间分区类，用于优化邻居查找算法 将3D空间划分为网格，每个网格格子存储位于其中的群体成员 """ def __init__(self, bounds: Dict[str, float], cell_size: float): """ 初始化空间分区 :param bounds: 边界信息 {'min_x': float, 'max_x': float, 'min_y': float, 'max_y': float, 'min_z': float, 'max_z': float} :param cell_size: 网格格子大小 """ self.bounds = bounds self.cell_size = cell_size # 计算网格尺寸 self.grid_width = int((bounds['max_x'] - bounds['min_x']) / cell_size) + 1 self.grid_height = int((bounds['max_y'] - bounds['min_y']) / cell_size) + 1 self.grid_depth = int((bounds['max_z'] - bounds['min_z']) / cell_size) + 1 # 创建网格 self.grid = {} # 邻居格子偏移量 self.neighbor_offsets = [ (-1, -1, -1), (-1, -1, 0), (-1, -1, 1), (-1, 0, -1), (-1, 0, 0), (-1, 0, 1), (-1, 1, -1), (-1, 1, 0), (-1, 1, 1), (0, -1, -1), (0, -1, 0), (0, -1, 1), (0, 0, -1), (0, 0, 0), (0, 0, 1), (0, 1, -1), (0, 1, 0), (0, 1, 1), (1, -1, -1), (1, -1, 0), (1, -1, 1), (1, 0, -1), (1, 0, 0), (1, 0, 1), (1, 1, -1), (1, 1, 0), (1, 1, 1) ] def get_grid_coords(self, position: Vec3) -> Tuple[int, int, int]: """ 获取位置对应的网格坐标 :param position: 3D位置 :return: (x, y, z) 网格坐标 """ grid_x = int((position.x - self.bounds['min_x']) / self.cell_size) grid_y = int((position.y - self.bounds['min_y']) / self.cell_size) grid_z = int((position.z - self.bounds['min_z']) / self.cell_size) # 确保坐标在有效范围内 grid_x = max(0, min(grid_x, self.grid_width - 1)) grid_y = max(0, min(grid_y, self.grid_height - 1)) grid_z = max(0, min(grid_z, self.grid_depth - 1)) return (grid_x, grid_y, grid_z) def add_member(self, member: Dict, position: Vec3): """ 将成员添加到网格中 :param member: 成员对象 :param position: 成员位置 """ grid_coords = self.get_grid_coords(position) grid_key = f"{grid_coords[0]}_{grid_coords[1]}_{grid_coords[2]}" if grid_key not in self.grid: self.grid[grid_key] = [] self.grid[grid_key].append(member) def remove_member(self, member: Dict, position: Vec3): """ 从网格中移除成员 :param member: 成员对象 :param position: 成员位置 """ grid_coords = self.get_grid_coords(position) grid_key = f"{grid_coords[0]}_{grid_coords[1]}_{grid_coords[2]}" if grid_key in self.grid: if member in self.grid[grid_key]: self.grid[grid_key].remove(member) def update_member(self, member: Dict, old_position: Vec3, new_position: Vec3): """ 更新成员在网格中的位置 :param member: 成员对象 :param old_position: 原位置 :param new_position: 新位置 """ # 从旧位置移除 self.remove_member(member, old_position) # 添加到新位置 self.add_member(member, new_position) def get_neighbors(self, position: Vec3, radius: float) -> List[Dict]: """ 获取指定位置周围的邻居 :param position: 中心位置 :param radius: 搜索半径 :return: 邻居列表 """ neighbors = [] # 计算需要检查的网格格子范围 cells_to_check = int(radius / self.cell_size) + 1 center_coords = self.get_grid_coords(position) # 检查中心格子及其邻居 for offset in self.neighbor_offsets: grid_x = center_coords[0] + offset[0] grid_y = center_coords[1] + offset[1] grid_z = center_coords[2] + offset[2] # 检查格子是否在有效范围内 if (0 <= grid_x < self.grid_width and 0 <= grid_y < self.grid_height and 0 <= grid_z < self.grid_depth): grid_key = f"{grid_x}_{grid_y}_{grid_z}" if grid_key in self.grid: # 检查格子中的每个成员是否在半径范围内 for member in self.grid[grid_key]: distance = (position - member['position']).length() if distance <= radius and member not in neighbors: neighbors.append(member) return neighbors def clear(self): """ 清空网格 """ self.grid.clear() class PhysicsSimulator: """ 物理模拟器，提供更真实的物理行为 """ def __init__(self): self.gravity = Vec3(0, 0, -9.81) # 重力加速度 self.air_resistance = 0.01 # 空气阻力系数 self.fluid_density = 1.225 # 空气密度 (kg/m^3) def apply_gravity(self, member: Dict, dt: float): """ 应用重力 """ if 'mass' in member: mass = member['mass'] else: mass = 1.0 # 默认质量 gravity_force = self.gravity * mass member['velocity'] += gravity_force * dt / mass def apply_air_resistance(self, member: Dict, dt: float): """ 应用空气阻力 """ velocity = member['velocity'] speed = velocity.length() if speed > 0: # 空气阻力与速度平方成正比 drag_force_magnitude = 0.5 * self.fluid_density * speed * speed * 0.01 # 简化的阻力系数 drag_force = -velocity.normalized() * drag_force_magnitude # 应用阻力 member['velocity'] += drag_force * dt def apply_buoyancy(self, member: Dict, fluid_level: float, dt: float): """ 应用浮力（适用于模拟水中生物） """ if member['position'].z < fluid_level: # 计算浮力（简化模型） buoyancy_force = Vec3(0, 0, 0.5) # 向上的浮力 member['velocity'] += buoyancy_force * dt def update_physics(self, member: Dict, dt: float = 1.0/60.0): """ 更新物理状态 """ # 应用重力 self.apply_gravity(member, dt) # 应用空气阻力 self.apply_air_resistance(member, dt) class AdvancedBoidsAlgorithm: """ 高级Boids算法实现类 包含空间分区优化、物理模拟和高级行为规则 """ def __init__(self, config): self.config = config # 初始化空间分区系统 bounds = { 'min_x': -100, 'max_x': 100, 'min_y': -100, 'max_y': 100, 'min_z': -10, 'max_z': 100 } cell_size = self.config.get('perception_radius', 10.0) # 使用感知半径作为网格大小 self.spatial_partition = SpatialPartition(bounds, cell_size) # 初始化物理模拟器 self.physics_simulator = PhysicsSimulator() # 用于记录性能统计 self.stats = { 'neighbor_lookups': 0, 'total_neighbors_found': 0, 'algorithm_calls': 0 } def calculate_cohesion(self, member, neighbors): """ 计算聚集力 个体向邻居群集中心移动 """ if not neighbors: return Vec3(0, 0, 0) # 计算邻居的中心位置 center = Vec3(0, 0, 0) for neighbor in neighbors: center += neighbor['position'] center /= len(neighbors) # 返回指向中心的向量 return (center - member['position']).normalized() def calculate_separation(self, member, neighbors): """ 计算分离力 个体避免与邻居过于靠近 """ if not neighbors: return Vec3(0, 0, 0) separation = Vec3(0, 0, 0) for neighbor in neighbors: diff = member['position'] - neighbor['position'] distance = diff.length() if distance > 0: # 避免除零 # 距离越近，分离力越大 separation += diff.normalized() / distance if len(neighbors) > 0: separation /= len(neighbors) return separation.normalized() if separation.length() > 0 else separation def calculate_alignment(self, member, neighbors): """ 计算对齐力 个体与邻居的移动方向保持一致 """ if not neighbors: return Vec3(0, 0, 0) # 计算邻居的平均速度方向 avg_velocity = Vec3(0, 0, 0) for neighbor in neighbors: avg_velocity += neighbor['velocity'] avg_velocity /= len(neighbors) return avg_velocity.normalized() def calculate_obstacle_avoidance(self, member, obstacles): """ 计算避障力 避开场景中的障碍物 """ if not obstacles: return Vec3(0, 0, 0) avoidance = Vec3(0, 0, 0) for obstacle in obstacles: diff = member['position'] - obstacle['position'] distance = diff.length() if distance < obstacle['radius']: avoidance += diff.normalized() / distance return avoidance.normalized() if avoidance.length() > 0 else avoidance def calculate_seek(self, member, target): """ 计算寻求力 向目标位置移动 """ desired = target - member['position'] distance = desired.length() if distance > 0: desired.normalize() # 如果启用了达到行为，当接近目标时减速 if self.config.get("arrival_enabled", False): slow_radius = self.config.get("arrival_slow_radius", 5.0) if distance < slow_radius: desired *= self.config.get("max_speed", 5.0) * (distance / slow_radius) else: desired *= self.config.get("max_speed", 5.0) else: desired *= self.config.get("max_speed", 5.0) # 计算转向力 steer = desired - member['velocity'] # 限制转向力 max_force = self.config.get("max_force", 0.5) if steer.length() > max_force: steer.normalize() steer *= max_force return steer return Vec3(0, 0, 0) def calculate_flee(self, member, target): """ 计算逃离力 远离目标位置 """ desired = member['position'] - target distance = desired.length() if distance > 0: desired.normalize() desired *= self.config.get("max_speed", 5.0) # 计算转向力 steer = desired - member['velocity'] # 限制转向力 max_force = self.config.get("max_force", 0.5) if steer.length() > max_force: steer.normalize() steer *= max_force return steer return Vec3(0, 0, 0) def calculate_wander(self, member): """ 计算游走力 产生自然的随机移动 """ # 获取游走参数 wander_radius = self.config.get("wander_radius", 5.0) wander_distance = self.config.get("wander_distance", 10.0) wander_jitter = self.config.get("wander_jitter", 1.0) # 如果成员没有游走目标，创建一个 if 'wander_target' not in member: member['wander_target'] = Vec3( random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1) ).normalized() * wander_radius # 添加随机扰动 jitter = Vec3( random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1) ).normalized() * wander_jitter member['wander_target'] += jitter member['wander_target'].normalize() member['wander_target'] *= wander_radius # 计算游走目标在前方的距离 target = member['wander_target'] + Vec3(0, 0, 0) # 相对于成员前方的点 target *= wander_distance # 转换到世界坐标系 # 这里简化处理，实际应用中需要考虑成员的朝向 desired = target - member['position'] if desired.length() > 0: desired.normalize() desired *= self.config.get("max_speed", 5.0) # 计算转向力 steer = desired - member['velocity'] # 限制转向力 max_force = self.config.get("max_force", 0.5) if steer.length() > max_force: steer.normalize() steer *= max_force return steer return Vec3(0, 0, 0) def calculate_boundaries(self, member, bounds): """ 计算边界力 将成员保持在指定区域内 """ if not bounds: return Vec3(0, 0, 0) # 获取边界参数 min_x, max_x = bounds['min_x'], bounds['max_x'] min_y, max_y = bounds['min_y'], bounds['max_y'] min_z, max_z = bounds['min_z'], bounds['max_z'] # 边界缓冲距离 buffer = self.config.get("boundary_buffer", 5.0) # 计算边界力 bound_force = Vec3(0, 0, 0) if member['position'].x < min_x + buffer: bound_force.x = 1.0 elif member['position'].x > max_x - buffer: bound_force.x = -1.0 if member['position'].y < min_y + buffer: bound_force.y = 1.0 elif member['position'].y > max_y - buffer: bound_force.y = -1.0 if member['position'].z < min_z + buffer: bound_force.z = 1.0 elif member['position'].z > max_z - buffer: bound_force.z = -1.0 # 应用边界权重 bound_force *= self.config.get("boundary_weight", 2.0) return bound_force def calculate_follow_path(self, member, path): """ 计算路径跟随力 沿着预定义路径移动 """ if not path or len(path) < 2: return Vec3(0, 0, 0) # 获取路径参数 path_radius = self.config.get("path_radius", 3.0) # 找到最近的路径点 nearest_index = 0 nearest_distance = float('inf') for i, point in enumerate(path): distance = (member['position'] - point).length() if distance < nearest_distance: nearest_distance = distance nearest_index = i # 预测位置 prediction = member['velocity'].normalized() prediction *= self.config.get("prediction_distance", 10.0) predict_pos = member['position'] + prediction # 找到预测位置最近的路径点 predict_nearest_index = 0 predict_nearest_distance = float('inf') for i, point in enumerate(path): distance = (predict_pos - point).length() if distance < predict_nearest_distance: predict_nearest_distance = distance predict_nearest_index = i # 如果偏离路径太远，直接寻求最近的路径点 if nearest_distance > path_radius: target = path[nearest_index] return self.calculate_seek(member, target) # 否则寻求预测位置前方的路径点 if predict_nearest_index < len(path) - 1: target = path[predict_nearest_index + 1] return self.calculate_seek(member, target) return Vec3(0, 0, 0) def calculate_flock_formation(self, member, neighbors, formation_type): """ 计算队形保持力 维持特定的群体队形 """ if not neighbors: return Vec3(0, 0, 0) # 根据队形类型计算期望位置 target_position = Vec3(0, 0, 0) if formation_type == "V": # V字形队形 # 简化实现，实际应用中需要更复杂的计算 if 'index' in member: index = member['index'] angle = math.radians(index * 15) # 每个成员间隔15度 distance = 5.0 # 固定间距 target_position = Vec3( math.cos(angle) * distance, math.sin(angle) * distance, 0 ) elif formation_type == "line": # 直线队形 if 'index' in member: index = member['index'] spacing = 3.0 target_position = Vec3(0, index * spacing, 0) elif formation_type == "circle": # 圆形队形 if 'index' in member: index = member['index'] total = len(neighbors) + 1 angle = 2 * math.pi * index / total radius = 10.0 target_position = Vec3( math.cos(angle) * radius, math.sin(angle) * radius, 0 ) # 计算到达期望位置的力 if target_position.length() > 0: desired = target_position - member['position'] if desired.length() > 0: desired.normalize() desired *= self.config.get("max_speed", 5.0) # 计算转向力 steer = desired - member['velocity'] # 应用队形权重 steer *= self.config.get("formation_weight", 1.0) # 限制转向力 max_force = self.config.get("max_force", 0.5) if steer.length() > max_force: steer.normalize() steer *= max_force return steer return Vec3(0, 0, 0) def calculate_avoid_predator(self, member, predators): """ 计算躲避捕食者力 远离捕食者 """ if not predators: return Vec3(0, 0, 0) avoidance = Vec3(0, 0, 0) predator_radius = self.config.get("predator_radius", 15.0) for predator in predators: diff = member['position'] - predator['position'] distance = diff.length() if distance < predator_radius: # 距离越近，躲避力越大 force = diff.normalized() / (distance / predator_radius) avoidance += force # 应用捕食者躲避权重 avoidance *= self.config.get("predator_avoid_weight", 3.0) return avoidance.normalized() if avoidance.length() > 0 else avoidance def update_member(self, member, neighbors, obstacles=[], target=None, bounds=None, path=None, predators=[]): """ 更新群体成员状态 计算所有作用力并更新成员的位置和速度 """ # 计算基本Boids力 cohesion_force = self.calculate_cohesion(member, neighbors) separation_force = self.calculate_separation(member, neighbors) alignment_force = self.calculate_alignment(member, neighbors) # 计算扩展力 obstacle_force = self.calculate_obstacle_avoidance(member, obstacles) bound_force = self.calculate_boundaries(member, bounds) predator_force = self.calculate_avoid_predator(member, predators) # 计算目标导向力 seek_force = Vec3(0, 0, 0) if target: seek_force = self.calculate_seek(member, target) # 计算路径跟随力 path_force = Vec3(0, 0, 0) if path: path_force = self.calculate_follow_path(member, path) # 计算游走力 wander_force = Vec3(0, 0, 0) if self.config.get("wander_enabled", False): wander_force = self.calculate_wander(member) # 应用权重 cohesion_force *= self.config.get("cohesion_weight", 1.0) separation_force *= self.config.get("separation_weight", 1.5) alignment_force *= self.config.get("alignment_weight", 1.0) obstacle_force *= self.config.get("obstacle_weight", 2.0) seek_force *= self.config.get("seek_weight", 1.0) bound_force *= self.config.get("boundary_weight", 2.0) path_force *= self.config.get("path_weight", 1.0) wander_force *= self.config.get("wander_weight", 0.5) predator_force *= self.config.get("predator_avoid_weight", 3.0) # 计算总加速度 acceleration = ( cohesion_force + separation_force + alignment_force + obstacle_force + seek_force + bound_force + path_force + wander_force + predator_force ) # 应用加速度限制 max_acceleration = self.config.get("max_acceleration", 1.0) if acceleration.length() > max_acceleration: acceleration.normalize() acceleration *= max_acceleration # 更新速度和位置 member['velocity'] += acceleration # 限制最大速度 max_speed = self.config.get("max_speed", 5.0) if member['velocity'].length() > max_speed: member['velocity'].normalize() member['velocity'] *= max_speed member['position'] += member['velocity'] # 更新成员的朝向（使其面向移动方向） if member['velocity'].length() > 0 and 'node' in member: # 简化实现，实际应用中可能需要更复杂的朝向计算 member['node'].lookAt(member['position'] + member['velocity'])