def intersection_plane_plane(plane1, plane2, epsilon=1e-6): """Computes the intersection of two planes Parameters ---------- plane1 : tuple The base point and normal (normalized) defining the 1st plane. plane2 : tuple The base point and normal (normalized) defining the 2nd plane. Returns ------- line : tuple Two points defining the intersection line. None if planes are parallel. """ # check for parallelity of planes if abs(dot_vectors(plane1[1], plane2[1])) > 1 - epsilon: return None vec = cross_vectors(plane1[1], plane2[1]) # direction of intersection line p1 = plane1[0] vec_inplane = cross_vectors(vec, plane1[1]) p2 = add_vectors(p1, vec_inplane) px1 = intersection_line_plane((p1, p2), plane2) px2 = add_vectors(px1, vec) return px1, px2
def intersection_plane_plane(plane1, plane2, tol=1e-6): """Computes the intersection of two planes Parameters ---------- plane1 : tuple The base point and normal (normalized) defining the 1st plane. plane2 : tuple The base point and normal (normalized) defining the 2nd plane. tol : float, optional A tolerance for membership verification. Default is ``1e-6``. Returns ------- line : tuple Two points defining the intersection line. None if planes are parallel. """ o1, n1 = plane1 o2, n2 = plane2 if fabs(dot_vectors(n1, n2)) >= 1 - tol: return None # direction of intersection line d = cross_vectors(n1, n2) # vector in plane 1 perpendicular to the direction of the intersection line v1 = cross_vectors(d, n1) # point on plane 1 p1 = add_vectors(o1, v1) x1 = intersection_line_plane((o1, p1), plane2, tol=tol) x2 = add_vectors(x1, d) return x1, x2
def check_length_sol_one(solution, pt_mean, pt, b1, b2, b1_key, b2_key, b_struct): vec_sol = solution dpp = dropped_perpendicular_points(pt, add_vectors(pt, vec_sol), b1["axis_endpoints"][0], b1["axis_endpoints"][1]) pt_x1 = dpp[0] pt_1 = dpp[1] dpp = dropped_perpendicular_points(pt, add_vectors(pt, vec_sol), b2["axis_endpoints"][0], b2["axis_endpoints"][1]) pt_x2 = dpp[0] pt_2 = dpp[1] if pt_x1 == None: return None if pt_x2 == None: return None pts_all_b1 = [] b_vert_n = b_struct.vertex_neighbors(b1_key) pts_all_b1.append(b_struct.vertex[b1_key]["axis_endpoints"][0]) pts_all_b1.append(b_struct.vertex[b1_key]["axis_endpoints"][1]) for n in b_vert_n: pts_all_b1.append( dropped_perpendicular_points( b1["axis_endpoints"][0], b1["axis_endpoints"][1], b_struct.vertex[n]["axis_endpoints"][0], b_struct.vertex[n]["axis_endpoints"][1])[0]) pts_all_b1.append(pt_1) pts_b1 = find_points_extreme(pts_all_b1, b1["axis_endpoints"]) pts_all_b2 = [] b_vert_n = b_struct.vertex_neighbors(b2_key) pts_all_b2.append(b_struct.vertex[b2_key]["axis_endpoints"][0]) pts_all_b2.append(b_struct.vertex[b2_key]["axis_endpoints"][1]) for n in b_vert_n: pts_all_b2.append( dropped_perpendicular_points( b2["axis_endpoints"][0], b2["axis_endpoints"][1], b_struct.vertex[n]["axis_endpoints"][0], b_struct.vertex[n]["axis_endpoints"][1])[0]) pts_all_b2.append(pt_2) pts_b2 = find_points_extreme(pts_all_b2, b2["axis_endpoints"]) vec_test_dir_1 = subtract_vectors(pt_mean, pt) if not check_dir(vec_sol, vec_test_dir_1): vec_sol = scale_vector(vec_sol, -1) lx1 = distance_point_point(pt, pt_x1) lx2 = distance_point_point(pt, pt_x2) l_max = lx1 if lx1 > lx2 else lx2 sol = [vec_sol, l_max, pts_b1, pts_b2] return sol
def lines_tangent_to_cylinder(base_point, line_vect, ref_point, dist): """Calculating of plane tangents to one cylinder See SP dissertation 3.1.3.b (p. 74) .. image:: ../images/plane_tangent_to_one_cylinder.png :scale: 80 % :align: center Parameters ---------- base_point : [type] [description] line_vect : [type] vector [other end of the axis, base_point], **the direction here is very important!** ref_point : point new point Q dist : float radius of the cylinder Returns ------- list [origin point M, vector MB, -1 * vector MB], the latter two entries represent the tangent points' local coordinate in the plane [point M, e_x, e_y] """ l_vect = normalize_vector(line_vect) line_QMprime = subtract_vectors(ref_point, base_point) # ppol = point M, project out longitutude axis component of base_point ppol = add_vectors(base_point, scale_vector(l_vect, dot_vectors(line_QMprime, l_vect))) # ppolr_vect = line QB line_QM = subtract_vectors(ref_point, ppol) e_x = normalize_vector(line_QM) e_y = cross_vectors(e_x, l_vect) if length_vector(line_QM) == 0: return None x = dist / length_vector(line_QM) # x coordinate in the local axis d_e1 = scale_vector(e_x, dist * x) # d(radius of bar section) has to be larger than l(distance from point to bar axis), otherwise the sqrt turns negative # if d < l: change ref_point if x * x < 1.0: # y coordinate in the local axis d_e2 = scale_vector(e_y, dist * math.sqrt(1.0 - x * x)) else: return None # upper tangent point d_e_add = add_vectors(d_e1, d_e2) # lower tangent point d_e_sub = subtract_vectors(d_e1, d_e2) return [ppol, d_e_add, d_e_sub]
def find_point_2(x, *args): ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, d1, d2, ind = args r_c = 2 * radius ref_point_tmp = add_vectors(scale_vector(ex, x), scale_vector(ey, math.sqrt(r_c * r_c - x * x))) ref_point = add_vectors(ref_point_tmp, ptM) vec_l = tangent_from_point_one(pt_b_1, l_1, pt_b_2, l_2, ref_point, d1, d2, ind)[0] return ref_point, vec_l
def calculate_offset_pos_two_side_one_point_locked(b_struct, v_key, pt_1, pt_2, v1, v2, d_o_1, d_o_2): """calculate offsetted plane when the bar's both sides are blocked by vector v1 and v2 # ! Note: the old y axis is kept in this function, local x axis is updated Parameters ---------- b_struct : [type] [description] v_key : int vertex key in BarStructure, representing a physical bar pt_1 : list first contact point's projection on the bar's axis pt_2 : list second contact point's projection on the bar's axis v1 : list first contact point - contact point vector v2 : list second contact point - contact point vector d_o_1 : float offset distance for end point #1 d_o_2 : float offset distance for end point #2 Returns ------- tuple offset plane's origin, x-, y-, z-axis """ pt_1_new = add_vectors(pt_1, scale_vector(v1, -1. * d_o_1)) pt_2_new = add_vectors(pt_2, scale_vector(v2, -1. * d_o_2)) vec_x_new = normalize_vector(vector_from_points(pt_1_new, pt_2_new)) x_ax = b_struct.vertex[v_key]["gripping_plane"][1] if not angle_vectors(x_ax, vec_x_new, deg=True) < 90: vec_x_new = scale_vector(vec_x_new, -1.) # transform gripping plane pt_o = b_struct.vertex[v_key]["gripping_plane"][0] y_ax = b_struct.vertex[v_key]["gripping_plane"][2] vec_z = cross_vectors(vec_x_new, y_ax) l_n = (pt_1_new, pt_2_new) pt_o_new = closest_point_on_line(pt_o, l_n) return pt_o_new, vec_x_new, y_ax, vec_z
def closest_point_on_line(point, line): """Computes closest point on line to a given point. Parameters ---------- point : sequence of float XYZ coordinates. line : tuple Two points defining the line. Returns ------- list XYZ coordinates of closest point. Examples -------- >>> See Also -------- :func:`compas.geometry.transformations.project_point_line` """ a, b = line ab = subtract_vectors(b, a) ap = subtract_vectors(point, a) c = vector_component(ap, ab) return add_vectors(a, c)
def tween_points_distance(points1, points2, dist, index=None): """Compute an interpolated set of points between two sets of points, at a given distance. Parameters ---------- points1 : list The first set of points points2 : list The second set of points dist : float The distance from the first set to the second at which to compute the interpolated set. index: int The index of the point in the first set from which to calculate the distance to the second set. If no value is given, the first point will be used. Returns ------- list List of points """ if not index: index = 0 d = distance_point_point(points1[index], points2[index]) scale = float(dist) / d tweens = [] for i in range(len(points1)): tweens.append( add_vectors( points1[i], scale_vector(vector_from_points(points1[i], points2[i]), scale))) return tweens
def tween_points(points1, points2, num): """Compute the interpolated points between two sets of points. Parameters ---------- points1 : list The first set of points points2 : list The second set of points num : int The number of interpolated sets to return Returns ------- list Nested list of points """ tweens = [] for j in range(num - 1): tween = [] for i in range(len(points1)): tween.append( add_vectors( points1[i], scale_vector(vector_from_points(points1[i], points2[i]), 1 / (num / (j + 1))))) tweens.append(tween) return tweens
def intersection_line_plane(line, plane, epsilon=1e-6): """Computes the intersection point of a line (ray) and a plane Parameters ---------- line : tuple Two points defining the line. plane : tuple The base point and normal defining the plane. Returns ------- point : tuple if the line (ray) intersects with the plane, None otherwise. """ pt1 = line[0] pt2 = line[1] p_cent = plane[0] p_norm = plane[1] v1 = subtract_vectors(pt2, pt1) dot = dot_vectors(p_norm, v1) if fabs(dot) > epsilon: v2 = subtract_vectors(pt1, p_cent) fac = -dot_vectors(p_norm, v2) / dot vec = scale_vector(v1, fac) return add_vectors(pt1, vec) return None
def project_point_line(point, line): """Project a point onto a line. Parameters ---------- point : list of float XYZ coordinates of the point. line : tuple Two points defining the projection line. Returns ------- list XYZ coordinates of the projected point. Notes ----- For more info, see [1]_. References ---------- .. [1] Wiki Books. *Linear Algebra/Orthogonal Projection Onto a Line*. Available at: https://en.wikibooks.org/wiki/Linear_Algebra/Orthogonal_Projection_Onto_a_Line. """ a, b = line ab = subtract_vectors(b, a) ap = subtract_vectors(point, a) c = vector_component(ap, ab) return add_vectors(a, c)
def intersection_segment_plane(segment, plane, epsilon=1e-6): """Computes the intersection point of a line segment and a plane Parameters: segment (tuple): Two points defining the line segment. plane (tuple): The base point and normal defining the plane. Returns: point (tuple) if the line segment intersects with the plane, None otherwise. """ pt1 = segment[0] pt2 = segment[1] p_cent = plane[0] p_norm = plane[1] v1 = subtract_vectors(pt2, pt1) dot = dot_vectors(p_norm, v1) if fabs(dot) > epsilon: v2 = subtract_vectors(pt1, p_cent) fac = -dot_vectors(p_norm, v2) / dot if fac > 0. and fac < 1.: vec = scale_vector(v1, fac) return add_vectors(pt1, vec) return None else: return None
def mirror_point_point(point, mirror): """Mirror a point about a point. Parameters: point (sequence of float): XYZ coordinates of the point to mirror. mirror (sequence of float): XYZ coordinates of the mirror point. """ return add_vectors(mirror, subtract_vectors(mirror, point))
def offset_line(line, distance, normal=[0., 0., 1.]): """Offset a line by a distance Parameters: line (tuple): Two points defining the line. distances (float or tuples of floats): The offset distance as float. A single value determines a constant offset. Alternatively, two offset values for the start and end point of the line can be used to a create variable offset. normal (tuple): The normal of the offset plane. Returns: offset line (tuple): Two points defining the offset line. Examples: .. code-block:: python line = [(0.0, 0.0, 0.0), (3.0, 3.0, 0.0)] distance = 0.2 # constant offset line_offset = offset_line(line, distance) print(line_offset) distance = [0.2, 0.1] # variable offset line_offset = offset_line(line, distance) print(line_offset) """ pt1, pt2 = line[0], line[1] vec = subtract_vectors(pt1, pt2) dir_vec = normalize_vector(cross_vectors(vec, normal)) if isinstance(distance, list): distances = distance else: distances = [distance, distance] vec_pt1 = scale_vector(dir_vec, distances[0]) vec_pt2 = scale_vector(dir_vec, distances[1]) pt1_new = add_vectors(pt1, vec_pt1) pt2_new = add_vectors(pt2, vec_pt2) return pt1_new, pt2_new
def find_point_3(x, *args): ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, b, ind_1, ind_2 = args x1 = x[0] x2 = x[1] pt_1_tmp = add_vectors(scale_vector(ex, x1), scale_vector(ey, x2)) pt_1 = add_vectors(pt_1_tmp, ptM) vec_l1 = tangent_from_point_one(pt_b_1, l_1, pt_b_2, l_2, pt_1, 2 * radius, 2 * radius, ind_1)[0] vec_l2 = tangent_from_point_one(pt_b_3, l_3, pt_b_4, l_4, pt_1, 2 * radius, 2 * radius, ind_2)[0] ref_point = pt_1 if not vec_l1 or not vec_l2: return None ang = angle_vectors(vec_l1, vec_l2) return ang, ref_point, vec_l1, vec_l2
def intersection_line_triangle(line, triangle, epsilon=1e-6): """Computes the intersection point of a line (ray) and a triangle based on the Moeller Trumbore intersection algorithm Parameters ---------- line : tuple Two points defining the line. triangle : sequence of sequence of float XYZ coordinates of the triangle corners. Returns ------- point : tuple if the line (ray) intersects with the triangle, None otherwise. Notes ----- The line is treated as continues, directed ray and not as line segment with a start and end point """ a, b, c = triangle v1 = subtract_vectors(line[1], line[0]) p1 = line[0] # Find vectors for two edges sharing V1 e1 = subtract_vectors(b, a) e2 = subtract_vectors(c, a) # Begin calculating determinant - also used to calculate u parameter p = cross_vectors(v1, e2) # if determinant is near zero, ray lies in plane of triangle det = dot_vectors(e1, p) # NOT CULLING if (det > -epsilon and det < epsilon): return None inv_det = 1.0 / det # calculate distance from V1 to ray origin t = subtract_vectors(p1, a) # Calculate u parameter and make_blocks bound u = dot_vectors(t, p) * inv_det # The intersection lies outside of the triangle if (u < 0.0 or u > 1.0): return None # Prepare to make_blocks v parameter q = cross_vectors(t, e1) # Calculate V parameter and make_blocks bound v = dot_vectors(v1, q) * inv_det # The intersection lies outside of the triangle if (v < 0.0 or u + v > 1.0): return None t = dot_vectors(e2, q) * inv_det if t > epsilon: return add_vectors(p1, scale_vector(v1, t)) # No hit return None
def p_planes_tangent_to_cylinder( base_point, line_vect, ref_point, dist, ): """find tangent planes of a cylinder passing through a given point () .. image:: ../images/plane_tangent_to_one_cylinder.png :scale: 80 % :align: center Parameters ---------- base_point : point point M line_vect : vector direction of the existing bar's axis, direction [the other pt, base_pt], **direction very important!** ref_point : point point Q dist : float cylinder radius Returns ------- list of two [ref_point, local_y, local_x] local x = QB local_y // line_vect """ l_vect = normalize_vector(line_vect) tangent_pts = lines_tangent_to_cylinder(base_point, line_vect, ref_point, dist) if tangent_pts is None: return None base_pt, upper_tang_pt, lower_tang_pt = tangent_pts r1 = subtract_vectors(add_vectors(base_pt, upper_tang_pt), ref_point) r1 = normalize_vector(r1) r2 = subtract_vectors(add_vectors(base_pt, lower_tang_pt), ref_point) r2 = normalize_vector(r2) return [[ref_point, l_vect, r1], [ref_point, l_vect, r2]]
def center_of_mass_polyhedron(polyhedron): """Compute the center of mass of a polyhedron""" vertices, faces = polyhedron V = 0 x = 0.0 y = 0.0 z = 0.0 ex = [1.0, 0.0, 0.0] ey = [0.0, 1.0, 0.0] ez = [0.0, 0.0, 1.0] for face in faces: if len(face) == 3: triangles = [face] else: triangles = [] # for i in range(1, len(face) - 1): # triangles.append(face[0:1] + vertices[i:i + 2]) for triangle in triangles: a = vertices[triangle[0]] b = vertices[triangle[1]] c = vertices[triangle[2]] ab = subtract_vectors(b, a) ac = subtract_vectors(c, a) n = cross_vectors(ab, ac) V += dot_vectors(a, n) nx = dot_vectors(n, ex) ny = dot_vectors(n, ey) nz = dot_vectors(n, ez) for j in (-1, 0, 1): ab = add_vectors(vertices[triangle[j]], vertices[triangle[j + 1]]) x += nx * dot_vectors(ab, ex)**2 y += ny * dot_vectors(ab, ey)**2 z += nz * dot_vectors(ab, ez)**2 if V < 1e-9: V = 0.0 d = 1.0 / 48.0 else: V = V / 6.0 d = 1.0 / 48.0 / V x *= d y *= d z *= d return x, y, z
def f_tangent_point_3(x, *args): ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, b, ind_1, ind_2 = args x1 = x[0] x2 = x[1] ref_point_tmp = add_vectors(scale_vector(ex, x1), scale_vector(ey, x2)) ref_point = add_vectors(ref_point_tmp, ptM) tfp_1 = tangent_from_point_one(pt_b_1, l_1, pt_b_2, l_2, ref_point, 2 * radius, 2 * radius, ind_1) if tfp_1: vec_l1 = tfp_1[0] else: print("problem in opt 3 - 1") f = 180 return f tfp_2 = tangent_from_point_one(pt_b_3, l_3, pt_b_4, l_4, ref_point, 2 * radius, 2 * radius, ind_2) if tfp_2: # vec_l2 = tfp_2[ind_2] vec_l2 = tfp_2[0] else: print("problem in opt 3 - 1") f = 180 return f #return None ang_v = angle_vectors(vec_l1, vec_l2, deg=True) if 180 - ang_v < 90: f = 180 - ang_v else: f = ang_v return f
def tangent_through_two_points(base_point1, line_vect1, ref_point1, base_point2, line_vect2, ref_point2, dist1, dist2): ind = [0, 1] sols_vec = [] sols_pts = [] # print("tangent_through_two_points", base_point1, line_vect1, ref_point1, base_point2, line_vect2, ref_point2, dist1, dist2) for i in ind: ret_p1 = p_planes_tangent_to_cylinder(base_point1, line_vect1, ref_point2, dist1 + dist2 + dist1 + dist2) ret1 = ret_p1[i] z_vec = cross_vectors(line_vect1, ret1[2]) plane1 = (ret1[0], z_vec) # print("plane1", plane1) pp1 = project_points_plane([ref_point1], plane1)[0] vec_move = scale_vector(subtract_vectors(ref_point1, pp1), 0.5) pt1 = add_vectors(pp1, vec_move) for j in ind: ret_p2 = p_planes_tangent_to_cylinder( base_point2, line_vect2, ref_point1, dist1 + dist2 + dist1 + dist2) ret2 = ret_p2[j] z_vec = cross_vectors(line_vect2, ret2[2]) plane2 = (ret2[0], z_vec) pp2 = project_points_plane([ref_point2], plane2)[0] vec_move = scale_vector(subtract_vectors(ref_point2, pp2), 0.5) pt2 = add_vectors(pp2, vec_move) sols_pts.append([pt1, pt2]) sol_vec = subtract_vectors(pt1, pt2) sols_vec.append(sol_vec) return sols_pts
def calculate_offset_pos_two_side_two_point_locked(b_struct, v_key, vecs_con_1, vecs_con_2, pts_con_1, pts_con_2, d_o_1, d_o_2): """calculate offsetted plane when the bar's both ends have two contact points """ assert len(vecs_con_1) == 2 and len(pts_con_1) == 2 assert len(vecs_con_2) == 2 and len(pts_con_2) == 2 map(normalize_vector, vecs_con_1) map(normalize_vector, vecs_con_2) v1_1, v1_2 = vecs_con_1 v2_1, v2_2 = vecs_con_2 pt_1_1, pt_1_2 = pts_con_1 pt_2_1, pt_2_2 = pts_con_2 vm_1 = scale_vector(normalize_vector(add_vectors(v1_1, v1_2)), -1. * d_o_1) # original contact point (assuming two bars have the same radius) pt_1 = centroid_points([pt_1_1, pt_1_2]) pt_1_new = translate_points([pt_1], vm_1)[0] vm_2 = scale_vector(normalize_vector(add_vectors(v2_1, v2_2)), -1. * d_o_2) pt_2 = centroid_points([pt_2_1, pt_2_2]) pt_2_new = translate_points([pt_2], vm_2)[0] vec_x_new = normalize_vector(vector_from_points(pt_1_new, pt_2_new)) x_ax = b_struct.vertex[v_key]["gripping_plane"][1] if not angle_vectors(x_ax, vec_x_new, deg=True) < 90: vec_x_new = scale_vector(vec_x_new, -1.) pt_o = b_struct.vertex[v_key]["gripping_plane"][0] y_ax = b_struct.vertex[v_key]["gripping_plane"][2] vec_z = cross_vectors(vec_x_new, y_ax) l_n = (pt_1_new, pt_2_new) pt_o_n = closest_point_on_line(pt_o, l_n) return pt_o_n, vec_x_new, y_ax, vec_z
def reflect_line_triangle(line, triangle, tol=1e-6): """Bounce a line of a reflection triangle. Parameters ---------- line : tuple Two points defining the line. triangle : tuple The triangle vertices. tol : float, optional A tolerance for membership verification. Default is ``1e-6``. Returns ------- tuple The reflected line defined by the intersection point of the line and triangle and the mirrored start point of the line with respect to a line perpendicular to the triangle through the intersection. Notes ----- The direction of the line and triangle are important. The line is only reflected if it points towards the front of the triangle. This is true if the dot product of the direction vector of the line and the normal vector of the triangle is smaller than zero. Examples -------- >>> triangle = [1.0, 0, 0], [-1.0, 0, 0], [0, 0, 1.0] >>> line = [-1, 1, 0], [-0.5, 0.5, 0] >>> reflect_line_triangle(line, triangle) ([0.0, 0.0, 0.0], [1.0, 1.0, 0.0]) """ x = intersection_line_triangle(line, triangle, tol=tol) if not x: return a, b = line t1, t2, t3 = triangle ab = subtract_vectors(b, a) n = cross_vectors(subtract_vectors(t2, t1), subtract_vectors(t3, t1)) if dot_vectors(ab, n) > 0: # the line does not point towards the front of the triangle return mirror = x, add_vectors(x, n) return x, mirror_point_line(a, mirror)
def f_tangent_point_2(x, *args): ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, d1, d2, ind = args r_c = 2 * radius ref_point_tmp = add_vectors(scale_vector(ex, x), scale_vector(ey, math.sqrt(r_c * r_c - x * x))) ref_point = add_vectors(ref_point_tmp, ptM) vecs_l_all = tangent_from_point_one(pt_b_1, l_1, pt_b_2, l_2, ref_point, d1, d2, ind) if vecs_l_all: vec_l = vecs_l_all[0] else: print("error in f") f = 1 return f f = abs( dot_vectors(normalize_vector(vec_l), normalize_vector(vector_from_points(ptM, ref_point)))) return f
def mirror_point_point(point, mirror): """Mirror a point about a point. Parameters ---------- point : list of float XYZ coordinates of the point to mirror. mirror : list of float XYZ coordinates of the mirror point. Returns ------- list of float The mirrored point. """ return add_vectors(mirror, subtract_vectors(mirror, point))
def mirror_point_plane(point, plane): """Mirror a point about a plane. Parameters ---------- point : list of float XYZ coordinates of mirror point. plane : tuple Base point and normal defining the mirror plane. Returns ------- list of float XYZ coordinates of the mirrored point. """ closest = closest_point_on_plane(point, plane) return add_vectors(closest, subtract_vectors(closest, point))
def mirror_point_line(point, line): """Mirror a point about a line. Parameters ---------- point : list of float XYZ coordinates of the point to mirror. line : tuple Two points defining the mirror line. Returns ------- list of float The mirrored point. """ closest = closest_point_on_line(point, line) return add_vectors(closest, subtract_vectors(closest, point))
def intersection_segment_plane(segment, plane, tol=1e-6): """Computes the intersection point of a line segment and a plane Parameters ---------- segment : tuple Two points defining the line segment. plane : tuple The base point and normal defining the plane. tol : float, optional A tolerance for membership verification. Default is ``1e-6``. Returns ------- point : tuple if the line segment intersects with the plane, None otherwise. """ a, b = segment o, n = plane ab = subtract_vectors(b, a) cosa = dot_vectors(n, ab) if fabs(cosa) <= tol: # if the dot product (cosine of the angle between segment and plane) # is close to zero the line and the normal are almost perpendicular # hence there is no intersection return None # based on the ratio = -dot_vectors(n, ab) / dot_vectors(n, oa) # there are three scenarios # 1) 0.0 < ratio < 1.0: the intersection is between a and b # 2) ratio < 0.0: the intersection is on the other side of a # 3) ratio > 1.0: the intersection is on the other side of b oa = subtract_vectors(a, o) ratio = -dot_vectors(n, oa) / cosa if 0.0 <= ratio and ratio <= 1.0: ab = scale_vector(ab, ratio) return add_vectors(a, ab) return None
def project_point_line(point, line): """Project a point onto a line. Parameters: point (sequence of float): XYZ coordinates. line (tuple): Two points defining a line. Returns: list: XYZ coordinates of the projected point. References: https://en.wikibooks.org/wiki/Linear_Algebra/Orthogonal_Projection_Onto_a_Line """ a, b = line ab = subtract_vectors(b, a) ap = subtract_vectors(point, a) c = vector_component(ap, ab) return add_vectors(a, c)
def translate_points(points, vector): """Translate points. Parameters ---------- points : list of point A list of points. vector : vector A translation vector. Returns ------- list of point The translated points. Examples -------- >>> """ return [add_vectors(point, vector) for point in points]
def intersection_sphere_sphere(sphere1, sphere2): """Computes the intersection of 2 spheres. There are 4 cases of sphere-sphere intersection : 1) the spheres intersect in a circle, 2) they intersect in a point, 3) they overlap, 4) they do not intersect. Parameters ---------- sphere1 : tuple center, radius of the sphere. sphere2 : tuple center, radius of the sphere. Returns ------- case : str `point`, `circle`, or `sphere` result : tuple - point: xyz coordinates - circle: center, radius, normal - sphere: center, radius Examples -------- >>> sphere1 = (3.0, 7.0, 4.0), 10.0 >>> sphere2 = (7.0, 4.0, 0.0), 5.0 >>> result = intersection_sphere_sphere(sphere1, sphere2) >>> if result: >>> case, res = result >>> if case == "circle": >>> center, radius, normal = res >>> elif case == "point": >>> point = res >>> elif case == "sphere": >>> center, radius = res References -------- https://gamedev.stackexchange.com/questions/75756/sphere-sphere-intersection-and-circle-sphere-intersection """ center1, radius1 = sphere1 center2, radius2 = sphere2 distance = distance_point_point(center1, center2) # Case 4: No intersection if radius1 + radius2 < distance: return None # Case 4: No intersection, sphere is within the other sphere elif distance + min(radius1, radius2) < max(radius1, radius2): return None # Case 3: sphere's overlap elif radius1 == radius2 and distance == 0: return "sphere", sphere1 # Case 2: point intersection elif radius1 + radius2 == distance: ipt = subtract_vectors(center2, center1) ipt = scale_vector(ipt, radius1 / distance) ipt = add_vectors(center1, ipt) return "point", ipt # Case 2: point intersection, smaller sphere is within the bigger elif distance + min(radius1, radius2) == max(radius1, radius2): if radius1 > radius2: ipt = subtract_vectors(center2, center1) ipt = scale_vector(ipt, radius1 / distance) ipt = add_vectors(center1, ipt) else: ipt = subtract_vectors(center1, center2) ipt = scale_vector(ipt, radius2 / distance) ipt = add_vectors(center2, ipt) return "point", ipt # Case 1: circle intersection else: h = 0.5 + (radius1**2 - radius2**2) / (2 * distance**2) ci = subtract_vectors(center2, center1) ci = scale_vector(ci, h) ci = add_vectors(center1, ci) ri = sqrt(radius1**2 - h**2 * distance**2) normal = scale_vector(subtract_vectors(center2, center1), 1 / distance) return "circle", (ci, ri, normal)