def get_x3d_cone(start: tuple, end: tuple, position: float, length: float, radius: float, color: tuple): # by default the X3D cone points along the y axis original_vector = (0, 1, 0) line_vector = psub(end, start) line_normalized = pnormalized(line_vector) # the cone position is at its bottom bottom_position = padd(start, pmul(line_vector, position)) # handling of corner cases if pnormsq(pcross(original_vector, line_normalized)) == 0: # Both vectors are aligned - but maybe point into different directions. rotation_axis = (1, 0, 0) rotation_angle = math.pi if original_vector != line_normalized else 0 else: # Both vectors are not aligned. Use a normal rotation. # Rotate around the vector in the vector in the middle by 180 degrees. rotation_axis = padd(original_vector, line_normalized) rotation_angle = math.pi yield ('<Transform translation="{:f} {:f} {:f}" rotation="{:f} {:f} {:f} {:f}">' .format(*bottom_position, *rotation_axis, rotation_angle)) yield "<Shape>" yield "<Appearance>" yield ('<Material diffuseColor="{:f} {:f} {:f}" transparency="{:f}" />' .format(color["red"], color["green"], color["blue"], 1 - color["alpha"])) yield "</Appearance>" yield '<Cone bottomRadius="{:f}" topRadius="0" height="{:f}" />'.format(radius, length) yield "</Shape>" yield "</Transform>"
def intersect_circle_point(center, axis, radius, radiussq, direction, point): # take a plane through the base plane = Plane(center, axis) # intersect with line gives ccp (ccp, l) = plane.intersect_point(direction, point) # check if inside circle if ccp and (pnormsq(psub(center, ccp)) < radiussq - epsilon): return (ccp, point, -l) return (None, None, INFINITE)
def intersect_sphere_edge(self, direction, edge, start=None): (cl, ccp, cp, l) = self.intersect_sphere_line(direction, edge, start=start) if cp: # check if the contact point is between the endpoints d = psub(edge.p2, edge.p1) m = pdot(psub(cp, edge.p1), d) if (m < -epsilon) or (m > pnormsq(d) + epsilon): return (None, INFINITE, None) return (cl, l, cp)
def intersect_sphere_point(center, radius, radiussq, direction, point): # line equation # (1) x = p_0 + \lambda * d # sphere equation # (2) (x-x_0)^2 = R^2 # (1) in (2) gives a quadratic in \lambda p0_x0 = psub(center, point) a = pnormsq(direction) b = 2 * pdot(p0_x0, direction) c = pnormsq(p0_x0) - radiussq d = b * b - 4 * a * c if d < 0: return (None, None, INFINITE) if a < 0: l = (-b + sqrt(d)) / (2 * a) else: l = (-b - sqrt(d)) / (2 * a) # cutter contact point ccp = padd(point, pmul(direction, -l)) return (ccp, point, l)
def get_intersection(self, line, infinite_lines=False): """ Get the point of intersection between two lines. Intersections outside the length of these lines are ignored. Returns (None, None) if no valid intersection was found. Otherwise the result is (CollisionPoint, distance). Distance is between 0 and 1. """ x1, x2, x3, x4 = self.p1, self.p2, line.p1, line.p2 a = psub(x2, x1) b = psub(x4, x3) c = psub(x3, x1) # see http://mathworld.wolfram.com/Line-LineIntersection.html (24) try: factor = pdot(pcross(c, b), pcross(a, b)) / pnormsq(pcross(a, b)) except ZeroDivisionError: # lines are parallel # check if they are _one_ line if pnorm(pcross(a, c)) != 0: # the lines are parallel with a distance return None, None # the lines are on one straight candidates = [] if self.is_point_inside(x3): candidates.append((x3, pnorm(c) / pnorm(a))) elif self.is_point_inside(x4): candidates.append((x4, pdist(line.p2, self.p1) / pnorm(a))) elif line.is_point_inside(x1): candidates.append((x1, 0)) elif line.is_point_inside(x2): candidates.append((x2, 1)) else: return None, None # return the collision candidate with the lowest distance candidates.sort(key=lambda collision: collision[1]) return candidates[0] if infinite_lines or (-epsilon <= factor <= 1 + epsilon): intersec = padd(x1, pmul(a, factor)) # check if the intersection is between x3 and x4 if infinite_lines: return intersec, factor elif ((min(x3[0], x4[0]) - epsilon <= intersec[0] <= max(x3[0], x4[0]) + epsilon) and (min(x3[1], x4[1]) - epsilon <= intersec[1] <= max(x3[1], x4[1]) + epsilon) and (min(x3[2], x4[2]) - epsilon <= intersec[2] <= max(x3[2], x4[2]) + epsilon)): return intersec, factor else: # intersection outside of the length of line(x3, x4) return None, None else: # intersection outside of the length of line(x1, x2) return None, None
def intersect_torus_plane(center, axis, majorradius, minorradius, direction, triangle): # take normal to the plane n = triangle.normal if pdot(n, direction) == 0: return (None, None, INFINITE) if pdot(n, axis) == 1: return (None, None, INFINITE) # find place on torus where surface normal is n b = pmul(n, -1) z = axis a = psub(b, pmul(z, pdot(z, b))) a_sq = pnormsq(a) if a_sq <= 0: return (None, None, INFINITE) a = pdiv(a, sqrt(a_sq)) ccp = padd(padd(center, pmul(a, majorradius)), pmul(b, minorradius)) # find intersection with plane (cp, l) = triangle.plane.intersect_point(direction, ccp) return (ccp, cp, l)
def intersect_circle_line(center, axis, radius, radiussq, direction, edge): # make a plane by sliding the line along the direction (1) d = edge.dir if pdot(d, axis) == 0: if pdot(direction, axis) == 0: return (None, None, INFINITE) plane = Plane(center, axis) (p1, l) = plane.intersect_point(direction, edge.p1) (p2, l) = plane.intersect_point(direction, edge.p2) pc = Line(p1, p2).closest_point(center) d_sq = pnormsq(psub(pc, center)) if d_sq >= radiussq: return (None, None, INFINITE) a = sqrt(radiussq - d_sq) d1 = pdot(psub(p1, pc), d) d2 = pdot(psub(p2, pc), d) ccp = None cp = None if abs(d1) < a - epsilon: ccp = p1 cp = psub(p1, pmul(direction, l)) elif abs(d2) < a - epsilon: ccp = p2 cp = psub(p2, pmul(direction, l)) elif ((d1 < -a + epsilon) and (d2 > a - epsilon)) \ or ((d2 < -a + epsilon) and (d1 > a - epsilon)): ccp = pc cp = psub(pc, pmul(direction, l)) return (ccp, cp, -l) n = pcross(d, direction) if pnorm(n) == 0: # no contact point, but should check here if circle *always* intersects # line... return (None, None, INFINITE) n = pnormalized(n) # take a plane through the base plane = Plane(center, axis) # intersect base with line (lp, l) = plane.intersect_point(d, edge.p1) if not lp: return (None, None, INFINITE) # intersection of 2 planes: lp + \lambda v v = pcross(axis, n) if pnorm(v) == 0: return (None, None, INFINITE) v = pnormalized(v) # take plane through intersection line and parallel to axis n2 = pcross(v, axis) if pnorm(n2) == 0: return (None, None, INFINITE) n2 = pnormalized(n2) # distance from center to this plane dist = pdot(n2, center) - pdot(n2, lp) distsq = dist * dist if distsq > radiussq - epsilon: return (None, None, INFINITE) # must be on circle dist2 = sqrt(radiussq - distsq) if pdot(d, axis) < 0: dist2 = -dist2 ccp = psub(center, psub(pmul(n2, dist), pmul(v, dist2))) plane = Plane(edge.p1, pcross(pcross(d, direction), d)) (cp, l) = plane.intersect_point(direction, ccp) return (ccp, cp, l)