second学习记录

代码：second
论文：paper

second部分代码流程：

second中的一些与向量相关的代码

# 可以得到长方体六个面的法向量，以及每个面上的点在法向量的投影
def surface_equ_3d_jitv2(surfaces):
    # polygon_surfaces: [num_polygon, num_surfaces, num_points_of_polygon, 3]
    # surfaces.shape: (1, 6, 3, 3)
    num_polygon = surfaces.shape[0]
    max_num_surfaces = surfaces.shape[1]
    normal_vec = np.zeros((num_polygon, max_num_surfaces, 3), dtype=surfaces.dtype)
    d = np.zeros((num_polygon, max_num_surfaces), dtype=surfaces.dtype)
    sv0 = surfaces[0, 0, 0] - surfaces[0, 0, 1]
    sv1 = surfaces[0, 0, 0] - surfaces[0, 0, 1]
    # 1
    for i in range(num_polygon):
        for j in range(max_num_surfaces):
            sv0[0] = surfaces[i, j, 0, 0] - surfaces[i, j, 1, 0]
            sv0[1] = surfaces[i, j, 0, 1] - surfaces[i, j, 1, 1]
            sv0[2] = surfaces[i, j, 0, 2] - surfaces[i, j, 1, 2]
            sv1[0] = surfaces[i, j, 1, 0] - surfaces[i, j, 2, 0]
            sv1[1] = surfaces[i, j, 1, 1] - surfaces[i, j, 2, 1]
            sv1[2] = surfaces[i, j, 1, 2] - surfaces[i, j, 2, 2]
            # vector outer product，向量外积，叉乘结果是法向量
            # (y1z2 - y2z1, -(x1z2 - x2z1), x1y2 - x2y1)
            normal_vec[i, j, 0] = (sv0[1] * sv1[2] - sv0[2] * sv1[1])
            normal_vec[i, j, 1] = (sv0[2] * sv1[0] - sv0[0] * sv1[2])
            normal_vec[i, j, 2] = (sv0[0] * sv1[1] - sv0[1] * sv1[0])
            
            d[i, j] = -surfaces[i, j, 0, 0] * normal_vec[i, j, 0] - \
                      surfaces[i, j, 0, 1] * normal_vec[i, j, 1] - \
                       surfaces[i, j, 0, 2] * normal_vec[i, j, 2]
    return normal_vec, d

# 计算每个点在不同面的法向量上的投影，由此判断该点是否在长方体内部
def _points_count_convex_polygon_3d_jit(points,
                                    polygon_surfaces,
                                    normal_vec, d,
                                    num_surfaces=None):
    # polygon_surfaces.shape: (1, 6, 4, 3)
    max_num_surfaces, max_num_points_of_surface = polygon_surfaces.shape[1:3]
    num_points = points.shape[0]
    num_polygons = polygon_surfaces.shape[0]
    ret = np.full((num_polygons,), num_points, dtype=np.int64)
    sign = 0.0
    for i in range(num_points):
        for j in range(num_polygons):
            for k in range(max_num_surfaces):
                if k > num_surfaces[j]:
                    break
                sign = points[i, 0] * normal_vec[j, k, 0] \
                     + points[i, 1] * normal_vec[j, k, 1] \
                     + points[i, 2] * normal_vec[j, k, 2] + d[j, k]
                if sign >= 0:
                    ret[j] -= 1
                    break
    return ret

# 判断点是否在矩形内部，将矩形的边按照顺时针方向，以向量形式表示
def points_in_convex_polygon_jit(points, polygon, clockwise=True):
    """check points is in 2d convex polygons. True when point in polygon"""
    # first convert polygon to directed lines
    # polygon.shape: (1, 4, 2)
    # polygon: array([[[  0. , -40. ],
    #                  [  0. ,  40. ],
    #                  [ 70.4,  40. ],
    #                  [ 70.4, -40. ]]], dtype=float32)
    num_points_of_polygon = polygon.shape[1]
    # points.shape: (15, 2)
    num_points = points.shape[0]
    num_polygons = polygon.shape[0]
    if clockwise:
        # [num_points_of_polygon - 1] + list(range(num_points_of_polygon - 1)): [3, 0, 1, 2]
        # polygon[:, [3, 0, 1, 2], :]: array([[[ 70.4, -40. ],
        #                                      [  0. , -40. ],
        #                                      [  0. ,  40. ],
        #                                      [ 70.4,  40. ]]], dtype=float32)
        # vec1: array([[[-70.4,   0. ],
        #               [  0. ,  80. ],
        #               [ 70.4,   0. ],
        #               [  0. , -80. ]]], dtype=float32)
        # 0, 1, 2, 3 - 3, 0, 1, 2 = 0->3, 1->0, 2->1, 3->2, clockwise
        vec1 = polygon - polygon[:, [num_points_of_polygon - 1] +
                                 list(range(num_points_of_polygon - 1)), :]
    else:
        vec1 = polygon[:, [num_points_of_polygon - 1] +
                       list(range(num_points_of_polygon - 1)), :] - polygon
    # vec1: [num_polygon, num_points_of_polygon, 2]
    ret = np.zeros((num_points, num_polygons), dtype=np.bool_)
    success = True
    cross = 0.0
    # 15
    for i in range(num_points):
        # 1
        for j in range(num_polygons):
            success = True
            # 4
            for k in range(num_points_of_polygon):
                # if cross > 0, clockwise, outside
                cross = vec1[j, k, 1] * (polygon[j, k, 0] - points[i, 0])
                cross -= vec1[j, k, 0] * (polygon[j, k, 1] - points[i, 1])
                if cross >= 0:
                    success = False
                    break
            ret[i, j] = success
    return ret

