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)
