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