def intersect_torus_point(center, axis, majorradius, minorradius,
majorradiussq, minorradiussq, direction, point):
dist = 0
if (direction[0] == 0) and (direction[1] == 0):
# drop
minlsq = (majorradius - minorradius)**2
maxlsq = (majorradius + minorradius)**2
l_sq = (point[0] - center[0])**2 + (point[1] - center[1])**2
if (l_sq < minlsq + epsilon) or (l_sq > maxlsq - epsilon):
return (None, None, INFINITE)
l_len = sqrt(l_sq)
z_sq = minorradiussq - (majorradius - l_len)**2
if z_sq < 0:
return (None, None, INFINITE)
z = sqrt(z_sq)
ccp = (point[0], point[1], center[2] - z)
dist = ccp[2] - point[2]
elif direction[2] == 0:
# push
z = point[2] - center[2]
if abs(z) > minorradius - epsilon:
return (None, None, INFINITE)
l_len = majorradius + sqrt(minorradiussq - z * z)
n = pcross(axis, direction)
d = pdot(n, point) - pdot(n, center)
if abs(d) > l_len - epsilon:
return (None, None, INFINITE)
a = sqrt(l_len * l_len - d * d)
ccp = padd(padd(center, pmul(n, d)), pmul(direction, a))
ccp = (ccp[0], ccp[1], point[2])
dist = pdot(psub(point, ccp), direction)
else:
# general case
x = psub(point, center)
v = pmul(direction, -1)
x_x = pdot(x, x)
x_v = pdot(x, v)
x1 = (x[0], x[1], 0)
v1 = (v[0], v[1], 0)
x1_x1 = pdot(x1, x1)
x1_v1 = pdot(x1, v1)
v1_v1 = pdot(v1, v1)
r2_major = majorradiussq
r2_minor = minorradiussq
a = 1.0
b = 4 * x_v
c = 2 * (x_x + 2 * x_v**2 +
(r2_major - r2_minor) - 2 * r2_major * v1_v1)
d = 4 * (x_x * x_v + x_v *
(r2_major - r2_minor) - 2 * r2_major * x1_v1)
e = ((x_x)**2 + 2 * x_x * (r2_major - r2_minor) +
(r2_major - r2_minor)**2 - 4 * r2_major * x1_x1)
r = poly4_roots(a, b, c, d, e)
if not r:
return (None, None, INFINITE)
else:
l_len = min(r)
ccp = padd(point, pmul(direction, -l_len))
dist = l_len
return (ccp, point, dist)
def poly4_roots(a, b, c, d, e):
if a == 0:
return poly3_roots(b, c, d, e)
c1 = float(b) / a
c2 = float(c) / a
c3 = float(d) / a
c4 = float(e) / a
roots3 = poly3_roots(1.0, -c2, c3 * c1 - 4 * c4,
-c3 * c3 - c4 * c1 * c1 + 4 * c4 * c2)
if not roots3:
return None
if len(roots3) == 1:
u = roots3[0]
else:
u = max(roots3[0], roots3[1], roots3[2])
p = c1 * c1 * INV_4 + u - c2
u *= INV_2
q = u * u - c4
if p < 0:
if p < -SMALL:
return None
p = 0
else:
p = sqrt(p)
if q < 0:
if q < -SMALL:
return None
q = 0
else:
q = sqrt(q)
quad1 = [1.0, c1 * INV_2 - p, 0]
quad2 = [1.0, c1 * INV_2 + p, 0]
q1 = u - q
q2 = u + q
p = quad1[1] * q2 + quad2[1] * q1 - c3
if near_zero(p):
quad1[2] = q1
quad2[2] = q2
else:
q = quad1[1] * q1 + quad2[1] * q2 - c3
if near_zero(q):
quad1[2] = q2
quad2[2] = q1
else:
return None
roots1 = poly2_roots(quad1[0], quad1[1], quad1[2])
roots2 = poly2_roots(quad2[0], quad2[1], quad2[2])
if roots1 and roots2:
return roots1 + roots2
elif roots1:
return roots1
elif roots2:
return roots2
else:
return None
def add_wave(self, freq=8, damp=3.0):
self.changed = True
rmax = sqrt(self.y[0] * self.y[0] + self.x[0] * self.x[0])
for y in range(0, self.yres):
for x in range(0, self.xres):
r = sqrt(self.y[y] * self.y[y] + self.x[x] * self.x[x])
self.buf[y][x].z = (
1 + math.cos(r / rmax * r / rmax * math.pi * freq) /
(1 + damp * (r / rmax)))
self.buf[y][x].changed = True
def poly3_roots(a, b, c, d):
if near_zero(a):
return poly2_roots(b, c, d)
c1 = b / a
c2 = c / a
c3 = d / a
c1_3 = c1 * INV_3
a = c2 - c1 * c1_3
b = (2 * c1 * c1 * c1 - 9 * c1 * c2 + 27 * c3) * INV_27
delta = a * a
delta = b * b * INV_4 + delta * a * INV_27
if delta > 0:
r_delta = sqrt(delta)
v_major_p3 = -INV_2 * b + r_delta
v_minor_p3 = -INV_2 * b - r_delta
v_major = cuberoot(v_major_p3)
v_minor = cuberoot(v_minor_p3)
return (v_major + v_minor - c1_3, )
elif delta == 0:
b_2 = -b * INV_2
s = cuberoot(b_2)
return (
2 * s - c1_3,
-s - c1_3,
-s - c1_3,
)
else:
if a > 0:
fact = 0
phi = 0
cs_phi = 1.0
sn_phi_s3 = 0.0
else:
a *= -INV_3
fact = sqrt(a)
f = -b * INV_2 / (a * fact)
if f >= 1.0:
phi = 0
cs_phi = 1.0
sn_phi_s3 = 0.0
elif f <= -1.0:
phi = PI_DIV_3
cs_phi = math.cos(phi)
sn_phi_s3 = math.sin(phi) * SQRT3
else:
phi = math.acos(f) * INV_3
cs_phi = math.cos(phi)
sn_phi_s3 = math.sin(phi) * SQRT3
r1 = 2 * fact * cs_phi
r2 = fact * (sn_phi_s3 - cs_phi)
r3 = fact * (-sn_phi_s3 - cs_phi)
return (r1 - c1_3, r2 - c1_3, r3 - c1_3)
def move_camera_by_screen(self, x_move, y_move, max_model_shift):
""" move the camera acoording to a mouse movement
@type x_move: int
@value x_move: movement of the mouse along the x axis
@type y_move: int
@value y_move: movement of the mouse along the y axis
@type max_model_shift: float
@value max_model_shift: maximum shifting of the model view (e.g. for
x_move == screen width)
"""
factors_x, factors_y = self._get_axes_vectors()
width, height = self._get_screen_dimensions()
# relation of x/y movement to the respective screen dimension
win_x_rel = (-2 * x_move) / float(width) / math.sin(self.view["fovy"])
win_y_rel = (-2 * y_move) / float(height) / math.sin(self.view["fovy"])
# This code is completely arbitrarily based on trial-and-error for
# finding a nice movement speed for all distances.
# Anyone with a better approach should just fix this.
distance_vector = self.get("distance")
distance = float(sqrt(sum([dim**2 for dim in distance_vector])))
win_x_rel *= math.cos(win_x_rel / distance)**20
win_y_rel *= math.cos(win_y_rel / distance)**20
# update the model position that should be centered on the screen
old_center = self.view["center"]
new_center = []
for i in range(3):
new_center.append(old_center[i] + max_model_shift *
(number(win_x_rel) * factors_x[i] +
number(win_y_rel) * factors_y[i]))
self.view["center"] = tuple(new_center)
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 get_length(vector):
""" calculate the lengt of a 3d vector
@type vector: tuple(float) | list(float)
@value vector: the given 3d vector
@rtype: float
@return: the length of a vector is the square root of the dot product
of the vector with itself
"""
return sqrt(get_dot_product(vector, vector))
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 poly2_roots(a, b, c):
d = b * b - 4 * a * c
if d < 0:
return None
if near_zero(a):
return poly1_roots(b, c)
if d == 0:
return (-b / (2 * a), )
q = sqrt(d)
if a < 0:
return ((-b + q) / (2 * a), (-b - q) / (2 * a))
else:
return ((-b - q) / (2 * a), (-b + q) / (2 * a))
def intersect_cylinder_point(center, axis, radius, radiussq, direction, point):
# take a plane along direction and axis
n = pnormalized(pcross(direction, axis))
# distance of the point to this plane
d = pdot(n, point) - pdot(n, center)
if abs(d) > radius - epsilon:
return (None, None, INFINITE)
# ccl is on cylinder
d2 = sqrt(radiussq - d * d)
ccl = padd(padd(center, pmul(n, d)), pmul(direction, d2))
# take plane through ccl and axis
plane = Plane(ccl, direction)
# intersect point with plane
(ccp, l) = plane.intersect_point(direction, point)
return (ccp, point, -l)
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 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 zoom_out(self):
self.scale_distance(sqrt(2))
def zoom_in(self):
self.scale_distance(sqrt(0.5))
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 pdist(a, b, axes=None):
return sqrt(pdist_sq(a, b, axes=axes))
def pnorm(a):
return sqrt(pdot(a, a))
def get_collision_waterline_of_triangle(model, cutter, up_vector, triangle, z):
# TODO: there are problems with "material allowance > 0"
plane = Plane((0, 0, z), up_vector)
if triangle.minz >= z:
# no point of the triangle is below z
# try all edges
# Case (4)
proj_points = []
for p in triangle.get_points():
proj_p = plane.get_point_projection(p)
if proj_p not in proj_points:
proj_points.append(proj_p)
if len(proj_points) == 3:
edges = []
for index in range(3):
edge = Line(proj_points[index - 1], proj_points[index])
# the edge should be clockwise around the model
if pdot(pcross(edge.dir, triangle.normal), up_vector) < 0:
edge = Line(edge.p2, edge.p1)
edges.append((edge, proj_points[index - 2]))
outer_edges = []
for edge, other_point in edges:
# pick only edges, where the other point is on the right side
if pdot(pcross(psub(other_point, edge.p1), edge.dir),
up_vector) > 0:
outer_edges.append(edge)
if len(outer_edges) == 0:
# the points seem to be an one line
# pick the longest edge
long_edge = edges[0][0]
for edge, other_point in edges[1:]:
if edge.len > long_edge.len:
long_edge = edge
outer_edges = [long_edge]
else:
edge = Line(proj_points[0], proj_points[1])
if pdot(pcross(edge.dir, triangle.normal), up_vector) < 0:
edge = Line(edge.p2, edge.p1)
outer_edges = [edge]
else:
# some parts of the triangle are above and some below the cutter level
# Cases (2a), (2b), (3a) and (3b)
points_above = [
plane.get_point_projection(p) for p in triangle.get_points()
if p[2] > z
]
waterline = plane.intersect_triangle(triangle)
if waterline is None:
if len(points_above) == 0:
# the highest point of the triangle is at z
outer_edges = []
else:
if abs(triangle.minz - z) < epsilon:
# This is just an accuracy issue (see the
# "triangle.minz >= z" statement above).
outer_edges = []
elif not [
p for p in triangle.get_points() if p[2] > z + epsilon
]:
# same as above: fix for inaccurate floating calculations
outer_edges = []
else:
# this should not happen
raise ValueError((
"Could not find a waterline, but there are points above z "
"level (%f): %s / %s") % (z, triangle, points_above))
else:
# remove points that are not part of the waterline
points_above = [
p for p in points_above
if (p != waterline.p1) and (p != waterline.p2)
]
if len(points_above) == 0:
# part of case (2a)
outer_edges = [waterline]
elif len(points_above) == 1:
other_point = points_above[0]
dot = pdot(
pcross(psub(other_point, waterline.p1), waterline.dir),
up_vector)
if dot > 0:
# Case (2b)
outer_edges = [waterline]
elif dot < 0:
# Case (3b)
edges = []
edges.append(Line(waterline.p1, other_point))
edges.append(Line(waterline.p2, other_point))
outer_edges = []
for edge in edges:
if pdot(pcross(edge.dir, triangle.normal),
up_vector) < 0:
outer_edges.append(Line(edge.p2, edge.p1))
else:
outer_edges.append(edge)
else:
# the three points are on one line
# part of case (2a)
edges = []
edges.append(waterline)
edges.append(Line(waterline.p1, other_point))
edges.append(Line(waterline.p2, other_point))
edges.sort(key=lambda x: x.len)
edge = edges[-1]
if pdot(pcross(edge.dir, triangle.normal), up_vector) < 0:
outer_edges = [Line(edge.p2, edge.p1)]
else:
outer_edges = [edge]
else:
# two points above
other_point = points_above[0]
dot = pdot(
pcross(psub(other_point, waterline.p1), waterline.dir),
up_vector)
if dot > 0:
# Case (2b)
# the other two points are on the right side
outer_edges = [waterline]
elif dot < 0:
# Case (3a)
edge = Line(points_above[0], points_above[1])
if pdot(pcross(edge.dir, triangle.normal), up_vector) < 0:
outer_edges = [Line(edge.p2, edge.p1)]
else:
outer_edges = [edge]
else:
edges = []
# pick the longest combination of two of these points
# part of case (2a)
# TODO: maybe we should use the waterline instead?
# (otherweise the line could be too long and thus
# connections to the adjacent waterlines are not discovered?
# Test this with an appropriate test model.)
points = [waterline.p1, waterline.p2] + points_above
for p1 in points:
for p2 in points:
if p1 is not p2:
edges.append(Line(p1, p2))
edges.sort(key=lambda x: x.len)
edge = edges[-1]
if pdot(pcross(edge.dir, triangle.normal), up_vector) < 0:
outer_edges = [Line(edge.p2, edge.p1)]
else:
outer_edges = [edge]
# calculate the maximum diagonal length within the model
x_dim = abs(model.maxx - model.minx)
y_dim = abs(model.maxy - model.miny)
z_dim = abs(model.maxz - model.minz)
max_length = sqrt(x_dim**2 + y_dim**2 + z_dim**2)
result = []
for edge in outer_edges:
direction = pnormalized(pcross(up_vector, edge.dir))
if direction is None:
continue
direction = pmul(direction, max_length)
edge_dir = psub(edge.p2, edge.p1)
# TODO: Adapt the number of potential starting positions to the length
# of the line. Don't use 0.0 and 1.0 - this could result in ambiguous
# collisions with triangles sharing these vertices.
for factor in (0.5, epsilon, 1.0 - epsilon, 0.25, 0.75):
start = padd(edge.p1, pmul(edge_dir, factor))
# We need to use the triangle collision algorithm here - because we
# need the point of collision in the triangle.
collisions = get_free_paths_triangles([model],
cutter,
start,
padd(start, direction),
return_triangles=True)
for index, coll in enumerate(collisions):
if ((index % 2 == 0) and (coll[1] is not None)
and (coll[2] is not None)
and (pdot(psub(coll[0], start), direction) > 0)):
cl, hit_t, cp = coll
break
else:
log.debug("Failed to detect any collision: %s / %s -> %s",
edge, start, direction)
continue
proj_cp = plane.get_point_projection(cp)
# e.g. the Spherical Cutter often does not collide exactly above
# the potential collision line.
# TODO: maybe an "is cp inside of the triangle" check would be good?
if (triangle is hit_t) or (edge.is_point_inside(proj_cp)):
result.append((cl, edge))
# continue with the next outer_edge
break
# Don't check triangles again that are completely above the z level and
# did not return any collisions.
if not result and (triangle.minz > z):
# None indicates that the triangle needs no further evaluation
return None
return result
You should have received a copy of the GNU General Public License
along with PyCAM. If not, see <http://www.gnu.org/licenses/>.
"""
import math
from pycam.Geometry import sqrt
# see BRL-CAD/src/libbn/poly.c
EPSILON = 1e-4
SMALL = 1e-4
INV_2 = 0.5
INV_3 = 1.0 / 3.0
INV_4 = 0.25
INV_27 = 1.0 / 27.0
SQRT3 = sqrt(3.0)
PI_DIV_3 = math.pi / 3.0
def near_zero(x, epsilon=EPSILON):
return abs(x) < epsilon
def cuberoot(x):
if x >= 0:
return pow(x, INV_3)
else:
return -pow(-x, INV_3)
def poly1_roots(a, b):
