quad.py 文件源码

def projection_onto_quad(self, _point):
        from scipy.linalg import solve_triangular
        import numpy as np

        # first assume that _point is below diagonal BD
        vertexA = self.vertices_plane[0,:]
        vector_vertexA_point = _point - vertexA
        # we want to transform _point to the BASIS=[normal,AB,AC] and use QR decomposition of BASIS = Q*R
        # BASIS * coords = _point -> R * coords = Q' * _point
        R_BAD = np.dot(self.ortho_basis_AB.transpose(),self.basis_BAD)
        b = np.dot(self.ortho_basis_AB.transpose(),vector_vertexA_point)
        x = solve_triangular(R_BAD,b)
        distance = x[0]
        projected_point = _point - distance * self.normal
        u = x[1]
        v = x[2]

        # if not, _point is above diagonal BD
        if u+v > 1:
            vertexC = self.vertices_plane[2,:]
            vector_vertexC_point = _point - vertexC
            R_BCD = np.dot(self.ortho_basis_CB.transpose(),self.basis_BCD)
            b = np.dot(self.ortho_basis_CB.transpose(),vector_vertexC_point)
            x = solve_triangular(R_BCD,b)
            distance = x[0]
            projected_point = _point - distance * self.normal
            u = 1-x[1]
            v = 1-x[2]

        distance = abs(distance)

        u_crop = u
        v_crop = v

        if not (0<=u<=1 and 0<=v<=1):
            if u < 0:
                u_crop = 0
            elif u > 1:
                u_crop = 1

            if v < 0:
                v_crop = 0
            elif v > 1:
                v_crop = 1

            projected_point = self.point_on_quad(u_crop,v_crop)
            distance = np.linalg.norm(_point-projected_point)

        return projected_point, distance, u, v