# 碰撞测试
def box_collision_test(boxes, qboxes, clockwise=True):
    # 16
    N = boxes.shape[0]
    # 16
    K = qboxes.shape[0]
    # ret.shape: (16, 16)
    ret = np.zeros((N, K), dtype=np.bool_)
    slices = np.array([1, 2, 3, 0])
    # lines_boxes.shape: (16, 4, 2, 2)
    lines_boxes = np.stack((boxes, boxes[:, slices, :]),
                           axis=2)  # [N, 4, 2(line), 2(xy)]
    lines_qboxes = np.stack((qboxes, qboxes[:, slices, :]), axis=2)
    # vec = np.zeros((2,), dtype=boxes.dtype)
    # maxmium bouding rectangle
    # (xmin, ymin, xmax, ymax)
    boxes_standup = box_np_ops.corner_to_standup_nd_jit(boxes)
    qboxes_standup = box_np_ops.corner_to_standup_nd_jit(qboxes)
    for i in range(N):
        for j in range(K):
            # calculate standup first
            # boxes_standup[i]: array([ 8.48539387, -2.45828748,  8.97736966, -1.25354751])
            # qboxes_standup[j]: array([ 8.48539387, -2.45828748,  8.97736966, -1.25354751])
            iw = (min(boxes_standup[i, 2], qboxes_standup[j, 2]) - max(
                boxes_standup[i, 0], qboxes_standup[j, 0]))
            if iw > 0:
                ih = (min(boxes_standup[i, 3], qboxes_standup[j, 3]) - max(
                    boxes_standup[i, 1], qboxes_standup[j, 1]))
                if ih > 0:
                    for k in range(4):
                        for l in range(4):
                            # array([ 8.48539387, -2.45348756])
                            A = lines_boxes[i, k, 0]
                            # array([ 8.49739367, -1.25354751])
                            B = lines_boxes[i, k, 1]
                            # array([ 8.48539387, -2.45348756])
                            C = lines_qboxes[j, l, 0]
                            # array([ 8.49739367, -1.25354751])
                            D = lines_qboxes[j, l, 1]
                            # check the intersection of two lines segments
                            # 通过跨立实验判断两线段相交，判断a,b两点是否在cd向量的两端，即两个叉积结果是异号的
                            acd = (D[1] - A[1]) * (C[0] - A[0]) > (
                                C[1] - A[1]) * (D[0] - A[0])
                            bcd = (D[1] - B[1]) * (C[0] - B[0]) > (
                                C[1] - B[1]) * (D[0] - B[0])
                            if acd != bcd:
                            	# 判断c,d两点是否在ab向量的两端，即两个叉积结果是异号的
                                abc = (C[1] - A[1]) * (B[0] - A[0]) > (
                                    B[1] - A[1]) * (C[0] - A[0])
                                abd = (D[1] - A[1]) * (B[0] - A[0]) > (
                                    B[1] - A[1]) * (D[0] - A[0])
                                if abc != abd:
                                    ret[i, j] = True  # collision.
                                    break
                        if ret[i, j] is True:
                            break
                    if ret[i, j] is False:
                        # now check complete overlap.
                        # box overlap qbox:
                        # 判断点是否在矩形内部
                        box_overlap_qbox = True
                        for l in range(4):  # point l in qboxes
                            for k in range(4):  # corner k in boxes
                                vec = boxes[i, k] - boxes[i, (k + 1) % 4]
                                if clockwise:
                                    vec = -vec
                                cross = vec[1] * (
                                    boxes[i, k, 0] - qboxes[j, l, 0])
                                cross -= vec[0] * (
                                    boxes[i, k, 1] - qboxes[j, l, 1])
                                if cross >= 0:
                                    box_overlap_qbox = False
                                    break
                            if box_overlap_qbox is False:
                                break

                        if box_overlap_qbox is False:
                            qbox_overlap_box = True
                            for l in range(4):  # point l in boxes
                                for k in range(4):  # corner k in qboxes
                                    vec = qboxes[j, k] - qboxes[j, (k + 1) % 4]
                                    if clockwise:
                                        vec = -vec
                                    cross = vec[1] * (
                                        qboxes[j, k, 0] - boxes[i, l, 0])
                                    cross -= vec[0] * (
                                        qboxes[j, k, 1] - boxes[i, l, 1])
                                    if cross >= 0:  #
                                        qbox_overlap_box = False
                                        break
                                if qbox_overlap_box is False:
                                    break
                            if qbox_overlap_box:
                                ret[i, j] = True  # collision.
                        else:
                            ret[i, j] = True  # collision.
    return ret