Çarpışmasız yörünge: çarpışma denetimi ve RRT ile planlama

def seg_hits_box(a, b, lo, hi, pad):
    t0, t1 = 0.0, 1.0                    # doğru parçasının kutu içinde kaldığı [t0, t1] aralığı
    for i in range(3):
        d = b[i] - a[i]
        l, h = lo[i] - pad, hi[i] + pad  # kutuyu uzuv yarıçapı kadar şişir
        if abs(d) < 1e-12:
            if a[i] < l or a[i] > h:
                return False
            continue
        u, v = (l - a[i]) / d, (h - a[i]) / d
        if u > v:
            u, v = v, u
        t0, t1 = max(t0, u), min(t1, v)
        if t0 > t1:
            return False
    return True
print(seg_hits_box((-1, .5, .5), (2, .5, .5), (0, 0, 0), (1, 1, 1), 0))      # True: içinden geçer
print(seg_hits_box((-1, 1.05, .5), (2, 1.05, .5), (0, 0, 0), (1, 1, 1), 0))  # False: yanından geçer

import math, random
def rrt_connect(start, goal, free, step=0.1, iters=2000, lo=(0, 0), hi=(1, 1)):
    def dist(p, q): return math.hypot(p[0] - q[0], p[1] - q[1])
    def toward(p, q):
        d = dist(p, q)
        return q if d <= step else (p[0] + (q[0] - p[0]) * step / d, p[1] + (q[1] - p[1]) * step / d)
    def edge_free(p, q):                 # kenarı küçük adımlarla denetle
        n = max(1, int(dist(p, q) / 0.02))
        return all(free((p[0] + (q[0] - p[0]) * i / n, p[1] + (q[1] - p[1]) * i / n)) for i in range(n + 1))
    def extend(tree, target):
        i = min(range(len(tree)), key=lambda k: dist(tree[k][0], target))
        q = toward(tree[i][0], target)
        if not edge_free(tree[i][0], q):
            return None
        tree.append((q, i))
        return len(tree) - 1
    def chain(tree, i):
        out = []
        while i is not None:
            out.append(tree[i][0]); i = tree[i][1]
        return out
    a, b = [(start, None)], [(goal, None)]   # biri başlangıçtan, biri hedeften büyür
    for _ in range(iters):
        ia = extend(a, (random.uniform(lo[0], hi[0]), random.uniform(lo[1], hi[1])))
        if ia is not None:
            while True:                  # öbür ağacı yeni düğüme doğru ısrarla uzat
                ib = extend(b, a[ia][0])
                if ib is None: break
                if dist(b[ib][0], a[ia][0]) < 1e-9:
                    path = chain(a, ia)[::-1] + chain(b, ib)[1:]
                    return path if a[0][0] == start else path[::-1]
        a, b = b, a                      # ağaçların rolünü değiştir
    return None
random.seed(1)
wall = lambda p: not (0.45 < p[0] < 0.55 and p[1] < 0.8)
print(len(rrt_connect((0.1, 0.2), (0.9, 0.2), wall)))   # 25 nokta (tohum 1)

# A ile B arasında duvar var: yük taşırken plan kullan
var ust_a = pose(0.55, -0.32, 0.25)
var al_a = pose(0.55, -0.32, 0.03)
var ust_b = pose(0.55, 0.32, 0.25)
var koy_b = pose(0.55, 0.32, 0.03)
open
plan ust_a         # çarpışmasız yol iste
lin al_a
close
lin ust_a
plan ust_b         # duvarın üstünden ya da çevresinden
lin koy_b
open
lin ust_b
print "bitti, parça B'de"