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 project_point_plane(point, plane): """Project a point onto a plane. The projection is in the direction perpendicular to the plane. The projected point is thus the closest point on the plane to the original point. Parameters: point (sequence of float): XYZ coordinates of the original point. plane (tuple): Base poin.t and normal vector defining the plane Returns: list: XYZ coordinates of the projected point. Examples: >>> from compas.geometry.transformations import project_point_plane >>> point = [3.0, 3.0, 3.0] >>> plane = ([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]) # the XY plane >>> project_point_plane(point, plane) [3.0, 3.0, 3.0] References: http://stackoverflow.com/questions/8942950/how-do-i-find-the-orthogonal-projection-of-a-point-onto-a-plane http://math.stackexchange.com/questions/444968/project-a-point-in-3d-on-a-given-plane """ base, normal = plane normal = normalize_vector(normal) vector = subtract_vectors(point, base) snormal = scale_vector(normal, dot_vectors(vector, normal)) return subtract_vectors(point, snormal)
def mirror_vector_vector(v1, v2): """Mirrors vector about vector. Parameters ---------- v1 : list of float The vector. v2 : list of float The normalized vector as mirror axis Returns ------- list of float The mirrored vector. Notes ----- For more info, see [1]_. References ---------- .. [1] Math Stack Exchange. *How to get a reflection vector?* Available at: https://math.stackexchange.com/questions/13261/how-to-get-a-reflection-vector. """ return subtract_vectors(v1, scale_vector(v2, 2 * dot_vectors(v1, v2)))
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 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 matrix_from_orthogonal_projection(point, normal): """Returns an orthogonal projection matrix to project onto a plane defined by point and normal. Parameters ---------- point : list of float Base point of the plane. normal : list of float Normal vector of the plane. Examples -------- >>> point = [0, 0, 0] >>> normal = [0, 0, 1] >>> P = matrix_from_orthogonal_projection(point, normal) """ T = identity_matrix(4) normal = normalize_vector(normal) for j in range(3): for i in range(3): T[i][j] -= normal[i] * normal[j] # outer_product T[0][3], T[1][3], T[2][3] = scale_vector( normal, dot_vectors(point, normal)) return T
def matrix_from_parallel_projection(point, normal, direction): """Returns an parallel projection matrix to project onto a plane defined by point, normal and direction. Parameters ---------- point : list of float Base point of the plane. normal : list of float Normal vector of the plane. direction : list of float Direction of the projection. Examples -------- >>> point = [0, 0, 0] >>> normal = [0, 0, 1] >>> direction = [1, 1, 1] >>> P = matrix_from_parallel_projection(point, normal, direction) """ T = identity_matrix(4) normal = normalize_vector(normal) scale = dot_vectors(direction, normal) for j in range(3): for i in range(3): T[i][j] -= direction[i] * normal[j] / scale T[0][3], T[1][3], T[2][3] = scale_vector( direction, dot_vectors(point, normal) / scale) return T
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 circle_from_points(a, b, c): """Construct a circle from three points. Parameters ---------- a : sequence of float XYZ coordinates. b : sequence of float XYZ coordinates. c : sequence of float XYZ coordinates. Returns ------- circle : tuple Center, radius, normal of the circle. References ---------- https://en.wikipedia.org/wiki/Circumscribed_circle Examples -------- >>> """ ab = subtract_vectors(b, a) cb = subtract_vectors(b, c) ba = subtract_vectors(a, b) ca = subtract_vectors(a, c) ac = subtract_vectors(c, a) bc = subtract_vectors(c, b) normal = normalize_vector(cross_vectors(ab, ac)) d = 2 * length_vector_sqrd(cross_vectors(ba, cb)) A = length_vector_sqrd(cb) * dot_vectors(ba, ca) / d B = length_vector_sqrd(ca) * dot_vectors(ab, cb) / d C = length_vector_sqrd(ba) * dot_vectors(ac, bc) / d Aa = scale_vector(a, A) Bb = scale_vector(b, B) Cc = scale_vector(c, C) center = sum_vectors([Aa, Bb, Cc]) radius = length_vector(subtract_vectors(a, center)) return center, radius, normal
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 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 weighted_centroid_points(points, weights): """Compute the weighted centroid of a set of points. The weights can be any between minus and plus infinity. Parameters ---------- points : list A list of point coordinates. weights : list A list of weight floats. Returns ------- list The coordinates of the weighted centroid. """ vectors = [scale_vector(point, weight) for point, weight in zip(points, weights)] vector = scale_vector(sum_vectors(vectors), 1. / sum(weights)) return vector
def matrix_from_shear(angle, direction, point, normal): """Constructs a shear matrix by an angle along the direction vector on the shear plane (defined by point and normal). Parameters ---------- angle : float The angle in radians. direction : list of float The direction vector as list of 3 numbers. It must be orthogonal to the normal vector. point : list of float The point of the shear plane as list of 3 numbers. normal : list of float The normal of the shear plane as list of 3 numbers. Raises ------ ValueError If direction and normal are not orthogonal. Notes ----- A point P is transformed by the shear matrix into P" such that the vector P-P" is parallel to the direction vector and its extent is given by the angle of P-P'-P", where P' is the orthogonal projection of P onto the shear plane (defined by point and normal). Examples -------- >>> angle = 0.1 >>> direction = [0.1, 0.2, 0.3] >>> point = [4, 3, 1] >>> normal = cross_vectors(direction, [1, 0.3, -0.1]) >>> S = matrix_from_shear(angle, direction, point, normal) """ normal = normalize_vector(normal) direction = normalize_vector(direction) if math.fabs(dot_vectors(normal, direction)) > _EPS: raise ValueError('Direction and normal vectors are not orthogonal') angle = math.tan(angle) M = [[1. if i == j else 0. for i in range(4)] for j in range(4)] for j in range(3): for i in range(3): M[i][j] += angle * direction[i] * normal[j] M[0][3], M[1][3], M[2][3] = scale_vector( direction, -angle * dot_vectors(point, normal)) return M
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 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 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 mirror_vector_vector(v1, v2): """Mirrors vector about vector. Parameters: v1 (tuple, list, Vector): The vector. v2 (tuple, list, Vector): The normalized vector as mirror axis Returns: Tuple: mirrored vector Resources: http://math.stackexchange.com/questions/13261/how-to-get-a-reflection-vector """ return subtract_vectors(v1, scale_vector(v2, 2 * dot_vectors(v1, v2)))
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 distance_line_line(l1, l2, tol=0.0): """Compute the shortest distance between two lines. Parameters ---------- l1 : tuple Two points defining a line. l2 : tuple Two points defining a line. Returns ------- d : float The distance between the two lines. Notes ----- The distance is the absolute value of the dot product of a unit vector that is perpendicular to the two lines, and the vector between two points on the lines ([1]_, [2]_). If each of the lines is defined by two points (:math:`l_1 = (\mathbf{x_1}, \mathbf{x_2})`, :math:`l_2 = (\mathbf{x_3}, \mathbf{x_4})`), then the unit vector that is perpendicular to both lines is... References ---------- .. [1] Weisstein, E.W. *Line-line Distance*. Available at: http://mathworld.wolfram.com/Line-LineDistance.html. .. [2] Wikipedia. *Skew lines Distance*. Available at: https://en.wikipedia.org/wiki/Skew_lines#Distance. Examples -------- >>> """ a, b = l1 c, d = l2 ab = subtract_vectors(b, a) cd = subtract_vectors(d, c) ac = subtract_vectors(c, a) n = cross_vectors(ab, cd) l = length_vector(n) if l <= tol: return distance_point_point(closest_point_on_line(l1[0], l2), l1[0]) n = scale_vector(n, 1.0 / l) return fabs(dot_vectors(n, ac))
def axis_angle_vector(self): """Returns the axis-angle vector of the ``Rotation``. Returns: (:obj:`list` of :obj:`float`): Three numbers that represent the axis of rotation and angle of rotation through the vector's magnitude. Example: >>> aav1 = [-0.043, -0.254, 0.617] >>> R = Rotation.from_axis_angle_vector(aav1) >>> aav2 = R.axis_angle_vector >>> allclose(aav1, aav2) True """ axis, angle = self.axis_and_angle return scale_vector(axis, angle)
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_plane(point, plane): """Project a point onto a plane. Parameters ---------- point : list of float XYZ coordinates of the point. plane : tuple Base point and normal vector defining the projection plane. Returns ------- list XYZ coordinates of the projected point. Notes ----- The projection is in the direction perpendicular to the plane. The projected point is thus the closest point on the plane to the original point [1]_. References ---------- .. [1] Math Stack Exchange. *Project a point in 3D on a given plane*. Available at: https://math.stackexchange.com/questions/444968/project-a-point-in-3d-on-a-given-plane. Examples -------- >>> from compas.geometry import project_point_plane >>> point = [3.0, 3.0, 3.0] >>> plane = ([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]) # the XY plane >>> project_point_plane(point, plane) [3.0, 3.0, 0.0] """ base, normal = plane normal = normalize_vector(normal) vector = subtract_vectors(point, base) snormal = scale_vector(normal, dot_vectors(vector, normal)) return subtract_vectors(point, snormal)
def matrix_from_perspective_projection(point, normal, perspective): """Returns a perspective projection matrix to project onto a plane defined by point, normal and perspective. Parameters ---------- point : list of float Base point of the projection plane. normal : list of float Normal vector of the projection plane. perspective : list of float Perspective of the projection. Examples -------- >>> point = [0, 0, 0] >>> normal = [0, 0, 1] >>> perspective = [1, 1, 0] >>> P = matrix_from_perspective_projection(point, normal, perspective) """ T = identity_matrix(4) normal = normalize_vector(normal) T[0][0] = T[1][1] = T[2][2] = dot_vectors( subtract_vectors(perspective, point), normal) for j in range(3): for i in range(3): T[i][j] -= perspective[i] * normal[j] T[0][3], T[1][3], T[2][3] = scale_vector(perspective, dot_vectors(point, normal)) for i in range(3): T[3][i] -= normal[i] T[3][3] = dot_vectors(perspective, normal) return T
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)
def axis_angle_vector_from_matrix(M): """Returns the axis-angle vector of the rotation matrix M. """ axis, angle = axis_and_angle_from_matrix(M) return scale_vector(axis, angle)