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)]