class Line: ## Constructor # # @param p start point # @param q end point # # Line representation: (a, b, c) = (x1, y1, 1) $\times$ (x2, y2, 1) # - points on line: (a, b, c) $\cdot$ (x, y , 1) = 0 # - line intersection: (x, y, w) = (a1, b1, c1) $\times$ (a2, b2, c2) def __init__(self, p, q): self._p = np.array(p) self._q = np.array(q) peq = np.array([p[0], p[1], 1]) qeq = np.array([q[0], q[1], 1]) self._n = np.cross(peq, qeq) self._n = normalizeVector(self._n) self._e = self._q - self._p self._e = normalizeVector(self._e) self._bb = BoundingBox([p, q]) ## Return the positions of the line. def points(self): return np.array([self._p, self._q]) ## Return the position of the parameter [0, 1]. def pointAt(self, t): return self._p + t * self.length() * self._e ## Return the length of the line. def length(self): return np.linalg.norm(self._q - self._p) ## Find an intersected point with the given line. def intersect(self, l): ipeq = np.cross(self._n, l._n) if np.abs(ipeq[2]) < 0.000: return None ipeq *= 1.0 / ipeq[2] ip = np.array([ipeq[0], ipeq[1]]) if self._bb.contains(ip) and l._bb.contains(ip): return ip return None ## Returns the closest point on this line to the given point. def closestPoint(self, p): return self._closestPointVec(p) ## Returns the closest point on this line to the given point. def _closestPointEq(self, p): a, b, c = self._n x0, y0 = p x = (b * (b * x0 - a * y0) - a * c) / (a * a + b * b) y = (a * (-b * x0 + a * y0) - b * c) / (a * a + b * b) return np.array([x, y]) ## Returns the closest point on this line to the given point. def _closestPointVec(self, p): v = p - self._p return np.dot(v, self._e) * self._e + self._p ## Return the parameter of the closest point. def closestParam(self, p): v = p - self._p t = np.dot(v, self._e) return t / self.length() ## Return the distance from the given point to closest point on the line. def distanceToPoint(self, p): a, b, c = self._n x0, y0 = p return np.abs((a * x0 + b * y0 + c) / np.sqrt(a ** 2 + b ** 2)) ## Plot line with matplot. def plotLine(self, plt): ps = self.points() plt.plot(ps[:, 0], ps[:, 1], "-") def plotVector(self, plt): hl = 0.02 v = self._q - self._p v *= 1.0 - 2.0 * hl plt.arrow(self._p[0], self._p[1], v[0], v[1], head_width=0.5 * hl, head_length=hl) ## Plot closest point. def plotClosetPoint(self, plt, p): p = np.array(p) cp = self.closestPoint(p) plt.plot(cp[0], cp[1], "o") plt.annotate('closest point: %s' % cp, xy=cp) Line(p, cp).plotLine(plt) Line(self._p, p).plotVector(plt)
class BVH: ## Constructor def __init__(self, points, params, level=0): self._level = level self._bb = BoundingBox(points) self._children = [] self._line = None self._createChildren(points, params) ## Return if the node is leaf. def isLeaf(self): return len(self._children) == 0 ## Return the points in the node. def points(self): return self._points ## Return the children in the node. def children(self): if self.isLeaf(): return [self] return self._children ## Return true if the given point is included in the node. def contains(self, p): return self._bb.contains(p) ## Find intersections with the given BVH structure. def intersect(self, bvh): if self._bb.intersects(bvh._bb): if bvh.isLeaf() and self.isLeaf(): ip = self._line.intersect(bvh._line) if ip is not None: ilt = self._line.closestParam(ip) t_min, t_max = self._param_range it = (1.0 - ilt) * t_min + ilt * t_max return [(self, bvh, ip, it)] else: ibvhs = [] for self_ch in self.children(): for bvh_ch in bvh.children(): ibvh = self_ch.intersect(bvh_ch) if ibvh is not None: ibvhs.extend(ibvh) return ibvhs else: return None return None ## Plot BVH. def plotBVH(self, plt, color="b", alpha=0.05): self._bb.plotBoundingBox(plt, color=color, alpha=alpha) if self.isLeaf(): return for bvh in self.children(): bvh.plotBVH(plt, color) def _createChildren(self, points, params): if len(points) < 5: self._points = points self._params = params self._param_range = [np.min(params), np.max(params)] self._line = Line(self._points[0], self._points[-1]) return points_left = points[:len(points) / 2 + 1] points_right = points[len(points) / 2:] params_left = params[:len(points) / 2 + 1] params_right = params[len(points) / 2:] self._children = [BVH(points_left, params_left, self._level + 1), BVH(points_right, params_right, self._level + 1)]