GJK算法的python算法

以下是GJK算法的Python实现：

import numpy as np

def support(shape1, shape2, direction):
    """
    返回两个形状在给定方向上的支撑点
    """
    return shape1.support(direction) - shape2.support(-direction)

def gjk(shape1, shape2):
    """
    GJK算法实现
    """
    # 初始搜索方向
    direction = np.array([1, 0])
    
    # 初始支撑点
    point = support(shape1, shape2, direction)
    
    # 初始Simplex
    simplex = [point]
    
    # 初始搜索方向为Simplex中唯一点到原点的反向
    direction = -point
    
    while True:
        # 获取新的支撑点
        point = support(shape1, shape2, direction)
        
        # 如果新的支撑点与Simplex中最后一个点在搜索方向上的投影小于0，则两个形状不相交
        if np.dot(point, direction) < 0:
            return False
        
        # 将新的支撑点添加到Simplex中
        simplex.append(point)
        
        # 检查Simplex是否包含原点
        if contains_origin(simplex, direction):
            return True
        
def contains_origin(simplex, direction):
    """
    检查Simplex是否包含原点
    """
    if len(simplex) == 2:
        # Simplex为线段
        a, b = simplex
        ab = b - a
        ao = -a
        
        # 新的搜索方向为Simplex中唯一点到原点的反向
        direction = np.cross(ab, ao)
        if direction >= 0:
            direction = np.cross(ab, np.array([0, 1]))
            if direction == 0:
                direction = np.array([0, 1])
        else:
            direction = -direction
        
        return False
    
    elif len(simplex) == 3:
        # Simplex为三角形
        a, b, c = simplex
        ab = b - a
        ac = c - a
        ao = -a
        
        # 检查原点是否在ab和ac的外侧
        ab_perp = np.cross(ab, np.cross(ac, ab))
        if np.dot(ab_perp, ao) > 0:
            simplex.remove(c)
            direction = ab_perp
            return False
        
        ac_perp = np.cross(np.cross(ab, ac), ac)
        if np.dot(ac_perp, ao) > 0:
            simplex.remove(b)
            direction = ac_perp
            return False
        
        # 原点在Simplex内部
        return True
    
    else:
        # Simplex为四面体
        a, b, c, d = simplex
        ab = b - a
        ac = c - a
        ad = d - a
        ao = -a
        
        # 检查原点是否在abc、acd、adb的外侧
        abc_perp = np.cross(ab, ac)
        if np.dot(abc_perp, ao) > 0:
            simplex.remove(d)
            direction = abc_perp
            return False
        
        acd_perp = np.cross(ad, ac)
        if np.dot(acd_perp, ao) > 0:
            simplex.remove(b)
            direction = acd_perp
            return False
        
        adb_perp = np.cross(ab, ad)
        if np.dot(adb_perp, ao) > 0:
            simplex.remove(c)
            direction = adb_perp
            return False
        
        # 原点在Simplex内部
        return True