pengxiao
219232de74
以讲义内容为骨架迁移到标准目录格式： - officefile/ 主内容（12章 + 附录 + CC4SI补充） - dofile/ 代码示例（11个Python脚本） - data/ 图片资源 - output/ 生成输出（忽略） - Archive/ 归档旧目录（忽略） - .claude/skills/ 保留markdown-to-docx工具链 - .pandoc/ 保留CSL和本地化配置 Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
958 lines
28 KiB
Python
958 lines
28 KiB
Python
"""
|
|
空间推理示例 (Spatial Reasoning Example)
|
|
=======================================
|
|
|
|
本示例展示空间智能系统中的空间推理方法。
|
|
空间推理是从已知空间事实推导新知识的过程。
|
|
|
|
核心概念:
|
|
1. 定性推理 - 使用定性术语描述空间关系
|
|
2. 定量推理 - 使用精确数值计算
|
|
3. 空间逻辑 - 形式化的空间推理规则
|
|
4. 路径规划 - 寻找最优路径
|
|
5. 可见性分析 - 判断视线可见性
|
|
|
|
应用场景:
|
|
- 导航与路径规划
|
|
- 空间查询与分析
|
|
- 地理推理系统
|
|
- 机器人导航
|
|
|
|
作者: CC4SI 项目组
|
|
"""
|
|
|
|
import math
|
|
import heapq
|
|
from typing import List, Dict, Tuple, Optional, Set, Any
|
|
from dataclasses import dataclass, field
|
|
from enum import Enum
|
|
from abc import ABC, abstractmethod
|
|
import random
|
|
|
|
# 导入空间表征示例中的基础类
|
|
import sys
|
|
import os
|
|
sys.path.append(os.path.dirname(__file__))
|
|
|
|
try:
|
|
from spatial_representation import Point, LineString, Polygon, Envelope
|
|
except ImportError:
|
|
# 如果导入失败，定义简化版本
|
|
@dataclass
|
|
class Point:
|
|
x: float
|
|
y: float
|
|
def distance_to(self, other):
|
|
return math.sqrt((self.x - other.x)**2 + (self.y - other.y)**2)
|
|
def __repr__(self):
|
|
return f"({self.x:.2f}, {self.y:.2f})"
|
|
|
|
|
|
# ============================================================================
|
|
# 定性空间推理
|
|
# ============================================================================
|
|
|
|
class CardinalDirection(Enum):
|
|
"""基本方向"""
|
|
NORTH = "N"
|
|
SOUTH = "S"
|
|
EAST = "E"
|
|
WEST = "W"
|
|
NORTHEAST = "NE"
|
|
NORTHWEST = "NW"
|
|
SOUTHEAST = "SE"
|
|
SOUTHWEST = "SW"
|
|
|
|
@classmethod
|
|
def from_angle(cls, angle: float) -> 'CardinalDirection':
|
|
"""
|
|
从角度获取方向
|
|
|
|
Args:
|
|
angle: 角度 (度, 0=东, 90=北)
|
|
|
|
Returns:
|
|
方向枚举
|
|
"""
|
|
# 归一化到0-360
|
|
angle = angle % 360
|
|
|
|
if angle >= 337.5 or angle < 22.5:
|
|
return cls.EAST
|
|
elif 22.5 <= angle < 67.5:
|
|
return cls.NORTHEAST
|
|
elif 67.5 <= angle < 112.5:
|
|
return cls.NORTH
|
|
elif 112.5 <= angle < 157.5:
|
|
return cls.NORTHWEST
|
|
elif 157.5 <= angle < 202.5:
|
|
return cls.WEST
|
|
elif 202.5 <= angle < 247.5:
|
|
return cls.SOUTHWEST
|
|
elif 247.5 <= angle < 292.5:
|
|
return cls.SOUTH
|
|
else: # 292.5 <= angle < 337.5
|
|
return cls.SOUTHEAST
|
|
|
|
|
|
@dataclass
|
|
class QualitativeRelation:
|
|
"""定性空间关系"""
|
|
relation_type: str # "direction", "distance", "topology"
|
|
value: str # 如 "north", "near", "inside"
|
|
confidence: float = 1.0
|
|
|
|
def __repr__(self) -> str:
|
|
return f"{self.relation_type}={self.value} (conf={self.confidence:.2f})"
|
|
|
|
|
|
class QualitativeReasoner:
|
|
"""
|
|
定性空间推理器
|
|
|
|
使用定性术语进行空间推理。
|
|
"""
|
|
|
|
def __init__(self):
|
|
self.facts: List[Tuple[str, str, QualitativeRelation]] = []
|
|
|
|
def add_fact(self, entity1: str, entity2: str,
|
|
relation: QualitativeRelation) -> None:
|
|
"""添加空间事实"""
|
|
self.facts.append((entity1, entity2, relation))
|
|
|
|
def infer_direction(self, from_point: Point, to_point: Point) -> QualitativeRelation:
|
|
"""
|
|
推断两点之间的方向关系
|
|
|
|
Args:
|
|
from_point: 起始点
|
|
to_point: 目标点
|
|
|
|
Returns:
|
|
方向关系
|
|
"""
|
|
dx = to_point.x - from_point.x
|
|
dy = to_point.y - from_point.y
|
|
|
|
# 计算角度 (从东开始逆时针)
|
|
angle = math.degrees(math.atan2(dy, dx))
|
|
|
|
direction = CardinalDirection.from_angle(angle)
|
|
|
|
return QualitativeRelation(
|
|
relation_type="direction",
|
|
value=direction.value,
|
|
confidence=1.0
|
|
)
|
|
|
|
def infer_distance_category(self, p1: Point, p2: Point,
|
|
thresholds: Dict[str, float] = None) -> QualitativeRelation:
|
|
"""
|
|
推断距离类别
|
|
|
|
Args:
|
|
p1: 第一个点
|
|
p2: 第二个点
|
|
thresholds: 距离阈值字典
|
|
|
|
Returns:
|
|
距离类别关系
|
|
"""
|
|
if thresholds is None:
|
|
thresholds = {
|
|
"very_close": 100,
|
|
"close": 500,
|
|
"moderate": 1000,
|
|
"far": 5000
|
|
}
|
|
|
|
dist = p1.distance_to(p2)
|
|
|
|
if dist < thresholds.get("very_close", 100):
|
|
category = "very_close"
|
|
elif dist < thresholds.get("close", 500):
|
|
category = "close"
|
|
elif dist < thresholds.get("moderate", 1000):
|
|
category = "moderate"
|
|
else:
|
|
category = "far"
|
|
|
|
return QualitativeRelation(
|
|
relation_type="distance",
|
|
value=category,
|
|
confidence=1.0
|
|
)
|
|
|
|
def compose_relations(self, rel1: QualitativeRelation,
|
|
rel2: QualitativeRelation) -> QualitativeRelation:
|
|
"""
|
|
组合两个关系
|
|
|
|
例如: A在B的北边，B在C的东边 -> A在C的东北边
|
|
"""
|
|
if rel1.relation_type == "direction" and rel2.relation_type == "direction":
|
|
# 方向组合
|
|
return self._compose_directions(rel1.value, rel2.value)
|
|
|
|
return QualitativeRelation(
|
|
relation_type="unknown",
|
|
value="unknown",
|
|
confidence=0.5
|
|
)
|
|
|
|
def _compose_directions(self, dir1: str, dir2: str) -> QualitativeRelation:
|
|
"""组合两个方向"""
|
|
direction_map = {
|
|
"N": (0, 1), "S": (0, -1), "E": (1, 0), "W": (-1, 0),
|
|
"NE": (1, 1), "NW": (-1, 1), "SE": (1, -1), "SW": (-1, -1)
|
|
}
|
|
|
|
if dir1 in direction_map and dir2 in direction_map:
|
|
v1 = direction_map[dir1]
|
|
v2 = direction_map[dir2]
|
|
|
|
# 向量相加
|
|
result = (v1[0] + v2[0], v1[1] + v2[1])
|
|
|
|
# 找到最接近的方向
|
|
best_dir = "unknown"
|
|
best_dot = -1
|
|
|
|
for name, vec in direction_map.items():
|
|
dot = result[0] * vec[0] + result[1] * vec[1]
|
|
if dot > best_dot:
|
|
best_dot = dot
|
|
best_dir = name
|
|
|
|
return QualitativeRelation(
|
|
relation_type="direction",
|
|
value=best_dir,
|
|
confidence=0.8
|
|
)
|
|
|
|
return QualitativeRelation("direction", "unknown", 0.3)
|
|
|
|
|
|
# ============================================================================
|
|
# 路径规划
|
|
# ============================================================================
|
|
|
|
@dataclass
|
|
class Node:
|
|
"""图节点"""
|
|
id: str
|
|
point: Point
|
|
neighbors: List[str] = field(default_factory=list)
|
|
|
|
def __hash__(self):
|
|
return hash(self.id)
|
|
|
|
|
|
@dataclass
|
|
class Edge:
|
|
"""图边"""
|
|
from_node: str
|
|
to_node: str
|
|
weight: float # 权重 (如距离)
|
|
bidirectional: bool = True
|
|
|
|
|
|
class SpatialGraph:
|
|
"""
|
|
空间图
|
|
|
|
用于路径规划的空间网络结构。
|
|
"""
|
|
|
|
def __init__(self):
|
|
self.nodes: Dict[str, Node] = {}
|
|
self.edges: List[Edge] = []
|
|
|
|
def add_node(self, id: str, point: Point) -> Node:
|
|
"""添加节点"""
|
|
node = Node(id=id, point=point)
|
|
self.nodes[id] = node
|
|
return node
|
|
|
|
def add_edge(self, from_id: str, to_id: str, weight: float = None,
|
|
bidirectional: bool = True) -> None:
|
|
"""添加边"""
|
|
if from_id not in self.nodes or to_id not in self.nodes:
|
|
raise ValueError("节点不存在")
|
|
|
|
# 如果未指定权重，使用欧氏距离
|
|
if weight is None:
|
|
weight = self.nodes[from_id].point.distance_to(self.nodes[to_id].point)
|
|
|
|
edge = Edge(from_id, to_id, weight, bidirectional)
|
|
self.edges.append(edge)
|
|
|
|
# 更新邻接关系
|
|
self.nodes[from_id].neighbors.append(to_id)
|
|
if bidirectional:
|
|
self.nodes[to_id].neighbors.append(from_id)
|
|
|
|
def get_edge_weight(self, from_id: str, to_id: str) -> float:
|
|
"""获取边的权重"""
|
|
for edge in self.edges:
|
|
if edge.from_node == from_id and edge.to_node == to_id:
|
|
return edge.weight
|
|
if edge.bidirectional and edge.from_node == to_id and edge.to_node == from_id:
|
|
return edge.weight
|
|
return float('inf')
|
|
|
|
def shortest_path(self, start_id: str, end_id: str) -> Optional[List[str]]:
|
|
"""
|
|
使用Dijkstra算法计算最短路径
|
|
|
|
Args:
|
|
start_id: 起始节点ID
|
|
end_id: 目标节点ID
|
|
|
|
Returns:
|
|
节点ID列表，表示路径
|
|
"""
|
|
if start_id not in self.nodes or end_id not in self.nodes:
|
|
return None
|
|
|
|
# 优先队列: (距离, 节点ID)
|
|
pq = [(0, start_id)]
|
|
# 距离字典
|
|
distances = {node_id: float('inf') for node_id in self.nodes}
|
|
distances[start_id] = 0
|
|
# 前驱节点
|
|
previous = {start_id: None}
|
|
# 已访问
|
|
visited = set()
|
|
|
|
while pq:
|
|
current_dist, current_id = heapq.heappop(pq)
|
|
|
|
if current_id in visited:
|
|
continue
|
|
visited.add(current_id)
|
|
|
|
if current_id == end_id:
|
|
break
|
|
|
|
# 检查所有邻居
|
|
for neighbor_id in self.nodes[current_id].neighbors:
|
|
if neighbor_id in visited:
|
|
continue
|
|
|
|
edge_weight = self.get_edge_weight(current_id, neighbor_id)
|
|
new_dist = current_dist + edge_weight
|
|
|
|
if new_dist < distances[neighbor_id]:
|
|
distances[neighbor_id] = new_dist
|
|
previous[neighbor_id] = current_id
|
|
heapq.heappush(pq, (new_dist, neighbor_id))
|
|
|
|
# 重建路径
|
|
if distances[end_id] == float('inf'):
|
|
return None
|
|
|
|
path = []
|
|
current = end_id
|
|
while current is not None:
|
|
path.append(current)
|
|
current = previous.get(current)
|
|
|
|
path.reverse()
|
|
return path
|
|
|
|
def shortest_path_distance(self, start_id: str, end_id: str) -> float:
|
|
"""获取最短路径距离"""
|
|
path = self.shortest_path(start_id, end_id)
|
|
if not path:
|
|
return float('inf')
|
|
|
|
total = 0.0
|
|
for i in range(len(path) - 1):
|
|
total += self.get_edge_weight(path[i], path[i + 1])
|
|
|
|
return total
|
|
|
|
|
|
# ============================================================================
|
|
# A*路径规划
|
|
# ============================================================================
|
|
|
|
class AStarPlanner:
|
|
"""
|
|
A*路径规划器
|
|
|
|
使用启发式搜索的高效路径规划算法。
|
|
"""
|
|
|
|
def __init__(self, graph: SpatialGraph):
|
|
self.graph = graph
|
|
|
|
def heuristic(self, node_id: str, goal_id: str) -> float:
|
|
"""
|
|
启发式函数 (使用欧氏距离)
|
|
|
|
Args:
|
|
node_id: 当前节点
|
|
goal_id: 目标节点
|
|
|
|
Returns:
|
|
启发式估计值
|
|
"""
|
|
if node_id not in self.graph.nodes or goal_id not in self.graph.nodes:
|
|
return 0.0
|
|
|
|
return self.graph.nodes[node_id].point.distance_to(
|
|
self.graph.nodes[goal_id].point
|
|
)
|
|
|
|
def plan(self, start_id: str, goal_id: str) -> Optional[Tuple[List[str], float]]:
|
|
"""
|
|
规划路径
|
|
|
|
Args:
|
|
start_id: 起始节点ID
|
|
goal_id: 目标节点ID
|
|
|
|
Returns:
|
|
(路径节点列表, 总距离) 或 None
|
|
"""
|
|
if start_id not in self.graph.nodes or goal_id not in self.graph.nodes:
|
|
return None
|
|
|
|
# 开集和闭集
|
|
open_set = {start_id}
|
|
closed_set = set()
|
|
|
|
# g值: 从起点到当前节点的实际距离
|
|
g_score = {node_id: float('inf') for node_id in self.graph.nodes}
|
|
g_score[start_id] = 0
|
|
|
|
# f值: g值 + 启发式值
|
|
f_score = {node_id: float('inf') for node_id in self.graph.nodes}
|
|
f_score[start_id] = self.heuristic(start_id, goal_id)
|
|
|
|
# 前驱节点
|
|
came_from = {}
|
|
|
|
while open_set:
|
|
# 获取f值最小的节点
|
|
current = min(open_set, key=lambda x: f_score[x])
|
|
|
|
if current == goal_id:
|
|
# 重建路径
|
|
path = [current]
|
|
total_distance = g_score[current]
|
|
|
|
while current in came_from:
|
|
current = came_from[current]
|
|
path.append(current)
|
|
|
|
path.reverse()
|
|
return (path, total_distance)
|
|
|
|
open_set.remove(current)
|
|
closed_set.add(current)
|
|
|
|
# 检查邻居
|
|
for neighbor in self.graph.nodes[current].neighbors:
|
|
if neighbor in closed_set:
|
|
continue
|
|
|
|
# 计算 tentative_g_score
|
|
edge_weight = self.graph.get_edge_weight(current, neighbor)
|
|
tentative_g = g_score[current] + edge_weight
|
|
|
|
if neighbor not in open_set:
|
|
open_set.add(neighbor)
|
|
elif tentative_g >= g_score[neighbor]:
|
|
continue
|
|
|
|
# 更新
|
|
came_from[neighbor] = current
|
|
g_score[neighbor] = tentative_g
|
|
f_score[neighbor] = tentative_g + self.heuristic(neighbor, goal_id)
|
|
|
|
return None # 没有找到路径
|
|
|
|
|
|
# ============================================================================
|
|
# 可见性分析
|
|
# ============================================================================
|
|
|
|
class VisibilityAnalyzer:
|
|
"""
|
|
可见性分析器
|
|
|
|
判断点之间的可见性，考虑障碍物。
|
|
"""
|
|
|
|
def __init__(self, obstacles: List[Polygon] = None):
|
|
"""
|
|
初始化可见性分析器
|
|
|
|
Args:
|
|
obstacles: 障碍物多边形列表
|
|
"""
|
|
self.obstacles = obstacles or []
|
|
|
|
def add_obstacle(self, obstacle: Polygon) -> None:
|
|
"""添加障碍物"""
|
|
self.obstacles.append(obstacle)
|
|
|
|
def is_visible(self, p1: Point, p2: Point,
|
|
tolerance: float = 1e-6) -> bool:
|
|
"""
|
|
判断两点之间是否可见
|
|
|
|
Args:
|
|
p1: 第一个点
|
|
p2: 第二个点
|
|
tolerance: 容差
|
|
|
|
Returns:
|
|
是否可见
|
|
"""
|
|
# 检视线是否与任何障碍物相交
|
|
for obstacle in self.obstacles:
|
|
if self._line_intersects_polygon(p1, p2, obstacle):
|
|
return False
|
|
return True
|
|
|
|
def _line_intersects_polygon(self, p1: Point, p2: Point,
|
|
polygon: Polygon) -> bool:
|
|
"""判断线段是否与多边形相交"""
|
|
# 首先检查包围盒
|
|
line_min_x = min(p1.x, p2.x)
|
|
line_max_x = max(p1.x, p2.x)
|
|
line_min_y = min(p1.y, p2.y)
|
|
line_max_y = max(p1.y, p2.y)
|
|
|
|
poly_min_x = min(p.x for p in polygon.exterior)
|
|
poly_max_x = max(p.x for p in polygon.exterior)
|
|
poly_min_y = min(p.y for p in polygon.exterior)
|
|
poly_max_y = max(p.y for p in polygon.exterior)
|
|
|
|
# 包围盒不相交
|
|
if line_max_x < poly_min_x or line_min_x > poly_max_x or \
|
|
line_max_y < poly_min_y or line_min_y > poly_max_y:
|
|
return False
|
|
|
|
# 检查线段是否与多边形的任何边相交
|
|
n = len(polygon.exterior)
|
|
for i in range(n):
|
|
v1 = polygon.exterior[i]
|
|
v2 = polygon.exterior[(i + 1) % n]
|
|
|
|
if self._segments_intersect(p1, p2, v1, v2):
|
|
return True
|
|
|
|
return False
|
|
|
|
def _segments_intersect(self, p1: Point, p2: Point,
|
|
p3: Point, p4: Point) -> bool:
|
|
"""判断两条线段是否相交"""
|
|
def orientation(a, b, c):
|
|
val = (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y)
|
|
if abs(val) < 1e-10:
|
|
return 0 # 共线
|
|
return 1 if val > 0 else 2 # 顺时针/逆时针
|
|
|
|
def on_segment(a, b, c):
|
|
return min(a.x, c.x) <= b.x <= max(a.x, c.x) and \
|
|
min(a.y, c.y) <= b.y <= max(a.y, c.y)
|
|
|
|
o1 = orientation(p1, p2, p3)
|
|
o2 = orientation(p1, p2, p4)
|
|
o3 = orientation(p3, p4, p1)
|
|
o4 = orientation(p3, p4, p2)
|
|
|
|
# 一般情况
|
|
if o1 != o2 and o3 != o4:
|
|
return True
|
|
|
|
# 特殊情况
|
|
if o1 == 0 and on_segment(p1, p3, p2):
|
|
return True
|
|
if o2 == 0 and on_segment(p1, p4, p2):
|
|
return True
|
|
if o3 == 0 and on_segment(p3, p1, p4):
|
|
return True
|
|
if o4 == 0 and on_segment(p3, p2, p4):
|
|
return True
|
|
|
|
return False
|
|
|
|
def viewshed(self, observer: Point, radius: float,
|
|
num_rays: int = 360) -> List[Tuple[Point, bool]]:
|
|
"""
|
|
计算视域 (可视范围)
|
|
|
|
Args:
|
|
observer: 观察点
|
|
radius: 视距
|
|
num_rays: 射线数量
|
|
|
|
Returns:
|
|
(点, 可见性) 列表
|
|
"""
|
|
results = []
|
|
|
|
for i in range(num_rays):
|
|
angle = 2 * math.pi * i / num_rays
|
|
target = Point(
|
|
observer.x + radius * math.cos(angle),
|
|
observer.y + radius * math.sin(angle)
|
|
)
|
|
|
|
visible = self.is_visible(observer, target)
|
|
results.append((target, visible))
|
|
|
|
return results
|
|
|
|
|
|
# ============================================================================
|
|
# 空间推理引擎
|
|
# ============================================================================
|
|
|
|
class SpatialReasoningEngine:
|
|
"""
|
|
空间推理引擎
|
|
|
|
集成多种空间推理功能的综合引擎。
|
|
"""
|
|
|
|
def __init__(self):
|
|
self.qualitative_reasoner = QualitativeReasoner()
|
|
self.graph = SpatialGraph()
|
|
self.visibility_analyzer = VisibilityAnalyzer()
|
|
self.astar_planner = None
|
|
|
|
def build_road_network(self, points: List[Tuple[str, Point]],
|
|
connections: List[Tuple[str, str]]) -> None:
|
|
"""
|
|
构建道路网络
|
|
|
|
Args:
|
|
points: (节点ID, 点) 列表
|
|
connections: (节点1, 节点2) 连接列表
|
|
"""
|
|
for id, point in points:
|
|
self.graph.add_node(id, point)
|
|
|
|
for id1, id2 in connections:
|
|
self.graph.add_edge(id1, id2)
|
|
|
|
self.astar_planner = AStarPlanner(self.graph)
|
|
|
|
def navigate(self, start: str, goal: str) -> Optional[Dict[str, Any]]:
|
|
"""
|
|
导航规划
|
|
|
|
Args:
|
|
start: 起始点ID
|
|
goal: 目标点ID
|
|
|
|
Returns:
|
|
导航结果字典
|
|
"""
|
|
if not self.astar_planner:
|
|
return None
|
|
|
|
result = self.astar_planner.plan(start, goal)
|
|
|
|
if result:
|
|
path, distance = result
|
|
|
|
# 计算方向指示
|
|
directions = []
|
|
for i in range(len(path) - 1):
|
|
from_node = self.graph.nodes[path[i]]
|
|
to_node = self.graph.nodes[path[i + 1]]
|
|
|
|
relation = self.qualitative_reasoner.infer_direction(
|
|
from_node.point, to_node.point
|
|
)
|
|
directions.append({
|
|
"from": path[i],
|
|
"to": path[i + 1],
|
|
"direction": relation.value,
|
|
"distance": from_node.point.distance_to(to_node.point)
|
|
})
|
|
|
|
return {
|
|
"path": path,
|
|
"total_distance": distance,
|
|
"num_steps": len(path) - 1,
|
|
"directions": directions
|
|
}
|
|
|
|
return None
|
|
|
|
def query_relation(self, entity1: str, entity2: str,
|
|
point1: Point, point2: Point) -> Dict[str, Any]:
|
|
"""
|
|
查询两个实体之间的空间关系
|
|
|
|
Args:
|
|
entity1: 实体1名称
|
|
entity2: 实体2名称
|
|
point1: 实体1位置
|
|
point2: 实体2位置
|
|
|
|
Returns:
|
|
关系字典
|
|
"""
|
|
direction = self.qualitative_reasoner.infer_direction(point1, point2)
|
|
distance_cat = self.qualitative_reasoner.infer_distance_category(point1, point2)
|
|
actual_distance = point1.distance_to(point2)
|
|
|
|
return {
|
|
"entity1": entity1,
|
|
"entity2": entity2,
|
|
"direction": direction.value,
|
|
"distance_category": distance_cat.value,
|
|
"actual_distance": actual_distance,
|
|
"bearing": math.degrees(math.atan2(
|
|
point2.y - point1.y,
|
|
point2.x - point1.x
|
|
))
|
|
}
|
|
|
|
|
|
# ============================================================================
|
|
# 主程序
|
|
# ========================================================================
|
|
|
|
def main():
|
|
"""主程序 - 演示空间推理的使用"""
|
|
|
|
print("="*70)
|
|
print("空间推理示例演示")
|
|
print("="*70)
|
|
|
|
# ========================================================================
|
|
# 1. 定性空间推理
|
|
# ========================================================================
|
|
print("\n[部分 1] 定性空间推理")
|
|
print("-" * 50)
|
|
|
|
reasoner = QualitativeReasoner()
|
|
|
|
# 创建几个地标点
|
|
landmarks = {
|
|
"西湖": Point(120.148, 30.259),
|
|
"钱塘江": Point(120.250, 30.200),
|
|
"滨江": Point(120.350, 30.220),
|
|
"萧山": Point(120.400, 30.150)
|
|
}
|
|
|
|
print("\n地标位置:")
|
|
for name, point in landmarks.items():
|
|
print(f" {name}: {point}")
|
|
|
|
# 推断方向关系
|
|
print("\n方向关系:")
|
|
for i, (name1, point1) in enumerate(list(landmarks.items())[:-1]):
|
|
name2 = list(landmarks.keys())[i + 1]
|
|
point2 = landmarks[name2]
|
|
|
|
relation = reasoner.infer_direction(point1, point2)
|
|
dist_relation = reasoner.infer_distance_category(point1, point2)
|
|
|
|
print(f" {name1} -> {name2}:")
|
|
print(f" 方向: {relation.value}")
|
|
print(f" 距离类别: {dist_relation.value}")
|
|
|
|
# ========================================================================
|
|
# 2. 路径规划
|
|
# ========================================================================
|
|
print("\n\n[部分 2] 路径规划")
|
|
print("-" * 50)
|
|
|
|
# 创建道路网络
|
|
network_points = [
|
|
("A", Point(0, 0)),
|
|
("B", Point(50, 30)),
|
|
("C", Point(100, 50)),
|
|
("D", Point(30, 80)),
|
|
("E", Point(80, 100)),
|
|
("F", Point(120, 120)),
|
|
("G", Point(150, 60))
|
|
]
|
|
|
|
connections = [
|
|
("A", "B"), ("B", "C"), ("A", "D"),
|
|
("B", "D"), ("D", "E"), ("C", "E"),
|
|
("E", "F"), ("C", "G"), ("G", "F")
|
|
]
|
|
|
|
graph = SpatialGraph()
|
|
for id, point in network_points:
|
|
graph.add_node(id, point)
|
|
for id1, id2 in connections:
|
|
graph.add_edge(id1, id2)
|
|
|
|
print("\n道路网络:")
|
|
print(f" 节点数: {len(graph.nodes)}")
|
|
print(f" 边数: {len(graph.edges)}")
|
|
|
|
# Dijkstra最短路径
|
|
print("\nDijkstra最短路径 (A -> F):")
|
|
dijkstra_path = graph.shortest_path("A", "F")
|
|
if dijkstra_path:
|
|
print(f" 路径: {' -> '.join(dijkstra_path)}")
|
|
distance = graph.shortest_path_distance("A", "F")
|
|
print(f" 总距离: {distance:.2f}")
|
|
|
|
# A*路径规划
|
|
print("\nA*路径规划 (A -> F):")
|
|
astar = AStarPlanner(graph)
|
|
astar_result = astar.plan("A", "F")
|
|
if astar_result:
|
|
path, dist = astar_result
|
|
print(f" 路径: {' -> '.join(path)}")
|
|
print(f" 总距离: {dist:.2f}")
|
|
|
|
# ========================================================================
|
|
# 3. 可见性分析
|
|
# ========================================================================
|
|
print("\n\n[部分 3] 可见性分析")
|
|
print("-" * 50)
|
|
|
|
# 创建障碍物
|
|
obstacle1 = Polygon(exterior=[
|
|
Point(60, 40),
|
|
Point(80, 40),
|
|
Point(80, 70),
|
|
Point(60, 70),
|
|
Point(60, 40)
|
|
])
|
|
|
|
obstacle2 = Polygon(exterior=[
|
|
Point(100, 80),
|
|
Point(130, 80),
|
|
Point(130, 110),
|
|
Point(100, 110),
|
|
Point(100, 80)
|
|
])
|
|
|
|
visibility = VisibilityAnalyzer([obstacle1, obstacle2])
|
|
|
|
print("\n障碍物:")
|
|
print(f" 障碍物1: {obstacle1}")
|
|
print(f" 障碍物2: {obstacle2}")
|
|
|
|
# 测试可见性
|
|
observer = Point(30, 50)
|
|
test_points = [
|
|
("目标A", Point(90, 50)), # 被障碍物1遮挡
|
|
("目标B", Point(120, 120)), # 被障碍物2遮挡
|
|
("目标C", Point(150, 30)), # 可见
|
|
("目标D", Point(50, 90)) # 可见
|
|
]
|
|
|
|
print(f"\n从观察点 {observer} 观察:")
|
|
for name, target in test_points:
|
|
visible = visibility.is_visible(observer, target)
|
|
status = "可见" if visible else "不可见"
|
|
print(f" {name} {target}: {status}")
|
|
|
|
# ========================================================================
|
|
# 4. 综合推理引擎
|
|
# ========================================================================
|
|
print("\n\n[部分 4] 综合空间推理引擎")
|
|
print("-" * 50)
|
|
|
|
engine = SpatialReasoningEngine()
|
|
|
|
# 构建城市路网
|
|
city_points = [
|
|
("火车站", Point(100, 100)),
|
|
("市政府", Point(150, 120)),
|
|
("西湖", Point(200, 100)),
|
|
("钱江新城", Point(180, 180)),
|
|
("滨江", Point(120, 200)),
|
|
("萧山机场", Point(250, 250))
|
|
]
|
|
|
|
city_connections = [
|
|
("火车站", "市政府"),
|
|
("火车站", "滨江"),
|
|
("市政府", "西湖"),
|
|
("市政府", "钱江新城"),
|
|
("滨江", "钱江新城"),
|
|
("钱江新城", "萧山机场"),
|
|
("西湖", "萧山机场")
|
|
]
|
|
|
|
engine.build_road_network(city_points, city_connections)
|
|
|
|
print("\n城市路网:")
|
|
for name, point in city_points:
|
|
print(f" {name}: {point}")
|
|
|
|
# 导航示例
|
|
print("\n导航示例: 从 火车站 到 萧山机场")
|
|
nav_result = engine.navigate("火车站", "萧山机场")
|
|
|
|
if nav_result:
|
|
print(f"\n路径规划结果:")
|
|
print(f" 路径: {' -> '.join(nav_result['path'])}")
|
|
print(f" 总距离: {nav_result['total_distance']:.2f}")
|
|
print(f" 步数: {nav_result['num_steps']}")
|
|
|
|
print(f"\n详细指引:")
|
|
for i, step in enumerate(nav_result['directions'], 1):
|
|
dir_map = {
|
|
"N": "向北", "S": "向南", "E": "向东", "W": "向西",
|
|
"NE": "向东北", "NW": "向西北", "SE": "向东南", "SW": "向西南"
|
|
}
|
|
direction_cn = dir_map.get(step['direction'], step['direction'])
|
|
print(f" {i}. 从 {step['from']} {direction_cn} 前往 {step['to']} "
|
|
f"(距离: {step['distance']:.1f})")
|
|
|
|
# 空间关系查询
|
|
print("\n空间关系查询:")
|
|
relation = engine.query_relation(
|
|
"火车站", "西湖",
|
|
city_points[0][1], city_points[2][1]
|
|
)
|
|
|
|
print(f" {relation['entity1']} 相对于 {relation['entity2']}:")
|
|
print(f" 方向: {relation['direction']}")
|
|
print(f" 距离: {relation['actual_distance']:.2f}")
|
|
print(f" 方位角: {relation['bearing']:.1f}°")
|
|
|
|
# ========================================================================
|
|
# 5. 多路径比较
|
|
# ========================================================================
|
|
print("\n\n[部分 5] 多路径比较")
|
|
print("-" * 50)
|
|
|
|
destinations = ["市政府", "西湖", "钱江新城", "滨江", "萧山机场"]
|
|
start = "火车站"
|
|
|
|
print(f"\n从 {start} 到各目的地的距离:")
|
|
results = []
|
|
for dest in destinations:
|
|
if dest == start:
|
|
continue
|
|
path_info = engine.navigate(start, dest)
|
|
if path_info:
|
|
results.append((dest, path_info['total_distance'], path_info['path']))
|
|
|
|
results.sort(key=lambda x: x[1])
|
|
|
|
for i, (dest, dist, path) in enumerate(results, 1):
|
|
print(f" {i}. {dest:8s}: {dist:6.1f} (路径: {' -> '.join(path)})")
|
|
|
|
print("\n" + "="*70)
|
|
print("演示完成!")
|
|
print("="*70)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|