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
