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 calculate_point_height(self, x, y, func): point = (x, y, self.outer.minz) if not self.outer.is_point_inside(point): return None for poly in self.inner: if poly.is_point_inside(point): return None point = (x, y, self.outer.minz) line_distances = [] for line in self.lines: cross_product = pcross(line.dir, psub(point, line.p1)) if cross_product[2] > 0: close_points = [] close_point = line.closest_point(point) if not line.is_point_inside(close_point): close_points.append(line.p1) close_points.append(line.p2) else: close_points.append(close_point) for p in close_points: direction = psub(point, p) dist = pnorm(direction) line_distances.append(dist) elif cross_product[2] == 0: # the point is on the line line_distances.append(0.0) # no other line can get closer than this break else: # the point is in the left of this line pass line_distances.sort() return self.z_level + func(line_distances[0])
def intersect_sphere_line(center, radius, radiussq, direction, edge): # make a plane by sliding the line along the direction (1) d = edge.dir n = pcross(d, direction) if pnorm(n) == 0: # no contact point, but should check here if sphere *always* intersects # line... return (None, None, INFINITE) n = pnormalized(n) # calculate the distance from the sphere center to the plane dist = -pdot(center, n) + pdot(edge.p1, n) if abs(dist) > radius - epsilon: return (None, None, INFINITE) # this gives us the intersection circle on the sphere # now take a plane through the edge and perpendicular to the direction (2) # find the center on the circle closest to this plane # which means the other component is perpendicular to this plane (2) n2 = pnormalized(pcross(n, d)) # the contact point is on a big circle through the sphere... dist2 = sqrt(radiussq - dist * dist) # ... and it's on the plane (1) ccp = padd(center, padd(pmul(n, dist), pmul(n2, dist2))) # now intersect a line through this point with the plane (2) plane = Plane(edge.p1, n2) (cp, l) = plane.intersect_point(direction, ccp) return (ccp, cp, l)
def _process_one_triangle(extra_args): model, cutter, up_vector, triangle, z = extra_args result = [] # ignore triangles below the z level if triangle.maxz < z: # Case 1a return result, None # ignore triangles pointing upwards or downwards if pnorm(pcross(triangle.normal, up_vector)) == 0: # Case 1b return result, None edge_collisions = get_collision_waterline_of_triangle( model, cutter, up_vector, triangle, z) if edge_collisions is None: # don't try to use this edge again return result, [id(triangle)] elif len(edge_collisions) == 0: return result, None else: for cutter_location, edge in edge_collisions: shifted_edge = get_shifted_waterline(up_vector, edge, cutter_location) if shifted_edge is not None: if _DEBUG_DISBALE_WATERLINE_SHIFT: result.append((edge, edge)) else: result.append((edge, shifted_edge)) return result, None
def reset_cache(self): self.minx = min(self.p1[0], self.p2[0], self.p3[0]) self.miny = min(self.p1[1], self.p2[1], self.p3[1]) self.minz = min(self.p1[2], self.p2[2], self.p3[2]) self.maxx = max(self.p1[0], self.p2[0], self.p3[0]) self.maxy = max(self.p1[1], self.p2[1], self.p3[1]) self.maxz = max(self.p1[2], self.p2[2], self.p3[2]) self.e1 = Line(self.p1, self.p2) self.e2 = Line(self.p2, self.p3) self.e3 = Line(self.p3, self.p1) # calculate normal, if p1-p2-pe are in clockwise order if self.normal is None: self.normal = pnormalized( pcross(psub(self.p3, self.p1), psub(self.p2, self.p1))) if not len(self.normal) > 3: self.normal = (self.normal[0], self.normal[1], self.normal[2], 'v') self.center = pdiv(padd(padd(self.p1, self.p2), self.p3), 3) self.plane = Plane(self.center, self.normal) # calculate circumcircle (resulting in radius and middle) denom = pnorm(pcross(psub(self.p2, self.p1), psub(self.p3, self.p2))) self.radius = (pdist(self.p2, self.p1) * pdist(self.p3, self.p2) * pdist(self.p3, self.p1)) / (2 * denom) self.radiussq = self.radius**2 denom2 = 2 * denom * denom alpha = pdist_sq(self.p3, self.p2) * pdot(psub( self.p1, self.p2), psub(self.p1, self.p3)) / denom2 beta = pdist_sq(self.p1, self.p3) * pdot(psub( self.p2, self.p1), psub(self.p2, self.p3)) / denom2 gamma = pdist_sq(self.p1, self.p2) * pdot(psub( self.p3, self.p1), psub(self.p3, self.p2)) / denom2 self.middle = (self.p1[0] * alpha + self.p2[0] * beta + self.p3[0] * gamma, self.p1[1] * alpha + self.p2[1] * beta + self.p3[1] * gamma, self.p1[2] * alpha + self.p2[2] * beta + self.p3[2] * gamma)
def _check_deviance_of_adjacent_points(p1, p2, p3, min_distance): straight = psub(p3, p1) added = pdist(p2, p1) + pdist(p3, p2) # compare only the x/y distance of p1 and p3 with min_distance if straight[0]**2 + straight[1]**2 < min_distance**2: # the points are too close together return True else: # allow 0.1% deviance - this is an angle of around 2 degrees return (added / pnorm(straight)) < 1.001
def intersect_point(self, direction, point): if (direction is not None) and (pnorm(direction) != 1): # calculations will go wrong, if the direction is not a unit vector direction = pnormalized(direction) if direction is None: return (None, INFINITE) denom = pdot(self.n, direction) if denom == 0: return (None, INFINITE) l = -(pdot(self.n, point) - pdot(self.n, self.p)) / denom cp = padd(point, pmul(direction, l)) return (cp, l)
def draw_direction_cone_mesh(self, p1, p2, position=0.5, precision=12, size=0.1): distance = psub(p2, p1) length = pnorm(distance) direction = pnormalized(distance) if direction is None or length < 0.5: # zero-length line return [] cone_length = length * size cone_radius = cone_length / 3.0 bottom = padd(p1, pmul(psub(p2, p1), position - size / 2)) top = padd(p1, pmul(psub(p2, p1), position + size / 2)) # generate a a line perpendicular to this line, cross product is good at this cross = pcross(direction, (0, 0, -1)) conepoints = [] if pnorm(cross) != 0: # The line direction is not in line with the z axis. conep1 = padd(bottom, pmul(cross, cone_radius)) conepoints = [ self._rotate_point(conep1, bottom, direction, x) for x in numpy.linspace(0, 2 * math.pi, precision) ] else: # Z axis # just add cone radius to the x axis and rotate the point conep1 = (bottom[0] + cone_radius, bottom[1], bottom[2]) conepoints = [ self._rotate_point(conep1, p1, direction, x) for x in numpy.linspace(0, 2 * math.pi, precision) ] triangles = [(top, conepoints[idx], conepoints[idx + 1]) for idx in range(len(conepoints) - 1)] return triangles
def draw_direction_cone(p1, p2, position=0.5, precision=12, size=0.1): distance = psub(p2, p1) length = pnorm(distance) direction = pnormalized(distance) if direction is None: # zero-length line return cone_length = length * size cone_radius = cone_length / 3.0 # move the cone to the middle of the line GL.glTranslatef((p1[0] + p2[0]) * position, (p1[1] + p2[1]) * position, (p1[2] + p2[2]) * position) # rotate the cone according to the line direction # The cross product is a good rotation axis. cross = pcross(direction, (0, 0, -1)) if pnorm(cross) != 0: # The line direction is not in line with the z axis. try: angle = math.asin(sqrt(direction[0]**2 + direction[1]**2)) except ValueError: # invalid angle - just ignore this cone return # convert from radians to degree angle = angle / math.pi * 180 if direction[2] < 0: angle = 180 - angle GL.glRotatef(angle, cross[0], cross[1], cross[2]) elif direction[2] == -1: # The line goes down the z axis - turn it around. GL.glRotatef(180, 1, 0, 0) else: # The line goes up the z axis - nothing to be done. pass # center the cone GL.glTranslatef(0, 0, -cone_length * position) # draw the cone GLUT.glutSolidCone(cone_radius, cone_length, precision, 1)
def intersect_cylinder_line(center, axis, radius, radiussq, direction, edge): d = edge.dir # take a plane throught the line and along the cylinder axis (1) n = pcross(d, axis) if pnorm(n) == 0: # no contact point, but should check here if cylinder *always* # intersects line... return (None, None, INFINITE) n = pnormalized(n) # the contact line between the cylinder and this plane (1) # is where the surface normal is perpendicular to the plane # so line := ccl + \lambda * axis if pdot(n, direction) < 0: ccl = psub(center, pmul(n, radius)) else: ccl = padd(center, pmul(n, radius)) # now extrude the contact line along the direction, this is a plane (2) n2 = pcross(direction, axis) if pnorm(n2) == 0: # no contact point, but should check here if cylinder *always* # intersects line... return (None, None, INFINITE) n2 = pnormalized(n2) plane1 = Plane(ccl, n2) # intersect this plane with the line, this gives us the contact point (cp, l) = plane1.intersect_point(d, edge.p1) if not cp: return (None, None, INFINITE) # now take a plane through the contact line and perpendicular to the # direction (3) plane2 = Plane(ccl, direction) # the intersection of this plane (3) with the line through the contact point # gives us the cutter contact point (ccp, l) = plane2.intersect_point(direction, cp) cp = padd(ccp, pmul(direction, -l)) return (ccp, cp, -l)
def intersect_circle_plane(center, radius, direction, triangle): # let n be the normal to the plane n = triangle.normal if pdot(n, direction) == 0: return (None, None, INFINITE) # project onto z=0 n2 = (n[0], n[1], 0) if pnorm(n2) == 0: (cp, d) = triangle.plane.intersect_point(direction, center) ccp = psub(cp, pmul(direction, d)) return (ccp, cp, d) n2 = pnormalized(n2) # the cutter contact point is on the circle, where the surface normal is n ccp = padd(center, pmul(n2, -radius)) # intersect the plane with a line through the contact point (cp, d) = triangle.plane.intersect_point(direction, ccp) return (ccp, cp, d)
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)
def len(self): return pnorm(self.vector)
def get_area(self): cross = pcross(psub(self.p2, self.p1), psub(self.p3, self.p1)) return pnorm(cross) / 2
def get_offset_polygons_validated(self, offset): if self.is_outer(): inside_shifting = max(0, -offset) else: inside_shifting = max(0, offset) if inside_shifting * 2 >= self.get_max_inside_distance(): # no polygons will be left return [] points = [] for index in range(len(self._points)): points.append(self.get_shifted_vertex(index, offset)) max_dist = 1000 * epsilon def test_point_near(p, others): for o in others: if pdist(p, o) < max_dist: return True return False reverse_lines = [] shifted_lines = [] for index, p1 in enumerate(points): next_index = (index + 1) % len(points) p2 = points[next_index] diff = psub(p2, p1) old_dir = pnormalized( psub(self._points[next_index], self._points[index])) if pnormalized(diff) != old_dir: # the direction turned around if pnorm(diff) > max_dist: # the offset was too big return None else: reverse_lines.append(index) shifted_lines.append((True, Line(p1, p2))) else: shifted_lines.append((False, Line(p1, p2))) # look for reversed lines index = 0 while index < len(shifted_lines): line_reverse, line = shifted_lines[index] if line_reverse: prev_index = (index - 1) % len(shifted_lines) next_index = (index + 1) % len(shifted_lines) prev_reverse, prev_line = shifted_lines[prev_index] while prev_reverse and (prev_index != next_index): prev_index = (prev_index - 1) % len(shifted_lines) prev_reverse, prev_line = shifted_lines[prev_index] if prev_index == next_index: # no lines are left print("out 1") return [] next_reverse, next_line = shifted_lines[next_index] while next_reverse and (prev_index != next_index): next_index = (next_index + 1) % len(shifted_lines) next_reverse, next_line = shifted_lines[next_index] if prev_index == next_index: # no lines are left print("out 2") return [] if pdist(prev_line.p2, next_line.p1) > max_dist: cp, dist = prev_line.get_intersection(next_line) else: cp = prev_line.p2 if cp: shifted_lines[prev_index] = (False, Line(prev_line.p1, cp)) shifted_lines[next_index] = (False, Line(cp, next_line.p2)) else: cp, dist = prev_line.get_intersection(next_line, infinite_lines=True) raise BaseException( "Expected intersection not found: %s - %s - %s(%d) / %s(%d)" % (cp, shifted_lines[prev_index + 1:next_index], prev_line, prev_index, next_line, next_index)) if index > next_index: # we wrapped around the end of the list break else: index = next_index + 1 else: index += 1 non_reversed = [ one_line for rev, one_line in shifted_lines if not rev and one_line.len > 0 ] # split the list of lines into groups (based on intersections) split_points = [] index = 0 while index < len(non_reversed): other_index = 0 while other_index < len(non_reversed): other_line = non_reversed[other_index] if (other_index == index) \ or (other_index == ((index - 1) % len(non_reversed))) \ or (other_index == ((index + 1) % len(non_reversed))): # skip neighbours other_index += 1 continue line = non_reversed[index] cp, dist = line.get_intersection(other_line) if cp: if not test_point_near( cp, (line.p1, line.p2, other_line.p1, other_line.p2)): # the collision is not close to an end of the line return None elif (cp == line.p1) or (cp == line.p2): # maybe we have been here before if cp not in split_points: split_points.append(cp) elif (pdist(cp, line.p1) < max_dist) or (pdist( cp, line.p2) < max_dist): if pdist(cp, line.p1) < pdist(cp, line.p2): non_reversed[index] = Line(cp, line.p2) else: non_reversed[index] = Line(line.p1, cp) non_reversed.pop(other_index) non_reversed.insert(other_index, Line(other_line.p1, cp)) non_reversed.insert(other_index + 1, Line(cp, other_line.p2)) split_points.append(cp) if other_index < index: index += 1 # skip the second part of this line other_index += 1 else: # the split of 'other_line' will be handled later pass other_index += 1 index += 1 groups = [[]] current_group = 0 split_here = False for line in non_reversed: if line.p1 in split_points: split_here = True if split_here: split_here = False # check if any preceding group fits to the point for index, group in enumerate(groups): if not group: continue if index == current_group: continue if group[0].p1 == group[-1].p2: # the group is already closed continue if line.p1 == group[-1].p2: current_group = index groups[current_group].append(line) break else: current_group = len(groups) groups.append([line]) else: groups[current_group].append(line) if line.p2 in split_points: split_here = True # try to combine open groups for index1, group1 in enumerate(groups): if not group1: continue for index2, group2 in enumerate(groups): if not group2: continue if index2 <= index1: continue if (group1[-1].p2 == group2[0].p1) \ and (group1[0].p1 == group2[-1].p2): group1.extend(group2) groups[index2] = [] break result_polygons = [] print("********** GROUPS **************") for a in groups: print(a) for group in groups: if len(group) <= 2: continue poly = Polygon(self.plane) for line in group: try: poly.append(line) except ValueError: print("NON_REVERSED") for a in non_reversed: print(a) print(groups) print(split_points) print(poly) print(line) raise if self.is_closed and ((not poly.is_closed) or (self.is_outer() != poly.is_outer())): continue elif (not self.is_closed) and (poly.get_area() != 0): continue else: result_polygons.append(poly) return result_polygons