def __implementation_distance_to_segment(self,
segment=None,
pt_beg=None,
pt_end=None):
"""
Consider here triangle made by point and segment.
If there is any angle >= 90 degrees the distance is equal to the length
of the edge lying near this angle. Otherwise the distance is equal to
the height of the triangle.
"""
pt0 = self
if not segment is None:
pt1 = segment.pt_1
pt2 = segment.pt_2
elif not None in (pt_beg, pt_end):
pt1 = pt_beg
pt2 = pt_end
else:
print('error in point __util_distance_to_segment:',
'incorrect arguments')
return None
v01 = Vector(pt_1=pt0, pt_2=pt1)
v02 = Vector(pt_1=pt0, pt_2=pt2)
v12 = Vector(pt_1=pt1, pt_2=pt2)
if v01.product_with(v12) >= 0:
return v01.length()
elif v02.product_with(v12) <= 0:
return v02.length()
height = self.__implementation_distance_to_line(pt_1=pt1, pt_2=pt2)
return height
def __implementation_distance_to_plane(self,
plane=None,
pt_1=None, pt_2=None, pt_3=None):
"""
If plane and segment do not intersect, distance equals
to the smallest between the distances from segment's ends
to plane.
"""
if not plane is None:
sign1 = plane.get_semispace(self.pt_1)
sign2 = plane.get_semispace(self.pt_2)
if sign1 * sign2 <= 0: # segment and plane cross
return 0
return min(self.pt_1.distance_to_plane(plane),
self.pt_2.distance_to_plane(plane))
elif not None in (pt_1, pt_2, pt_3):
elements1 = [pt_1.x - self.pt_1.x,
pt_1.y - self.pt_1.y,
pt_1.z - self.pt_1.z,
pt_2.x - self.pt_1.x,
pt_2.y - self.pt_1.y,
pt_2.z - self.pt_1.z,
pt_3.x - self.pt_1.x,
pt_3.y - self.pt_1.y,
pt_3.z - self.pt_1.z]
elements2 = [pt_1.x - self.pt_2.x,
pt_1.y - self.pt_2.y,
pt_1.z - self.pt_2.z,
pt_2.x - self.pt_2.x,
pt_2.y - self.pt_2.y,
pt_2.z - self.pt_2.z,
pt_3.x - self.pt_2.x,
pt_3.y - self.pt_2.y,
pt_3.z - self.pt_2.z]
sign1 = Matrix3(elements=elements1).det()
sign2 = Matrix3(elements=elements2).det()
if sign1 * sign2 <= 0:
return 0
v12 = Vector(pt_1=pt_1, pt_2=pt_2)
v13 = Vector(pt_1=pt_1, pt_2=pt_3)
v23 = Vector(pt_1=pt_2, pt_2=pt_3)
cos_angle_1 = v12.product_with(v13) / v12.length() / v13.length()
sin_angle_1 = (1 - cos_angle_1**2)**0.5
triangle_square = 0.5 * v12.length() * v13.length() * sin_angle_1
return 3 * min(abs(sign1), abs(sign2)) / triangle_square
def __implementation_distance_to_plane(self,
plane=None,
pt_1=None,
pt_2=None,
pt_3=None): # TODO
"""
Find plane containing point and parallel to plane.
Difference in d values is equal to the desired distance.
"""
if not plane is None:
plane_a = plane.a
plane_b = plane.b
plane_c = plane.c
plane_d = plane.d
d = -self.x * plane_a - self.y * plane_b - self.z * plane_c
return abs(plane_d - d)
elif not None in (pt_1, pt_2, pt_3):
pt0 = self
v01 = Vector(pt_1=pt0, pt_2=pt_1)
v02 = Vector(pt_1=pt0, pt_2=pt_2)
v03 = Vector(pt_1=pt0, pt_2=pt_3)
# TODO - remove hardcode
if (v01.length() < 0.001 or v02.length() < 0.001
or v03.length() < 0.001):
return 0
distance_12 = pt_1.distance_to_point(pt_2)
distance_13 = pt_1.distance_to_point(pt_3)
distance_23 = pt_2.distance_to_point(pt_3)
triangle_123_perimeter = distance_12 + distance_13 + distance_23
matrix_elements = [
v01.x, v01.y, v01.z, v02.x, v02.y, v02.z, v03.x, v03.y, v03.z
]
p = triangle_123_perimeter / 2
triangle_123_area = (p * (p - distance_12) * (p - distance_13) *
(p - distance_23))**0.5
tetrahedron_volume = abs(
Matrix3(elements=matrix_elements).det() / 6)
tetrahedron_height = tetrahedron_volume * 3 / triangle_123_area
return tetrahedron_height
else:
print('error in __implementation_distance_to_plane:',
'incorrect args')
return None
def __implementation_distance_to_line(self,
line=None,
pt_1=None,
pt_2=None,
segment=None): # TODO
"""
pt0, pt1, pt2 form a triangle where
pt0 = self,
pt1 = line origin,
pt2 = line origin + parallel_vector
Firstly calculate triangle's square as 1/2 * v01 * v02 * sin(a102),
where a102 is angle between v01 and v02,
v01 is vector from pt0 to pt1,
v02 is vector from pt0 to pt2.
Then as 1/2 * h * v12.length()
where h is height from pt0 to the segment connecting pt1 and pt2
(the same as line.parallel_vector).
Equating them one can find h.
"""
pt0 = self
if line is not None:
pt1 = line.origin
pt2 = pt1.translated(line.parallel_vector)
elif not None in (pt_1, pt_2):
pt1 = pt_1
pt2 = pt_2
else:
print('error in point __util_distance_to_line:',
'incorrect arguments or not implemented yet')
return None
line_parallel_vector_length = pt1.distance_to_point(pt2)
v01 = Vector(pt_1=pt0, pt_2=pt1)
v02 = Vector(pt_1=pt0, pt_2=pt2)
cos_angle_v01_v02 = v01.product_with(v02) / v01.length() / v02.length()
sin_angle_v01_v02 = (1 - cos_angle_v01_v02**2)**0.5
double_area = v01.length() * v02.length() * sin_angle_v01_v02
if double_area == 0:
return 0
triangle_height = double_area / line_parallel_vector_length
return triangle_height
def normal(self):
"""
Return normal to the plane.
"""
return Vector(self.a, self.b, self.c)
def __implementation_distance_to_line(self,
line=None,
pt_1=None, pt_2=None):
"""
Self line AB and line including points C and D. EF is a vector
perpendicular to segment AB and line CD. E belongs to AB,
F - to CD. One can write:
(EF, AB) = 0
(EF, CD) = 0
E = A + alpha*AB
F = C + gamma*CD
And solve the system finding alpha and gamma.
"""
ptA = self.origin
ptB = self.origin.translated(self.parallel_vector)
if not line is None:
ptC = line.origin
ptD = line.origin.translated(line.parallel_vector)
elif not None in (pt_1, pt_2):
ptC = pt_1
ptD = pt_2
else:
print('error in line.__implementation_distance_to_line:',
'incorrect arguments')
return None
vecAB = Vector(pt_1=ptA, pt_2=ptB)
vecCD = Vector(pt_1=ptC, pt_2=ptD)
# check if these lines are parallel
param = abs(vecAB.product_with(vecCD) / vecAB.length() / vecCD.length())
if param - 1 < 0.001:
vecAD = Vector(pt_1=ptA, pt_2=ptD)
cos_angle_A = (vecAD.product_with(vecAB) /
vecAB.length() /
vecAD.length())
sin_angle_A = (1 - cos_angle_A**2)**0.5
return vecAD.length() * sin_angle_A
# lines are not parallel
c11 = -vecAB.length()**2
c12 = vecAB.product_with(vecCD)
c22 = vecCD.length()**2
vecBETA = Vector(x=ptC.x - ptA.x, # it is useful
y=ptC.y - ptA.y, # to introduce
z=ptC.z - ptA.z) # this vector
b1 = -vecBETA.product_with(vecAB)
b2 = -vecBETA.product_with(vecCD)
"""
System to solve is:
|c11 c12 | b1 |
|-c12 c22 | b2 |
"""
print('coeffs: ', c11, c12, c22, b1, b2)
det = c11*c22 + c12**2
if det == 0:
# line and line cross?
return 0
det_alpha = b1*c22 - b2*c12
det_gamma = c11*b2 + c12*b1
alpha = det_alpha / det
gamma = det_gamma / det
vecAB.multiply_by_number(alpha)
ptE = ptA.translated(vecAB)
vecCD.multiply_by_number(gamma)
ptF = ptC.translated(vecCD)
return Vector(pt_1=ptE, pt_2=ptF).length()
class Line:
"""
A 1d line in a 3d space.
Is always set in parametric form:
x = x0 + tau * dx
y = y0 + tau * dy
z = z0 + tau * dz
where (dx, dy, dz) is the vector parallel to the line
and (x0, y0, z0) is some chosen point on the line
(i call it "origin")
"""
def __init__(self,
pt_1=None, pt_2=None,
plane_1=None, plane_2=None): # TODO
if not None in (pt_1, pt_2):
self.init_2pts(pt_1, pt_2)
else:
print('error in line init:',
'incorrect args')
return None
def init_2pts(self, pt_1, pt_2):
self.parallel_vector = Vector(pt_2.x - pt_1.x,
pt_2.y - pt_1.y,
pt_2.z - pt_1.z)
self.origin = pt_1
def init_2planes(self, plane_1, plane_2):
pass
"""
Methods to calculate distances to other primitives.
"""
def distance_to_point(self, point):
return point.distance_to_line(self)
def distance_to_segment(self, segment):
return segment.distance_to_line(self)
def distance_to_line(self,
line=None,
pt_1=None, pt_2=None):
return self.__implementation_distance_to_line(line=line,
pt_1=pt_1,
pt_2=pt_2)
def __implementation_distance_to_line(self,
line=None,
pt_1=None, pt_2=None):
"""
Self line AB and line including points C and D. EF is a vector
perpendicular to segment AB and line CD. E belongs to AB,
F - to CD. One can write:
(EF, AB) = 0
(EF, CD) = 0
E = A + alpha*AB
F = C + gamma*CD
And solve the system finding alpha and gamma.
"""
ptA = self.origin
ptB = self.origin.translated(self.parallel_vector)
if not line is None:
ptC = line.origin
ptD = line.origin.translated(line.parallel_vector)
elif not None in (pt_1, pt_2):
ptC = pt_1
ptD = pt_2
else:
print('error in line.__implementation_distance_to_line:',
'incorrect arguments')
return None
vecAB = Vector(pt_1=ptA, pt_2=ptB)
vecCD = Vector(pt_1=ptC, pt_2=ptD)
# check if these lines are parallel
param = abs(vecAB.product_with(vecCD) / vecAB.length() / vecCD.length())
if param - 1 < 0.001:
vecAD = Vector(pt_1=ptA, pt_2=ptD)
cos_angle_A = (vecAD.product_with(vecAB) /
vecAB.length() /
vecAD.length())
sin_angle_A = (1 - cos_angle_A**2)**0.5
return vecAD.length() * sin_angle_A
# lines are not parallel
c11 = -vecAB.length()**2
c12 = vecAB.product_with(vecCD)
c22 = vecCD.length()**2
vecBETA = Vector(x=ptC.x - ptA.x, # it is useful
y=ptC.y - ptA.y, # to introduce
z=ptC.z - ptA.z) # this vector
b1 = -vecBETA.product_with(vecAB)
b2 = -vecBETA.product_with(vecCD)
"""
System to solve is:
|c11 c12 | b1 |
|-c12 c22 | b2 |
"""
print('coeffs: ', c11, c12, c22, b1, b2)
det = c11*c22 + c12**2
if det == 0:
# line and line cross?
return 0
det_alpha = b1*c22 - b2*c12
det_gamma = c11*b2 + c12*b1
alpha = det_alpha / det
gamma = det_gamma / det
vecAB.multiply_by_number(alpha)
ptE = ptA.translated(vecAB)
vecCD.multiply_by_number(gamma)
ptF = ptC.translated(vecCD)
return Vector(pt_1=ptE, pt_2=ptF).length()
def distance_to_plane(self,
plane=None,
pt_1=None, pt_2=None, pt_3=None):
return self.__implementation_distance_to_plane(plane=plane,
pt_1=pt_1, pt_2=pt_2, pt_3=pt_3)
def __implementation_distance_to_plane(self,
plane=None,
pt_1=None, pt_2=None, pt_3=None):
"""
Two different ways to check if they are parallel.
If not, it is easy to find the distance.
"""
if not plane is None:
plane_a = plane.a
plane_b = plane.b
plane_c = plane.c
normal = Vector(x=plane_a, y=plane_b, z=plane_c)
# TODO - remove hardcode
if abs(self.parallel_vector.product_with(normal)) > 0.001:
return 0
return self.origin.distance_to_plane(plane)
elif not None in (pt_1, pt_2, pt_3):
x = ((pt_2.y - pt_1.y) * (pt_3.z - pt_1.z) -
(pt_3.y - pt_1.y) * (pt_2.z - pt_1.z))
y = -((pt_2.x - pt_1.x) * (pt_3.z - pt_1.z) -
(pt_3.x - pt_1.x) * (pt_2.z - pt_1.z))
z = ((pt_2.x - pt_1.x) * (pt_3.y - pt_1.y) -
(pt_3.x - pt_1.x) * (pt_2.y - pt_1.y))
normal = Vector(x=x, y=y, z=z)
# TODO - remove hardcode
if self.parallel_vector.product_with(normal) > 0.001:
return 0
elements = [pt_1.x - self.origin.x,
pt_1.y - self.origin.y,
pt_1.z - self.origin.z,
pt_2.x - self.origin.x,
pt_2.y - self.origin.y,
pt_2.z - self.origin.z,
pt_3.x - self.origin.x,
pt_3.y - self.origin.y,
pt_3.z - self.origin.z]
tetrahedron_volume = Matrix3(elements=elements).det() / 6
vec12 = Vector(pt_1=pt_1, pt_2=pt_2)
vec13 = Vector(pt_1=pt_1, pt_2=pt_3)
cos_angle_1 = (vec12.product_with(vec13) /
vec12.length() /
vec13.length())
sin_angle_1 = (1 - cos_angle_1**2)**0.5
triangle_area = vec12.length() * vec13.length() * sin_angle_1 / 2
return 3 * tetrahedron_volume / triangle_area
else:
print('error in line.__implementation_distance_to_plane:',
'incorrect arguments')
return None
def distance_to_polygon(self,
polygon=None,
polygon_vertices=None):
return self.__implementation_distance_to_polygon(polygon,
polygon_vertices)
def __implementation_distance_to_polygon(self,
polygon=None,
polygon_vertices=None):
"""
If the line crosses polygon, return 0. Otherwise,
the distance of interest equals to the smallest from
the distances from polygon edges to line.
"""
if not polygon is None:
vertices = polygon.vertices
normal = polygon.containing_plane.normal()
elif not polygon_vertices is None:
vertices = polygon_vertices
dx1 = vertices[1].x - vertices[0].x
dx2 = vertices[2].x - vertices[0].x
dy1 = vertices[1].y - vertices[0].y
dy2 = vertices[2].y - vertices[0].y
dz1 = vertices[1].z - vertices[0].z
dz2 = vertices[2].z - vertices[0].z
normal = Vector(dy1 * dz2 - dz1 * dy2,
dx2 * dz1 - dz2 * dx1,
dx1 * dy2 - dx2 * dy1)
else:
print('error in line.__implementation_distance_to_polygon:',
'incorrect arguments')
return None
v1 = vertices[0]
v2 = vertices[1]
v3 = vertices[2]
distance_to_plane = self.distance_to_plane(pt_1=v1, pt_2=v2, pt_3=v3)
if distance_to_plane == 0:
# There is an intersection.
# Should find whether this point belongs to the polygon.
# origin + parallel_vector * alpha = vec01 * gamma + vec02 * beta
# if 0 < beta, gamma and gamma + beta <= 1
# intersection point is inside triangle (vertex_0,
# vertex_i,
# vertex_i+1)
# otherwise it
x = vertices[0].x
y = vertices[0].y
z = vertices[0].z
dx = self.parallel_vector.x
dy = self.parallel_vector.y
dz = self.parallel_vector.z
smallest_distance = self.distance_to_line(pt_1=vertices[0], # start
pt_2=vertices[1]) # value
for i in range(1, len(vertices) - 1):
vec01 = Vector(pt_1=vertices[0], pt_2=vertices[i])
vec02 = Vector(pt_1=vertices[0], pt_2=vertices[i + 1])
det = Matrix3(elements=[dx, -vec01.x, -vec02.x,
dy, -vec02.y, -vec02.y,
dz, -vec02.z, -vec02.z]).det()
det_alpha = Matrix3(elements=[-x, -vec01.x, -vec02.x,
-y, -vec01.y, -vec02.y,
-z, -vec01.z, -vec02.z]).det()
det_gamma = Matrix3(elements=[dx, -x, -vec02.x,
dy, -y, -vec02.y,
dz, -z, -vec02.z]).det()
det_beta = Matrix3(elements=[dx, -vec01.x, -x,
dy, -vec01.y, -y,
dz, -vec01.z, -z]).det()
# TODO - check this case!
if det != 0 and (det_gamma != 0 or det_beta != 0):
gamma = det_gamma / det
beta = det_beta / det
if gamma >= 0 and beta >= 0 and gamma + beta <= 1:
return 0
smallest_distance = min(smallest_distance,
self.distance_to_line(
pt_1=vertices[i],
pt_2=vertices[i + 1]))
else:
# parallel case?
smallest_distance = min(smallest_distance,
self.distance_to_point(vertices[i]))
return smallest_distance
else:
# No intersection.
# Result = (distance_to_plane**2 + in_plane_distance**2)**0.5
# in_plane_distance is distance between line and polygon
# translated to the line's plane
normal.renorm(distance_to_plane)
distances = []
# distance_in_plane = smallest from distance_to_vertex_in_plane
for i in range(len(vertices)):
if i == 0:
j = len(vertices) - 1
else:
j = i - 1
vim1 = vertices[j].translated(normal)
vim2 = vertices[j].translated(-normal)
vi1 = vertices[i].translated(normal)
vi2 = vertices[i].translated(-normal)
# re-implementation of distance to segment...
ptC = self.origin
ptD = self.origin.translated(self.parallel_vector)
vecCD = Vector(pt_1=ptC, pt_2=ptD)
for i, vecAB in enumerate((Vector(pt_1=vim1, pt_2=vi1),
Vector(pt_1=vim2, pt_2=vi2))):
if i == 0:
ptA = vim1
ptB = vi1
else:
ptA = vim2
ptB = vi2
param = abs(vecAB.product_with(vecCD) /
vecAB.length() /
vecCD.length())
if abs(param - 1) < 0.001:
vecAD = Vector(pt_1=ptA, pt_2=ptD)
cos_angle_A = (vecAD.product_with(vecAB) /
vecAB.length() /
vecAD.length())
sin_angle_A = (1 - cos_angle_A**2)**0.5
distances.append(vecAD.length() * sin_angle_A)
continue
# AB and CD are not parallel
c11 = -vecAB.length()**2
c12 = vecAB.product_with(vecCD)
c22 = vecCD.length()**2
vecBETA = Vector(x=self.origin.x - ptA.x, # it is useful
y=self.origin.y - ptA.y, # to introduce
z=self.origin.z - ptA.z) # this vector
b1 = -vecBETA.product_with(vecAB)
b2 = -vecBETA.product_with(vecCD)
"""
System to solve is:
|c11 c12 | b1 |
|-c12 c22 | b2 |
"""
det = c11*c22 + c12**2
if det == 0:
# segment and line cross?
distance_in_plane = 0
continue
det_alpha = b1*c22 - b2*c12
det_gamma = c11*b2 + c12*b1
alpha = det_alpha / det
gamma = det_gamma / det
if alpha <= 0:
ptE = ptA
elif alpha >=1:
ptE = ptB
else:
vecAB.multiply_by_number(alpha)
ptE = self.ptA.translated(vecAB)
# gamma may be equal to 0, so use this
# instead of Vector.multiply_by_number
len_tmp = ((ptE.x - self.origin.x - gamma * vecCD.x)**2 +
(ptE.y - self.origin.y - gamma * vecCD.y)**2 +
(ptE.z - self.origin.z - gamma * vecCD.z)**2)**0.5
distances.append(len_tmp)
distance_in_plane = min(distances)
return (distance_in_plane**2 + distance_to_plane**2)**0.5
def distance_to_prism(self,
prism=None,
top_facet=None, bot_facet=None):
return self.__implementation_distance_to_prism(prism, top_facet, bot_facet)
def __implementation_distance_to_prism(self,
prism,
top_facet, bot_facet):
if not prism is None:
top_vertices = prism.top_facet.vertices
bot_vertices = prism.bot_facet.vertices
elif not None in (top_facet, bot_facet):
top_vertices = top_facet.vertices
bot_vertices = bot_facet.vertices
else:
print('error in line.__implementation_distance_to_prism:',
'incorrect arguments!')
return None
distances = [self.distance_to_polygon(polygon_vertices=top_vertices),
self.distance_to_polygon(polygon_vertices=bot_vertices)]
for i in range(len(top_vertices)):
# TODO - check that top and bottom vertices correspond
# one to another
if i == 0:
j = len(top_vertices) - 1
else:
j = i - 1
tvi = top_vertices[i]
tvj = top_vertices[j]
bvi = bot_vertices[i]
bvj = bot_vertices[j]
distance = self.distance_to_polygon(polygon_vertices=[tvi, bvi,
bvj, tvj])
distances.append(distance)
return min(distances)
def init_2pts(self, pt_1, pt_2):
self.parallel_vector = Vector(pt_2.x - pt_1.x,
pt_2.y - pt_1.y,
pt_2.z - pt_1.z)
self.origin = pt_1
def __implementation_distance_to_polygon(self,
polygon=None,
polygon_vertices=None):
"""
If the line crosses polygon, return 0. Otherwise,
the distance of interest equals to the smallest from
the distances from polygon edges to line.
"""
if not polygon is None:
vertices = polygon.vertices
normal = polygon.containing_plane.normal()
elif not polygon_vertices is None:
vertices = polygon_vertices
dx1 = vertices[1].x - vertices[0].x
dx2 = vertices[2].x - vertices[0].x
dy1 = vertices[1].y - vertices[0].y
dy2 = vertices[2].y - vertices[0].y
dz1 = vertices[1].z - vertices[0].z
dz2 = vertices[2].z - vertices[0].z
normal = Vector(dy1 * dz2 - dz1 * dy2,
dx2 * dz1 - dz2 * dx1,
dx1 * dy2 - dx2 * dy1)
else:
print('error in line.__implementation_distance_to_polygon:',
'incorrect arguments')
return None
v1 = vertices[0]
v2 = vertices[1]
v3 = vertices[2]
distance_to_plane = self.distance_to_plane(pt_1=v1, pt_2=v2, pt_3=v3)
if distance_to_plane == 0:
# There is an intersection.
# Should find whether this point belongs to the polygon.
# origin + parallel_vector * alpha = vec01 * gamma + vec02 * beta
# if 0 < beta, gamma and gamma + beta <= 1
# intersection point is inside triangle (vertex_0,
# vertex_i,
# vertex_i+1)
# otherwise it
x = vertices[0].x
y = vertices[0].y
z = vertices[0].z
dx = self.parallel_vector.x
dy = self.parallel_vector.y
dz = self.parallel_vector.z
smallest_distance = self.distance_to_line(pt_1=vertices[0], # start
pt_2=vertices[1]) # value
for i in range(1, len(vertices) - 1):
vec01 = Vector(pt_1=vertices[0], pt_2=vertices[i])
vec02 = Vector(pt_1=vertices[0], pt_2=vertices[i + 1])
det = Matrix3(elements=[dx, -vec01.x, -vec02.x,
dy, -vec02.y, -vec02.y,
dz, -vec02.z, -vec02.z]).det()
det_alpha = Matrix3(elements=[-x, -vec01.x, -vec02.x,
-y, -vec01.y, -vec02.y,
-z, -vec01.z, -vec02.z]).det()
det_gamma = Matrix3(elements=[dx, -x, -vec02.x,
dy, -y, -vec02.y,
dz, -z, -vec02.z]).det()
det_beta = Matrix3(elements=[dx, -vec01.x, -x,
dy, -vec01.y, -y,
dz, -vec01.z, -z]).det()
# TODO - check this case!
if det != 0 and (det_gamma != 0 or det_beta != 0):
gamma = det_gamma / det
beta = det_beta / det
if gamma >= 0 and beta >= 0 and gamma + beta <= 1:
return 0
smallest_distance = min(smallest_distance,
self.distance_to_line(
pt_1=vertices[i],
pt_2=vertices[i + 1]))
else:
# parallel case?
smallest_distance = min(smallest_distance,
self.distance_to_point(vertices[i]))
return smallest_distance
else:
# No intersection.
# Result = (distance_to_plane**2 + in_plane_distance**2)**0.5
# in_plane_distance is distance between line and polygon
# translated to the line's plane
normal.renorm(distance_to_plane)
distances = []
# distance_in_plane = smallest from distance_to_vertex_in_plane
for i in range(len(vertices)):
if i == 0:
j = len(vertices) - 1
else:
j = i - 1
vim1 = vertices[j].translated(normal)
vim2 = vertices[j].translated(-normal)
vi1 = vertices[i].translated(normal)
vi2 = vertices[i].translated(-normal)
# re-implementation of distance to segment...
ptC = self.origin
ptD = self.origin.translated(self.parallel_vector)
vecCD = Vector(pt_1=ptC, pt_2=ptD)
for i, vecAB in enumerate((Vector(pt_1=vim1, pt_2=vi1),
Vector(pt_1=vim2, pt_2=vi2))):
if i == 0:
ptA = vim1
ptB = vi1
else:
ptA = vim2
ptB = vi2
param = abs(vecAB.product_with(vecCD) /
vecAB.length() /
vecCD.length())
if abs(param - 1) < 0.001:
vecAD = Vector(pt_1=ptA, pt_2=ptD)
cos_angle_A = (vecAD.product_with(vecAB) /
vecAB.length() /
vecAD.length())
sin_angle_A = (1 - cos_angle_A**2)**0.5
distances.append(vecAD.length() * sin_angle_A)
continue
# AB and CD are not parallel
c11 = -vecAB.length()**2
c12 = vecAB.product_with(vecCD)
c22 = vecCD.length()**2
vecBETA = Vector(x=self.origin.x - ptA.x, # it is useful
y=self.origin.y - ptA.y, # to introduce
z=self.origin.z - ptA.z) # this vector
b1 = -vecBETA.product_with(vecAB)
b2 = -vecBETA.product_with(vecCD)
"""
System to solve is:
|c11 c12 | b1 |
|-c12 c22 | b2 |
"""
det = c11*c22 + c12**2
if det == 0:
# segment and line cross?
distance_in_plane = 0
continue
det_alpha = b1*c22 - b2*c12
det_gamma = c11*b2 + c12*b1
alpha = det_alpha / det
gamma = det_gamma / det
if alpha <= 0:
ptE = ptA
elif alpha >=1:
ptE = ptB
else:
vecAB.multiply_by_number(alpha)
ptE = self.ptA.translated(vecAB)
# gamma may be equal to 0, so use this
# instead of Vector.multiply_by_number
len_tmp = ((ptE.x - self.origin.x - gamma * vecCD.x)**2 +
(ptE.y - self.origin.y - gamma * vecCD.y)**2 +
(ptE.z - self.origin.z - gamma * vecCD.z)**2)**0.5
distances.append(len_tmp)
distance_in_plane = min(distances)
return (distance_in_plane**2 + distance_to_plane**2)**0.5
def __implementation_distance_to_plane(self,
plane=None,
pt_1=None, pt_2=None, pt_3=None):
"""
Two different ways to check if they are parallel.
If not, it is easy to find the distance.
"""
if not plane is None:
plane_a = plane.a
plane_b = plane.b
plane_c = plane.c
normal = Vector(x=plane_a, y=plane_b, z=plane_c)
# TODO - remove hardcode
if abs(self.parallel_vector.product_with(normal)) > 0.001:
return 0
return self.origin.distance_to_plane(plane)
elif not None in (pt_1, pt_2, pt_3):
x = ((pt_2.y - pt_1.y) * (pt_3.z - pt_1.z) -
(pt_3.y - pt_1.y) * (pt_2.z - pt_1.z))
y = -((pt_2.x - pt_1.x) * (pt_3.z - pt_1.z) -
(pt_3.x - pt_1.x) * (pt_2.z - pt_1.z))
z = ((pt_2.x - pt_1.x) * (pt_3.y - pt_1.y) -
(pt_3.x - pt_1.x) * (pt_2.y - pt_1.y))
normal = Vector(x=x, y=y, z=z)
# TODO - remove hardcode
if self.parallel_vector.product_with(normal) > 0.001:
return 0
elements = [pt_1.x - self.origin.x,
pt_1.y - self.origin.y,
pt_1.z - self.origin.z,
pt_2.x - self.origin.x,
pt_2.y - self.origin.y,
pt_2.z - self.origin.z,
pt_3.x - self.origin.x,
pt_3.y - self.origin.y,
pt_3.z - self.origin.z]
tetrahedron_volume = Matrix3(elements=elements).det() / 6
vec12 = Vector(pt_1=pt_1, pt_2=pt_2)
vec13 = Vector(pt_1=pt_1, pt_2=pt_3)
cos_angle_1 = (vec12.product_with(vec13) /
vec12.length() /
vec13.length())
sin_angle_1 = (1 - cos_angle_1**2)**0.5
triangle_area = vec12.length() * vec13.length() * sin_angle_1 / 2
return 3 * tetrahedron_volume / triangle_area
else:
print('error in line.__implementation_distance_to_plane:',
'incorrect arguments')
return None
def __implementation_distance_to_polygon(self,
polygon=None,
vertices=None):
"""
Find the distance to the plane containing polygon.
If it equals zero then desired distance equals to the smallest of
the distances to the polygon's edges.
Otherwise, firstly, check whether the desired distance equals to the
distance to the plane containing poly. It happens if the prism
with top and bot equal to the poly translated to the vector vec,
which is perpendicular to poly and has length equal to the distance to
the plane, contains point.
Otherwise the distance to the polygon is found by Pythagoras theorem:
a = the smallest of the diatances to the segments forming prisms's
top and bottom
b = distance to the plane containing polygon
c = (a**2 + b**2)**0.5 - distance of interest
"""
if not polygon is None:
distance_to_plane = self.distance_to_plane(
plane=polygon.containing_plane)
polygon_center_x = polygon.center.x
polygon_center_y = polygon.center.y
polygon_center_z = polygon.center.z
vertex0 = polygon.vertices[0]
vertex1 = polygon.vertices[1]
# FIXME - why these coords? they seem to be incorrect!!!
vec = Vector(x=polygon_center_x - self.x,
y=polygon_center_y - self.y,
z=polygon_center_z - self.z)
vertices = polygon.vertices
top_facet_vertices = [
vertex.translated(vec) for vertex in vertices
]
bot_facet_vertices = [
vertex.translated(-vec) for vertex in vertices
]
elif not vertices is None and not len(vertices) < 3:
distance_to_plane = self.__implementation_distance_to_plane(
pt_1=vertices[0], pt_2=vertices[1], pt_3=vertices[2])
x = 0
y = 0
z = 0
for vertex in vertices:
x += vertex.x
y += vertex.y
z += vertex.z
N = len(vertices)
polygon_center_x = x / N
polygon_center_y = y / N
polygon_center_z = z / N
vertex0 = vertices[0]
vertex1 = vertices[1]
vec = Vector(x=polygon_center_x - self.x,
y=polygon_center_y - self.y,
z=polygon_center_z - self.z)
top_facet_vertices = [
vertex.translated(vec) for vertex in vertices
]
bot_facet_vertices = [
vertex.translated(-vec) for vertex in vertices
]
else:
print('error in point.__implementation_distance_to_polygon:',
'incorrect arguments')
print(polygon, vertices)
return 0
# FIXME - remove hardcoded constant
if distance_to_plane < 0.001:
impl_d_to_s = self.__implementation_distance_to_segment
distance = impl_d_to_s(pt_beg=vertices[0], pt_end=vertices[1])
for i, vertex in enumerate(vertices):
if i == 0:
j = 1
else:
j = i - 1
distance = min(distance,
impl_d_to_s(pt_beg=vertex, pt_end=vertices[j]))
return distance
if self.__implementation_is_inside_prism(
top_facet_vertices=top_facet_vertices,
bot_facet_vertices=bot_facet_vertices):
return distance_to_plane
distance_to_segment = self.__implementation_distance_to_segment(
pt_beg=vertices[0], pt_end=vertices[1]) # start value
for i, vertex in enumerate(vertices):
if i == 0:
j = 1
else:
j = i - 1
distance_to_segment = min(
distance_to_segment,
self.__implementation_distance_to_segment(pt_beg=vertex,
pt_end=vertices[j]))
return distance_to_segment
def __implementation_distance_to_line(self,
line=None,
pt_1=None, pt_2=None):
"""
Segment AB and line including points C and D. EF is a vector
perpendicular to segment AB and line CD. E belongs to AB,
F - to CD. One can write:
(EF, AB) = 0
(EF, CD) = 0
E = A + alpha*AB
F = C + gamma*CD
And solve the system finding alpha and gamma.
If 0 <= alpha <= 1, desired distance equals
to the length of EF. Otherwise it equals to the less
of the distances from segment's point to line.
"""
vecAB = Vector(segment=self)
if line is not None:
vecCD = Vector(pt_1=line.origin,
pt_2=line.origin.translated(line.parallel_vector))
ptD = pt_2=line.origin.translated(line.parallel_vector)
line_origin_x = line.origin.x
line_origin_y = line.origin.y
line_origin_z = line.origin.z
elif not None in (pt_1, pt_2):
vecCD = Vector(pt_1=pt_1, pt_2=pt_2)
line_origin_x = pt_1.x
line_origin_y = pt_1.y
line_origin_z = pt_1.z
ptD = pt_2
# check if line and segment are parallel
param = abs(vecAB.product_with(vecCD) / vecAB.length() / vecCD.length())
if abs(param - 1) < 0.001:
vecAD = Vector(pt_1=self.pt_1, pt_2=ptD)
cos_angle_A = (vecAD.product_with(vecAB) /
vecAB.length() /
vecAD.length())
sin_angle_A = (1 - cos_angle_A**2)**0.5
return vecAD.length() * sin_angle_A
# lines are not parallel
c11 = -vecAB.length()**2
c12 = vecAB.product_with(vecCD)
c22 = vecCD.length()**2
vecBETA = Vector(x=line_origin_x - self.pt_1.x, # it is useful
y=line_origin_y - self.pt_1.y, # to introduce
z=line_origin_z - self.pt_1.z) # this vector
b1 = -vecBETA.product_with(vecAB)
b2 = -vecBETA.product_with(vecCD)
"""
System to solve is:
|c11 c12 | b1 |
|-c12 c22 | b2 |
"""
det = c11*c22 + c12**2
if det == 0:
# segment and line cross?
return 0
det_alpha = b1*c22 - b2*c12
det_gamma = c11*b2 + c12*b1
alpha = det_alpha / det
gamma = det_gamma / det
if alpha <= 0:
ptE = self.pt_1
elif alpha >=1:
ptE = self.pt_2
else:
vecAB.multiply_by_number(alpha)
ptE = self.pt_1.translated(vecAB)
vecCD.multiply_by_number(gamma)
ptF = line.origin.translated(vecCD)
return Vector(pt_1=ptE, pt_2=ptF).length()
def __implementation_distance_to_polygon(self,
polygon=None,
polygon_vertices=None): # TODO
"""
1) Does segment cross plane where does poly lie?
2a) If does, does the intersection point belong to poly?
2aa) If yes - return 0.
2ab) If not - return the smallest from the distances to
the polygon's edges.
2b) If not - does the closest segment's end to the plane
lie in the prism?
2ba) If yes - return distance to the plane.
2bb) If not - return the less from the distances to
the polygon's edges.
"""
if polygon is not None:
polygon_vertices = polygon.vertices
plane_pt_1 = polygon.vertices[0]
plane_pt_2 = polygon.vertices[1]
plane_pt_3 = polygon.vertices[2]
normal_to_plane = Vector(x=polygon.containing_plane.a,
y=polygon.containing_plane.b,
z=polygon.containing_plane.c)
else:
plane_pt_1 = polygon_vertices[0]
plane_pt_2 = polygon_vertices[1]
plane_pt_3 = polygon_vertices[2]
x = ((plane_pt_2.y - plane_pt_1.y) * (plane_pt_3.z - plane_pt_1.z) -
(plane_pt_3.y - plane_pt_1.y) * (plane_pt_2.z - plane_pt_1.z))
y = (-(plane_pt_2.x - plane_pt_1.x) * (plane_pt_3.z - plane_pt_1.z) +
(plane_pt_3.x - plane_pt_1.x) * (plane_pt_2.z - plane_pt_1.z))
z = ((plane_pt_2.x - plane_pt_1.x) * (plane_pt_3.y - plane_pt_1.y) -
(plane_pt_3.x - plane_pt_1.x) * (plane_pt_2.y - plane_pt_1.y))
normal_to_plane = Vector(x=x, y=y, z=z)
distance_to_plane = self.__implementation_distance_to_plane(
pt_1=plane_pt_1,
pt_2=plane_pt_2,
pt_3=plane_pt_3)
if distance_to_plane <= 0.001:
dist1 = self.pt_1.distance_to_plane(pt_1=plane_pt_1,
pt_2=plane_pt_2,
pt_3=plane_pt_3)
dist2 = self.pt_2.distance_to_plane(pt_1=plane_pt_1,
pt_2=plane_pt_2,
pt_3=plane_pt_3)
if dist1 <= 0.001:
intersection_point = self.pt_1
elif dist2 <= 0.001:
intersection_point = self.pt_2
else:
v12 = Vector(segment=self)
v12.multiply_by_number(dist1 / (abs(dist1) + abs(dist2)))
intersection_point = self.pt_1.translated(v12)
if intersection_point.distance_to_polygon(
polygon_vertices=polygon_vertices) == 0:
return 0
distance = self.distance_to_segment(Segment(pt_1=polygon_vertices[0],
pt_2=polygon_vertices[1]))
for i, vertex in enumerate(polygon_vertices):
if i == 0:
j = 1
else:
j = i - 1
vertex1 = polygon_vertices[j]
distance1 = self.distance_to_segment(Segment(pt_1=vertex,
pt_2=vertex1))
distance = min(distance, distance1)
return distance
# distance_to_plane != 0
dist1 = self.pt_1.distance_to_plane(pt_1=plane_pt_1,
pt_2=plane_pt_2,
pt_3=plane_pt_3)
dist2 = self.pt_2.distance_to_plane(pt_1=plane_pt_1,
pt_2=plane_pt_2,
pt_3=plane_pt_3)
if dist1 <= dist2:
normal = normal_to_plane
normal.renorm(dist1)
# TODO
if not polygon is None:
top_facet_vertices = polygon.translated(normal).vertices
bot_facet_vertices = polygon.translated(-normal).vertices
elif not polygon_vertices is None:
top_facet_vertices = [vertex.translated(normal)
for vertex in polygon_vertices]
bot_facet_vertices = [vertex.translated(-normal)
for vertex in polygon_vertices]
if self.pt_1.is_inside_prism(
top_facet_vertices=top_facet_vertices,
bot_facet_vertices=bot_facet_vertices):
return dist1
else:
distance = self.distance_to_segment(
Segment(pt_1=polygon_vertices[0],
pt_2=polygon_vertices[1]))
for i, vertex in enumerate(polygon_vertices):
if i == 0:
j = 1
else:
j = i - 1
vertex1 = polygon_vertices[j]
distance1 = self.distance_to_segment(Segment(pt_1=vertex,
pt_2=vertex1))
distance = min(distance, distance1)
return distance
else:
normal = normal_to_plane
normal.renorm(dist2)
if self.pt_2.is_inside_prism(
top_facet_vertices=top_facet_vertices,
bot_facet_vertices=bot_facet_vertices):
return dist2
else:
distance = self.distance_to_segment(
Segment(pt_1=polygon_vertices[0],
pt_2=polygon_vertices[1]))
for i, vertex in enumerate(polygon_vertices):
if i == 0:
j = 1
else:
j = i - 1
vertex1 = polygon.vertices[j]
distance1 = self.distance_to_segment(Segment(pt_1=vertex,
pt_2=vertex1))
distance = min(distance, distance1)
return distance
def __implementation_distance_to_segment(self,
segment=None,
pt_1=None, pt_2=None):
"""
Almost the same as distance to line, but not only
should be 0 <= alpha <= 1, also 0 <= gamma <= 1
"""
if not segment is None:
pt1 = segment.pt_1
pt2 = segment.pt_2
elif not None in (pt_1, pt_2):
pt1 = pt_1
pt2 = pt_2
vecAB = Vector(segment=self)
vecCD = Vector(pt_1=pt1, pt_2=pt2)
# check if segments are parallel!
par = abs(vecAB.product_with(vecCD) / vecAB.length() / vecCD.length()) - 1
# TODO -remove hardcode
if abs(par) <= 0.001:
vecAD = Vector(pt_1=pt1, pt_2=self.pt_2)
cos_angleA = abs(vecAB.product_with(vecAD) /
vecAB.length() /
vecAD.length())
sin_angleA = (1 - cos_angleA**2)**0.5
return vecAD.length() * sin_angleA
c11 = -vecAB.length()**2
c12 = vecAB.product_with(vecCD)
c22 = vecCD.length()**2
vecBETA = Vector(x=pt1.x - self.pt_1.x, # it is useful
y=pt1.y - self.pt_1.y, # to introduce
z=pt1.z - self.pt_1.z) # this vector
b1 = -vecBETA.product_with(vecAB)
b2 = -vecBETA.product_with(vecCD)
"""
System to solve is:
| c11 c12 | b1 |
| -c12 c22 | b2 |
"""
det = c11*c22 + c12**2
if det == 0:
# segment and segment cross?
return 0
det_alpha = b1*c22 - b2*c12
det_gamma = c11*b2 + c12*b1
alpha = det_alpha / det
gamma = det_gamma / det
if alpha <= 0:
ptE = self.pt_1
elif alpha >= 1:
ptE = self.pt_2
else:
vecAB.multiply_by_number(alpha)
ptE = self.pt_1.translated(vecAB)
if gamma <= 0:
ptF = pt1
elif gamma >= 1:
ptF = pt2
else:
vecCD.multiply_by_number(gamma)
ptF = pt1.translated(vecCD)
return Vector(pt_1=ptE, pt_2=ptF).length()
def test_inits():
print('pt...')
pt = Point(0, 0, 0)
print('pt ok')
print('vec...')
vec = Vector(1, 1, 1)
print('vec ok')
print('m2...')
m21 = Matrix2(a11=0, a12=0, a21=0, a22=0)
print('1 ok')
m22 = Matrix2(elements=[0, 0, 0, 0])
print('2 ok')
print('m2 ok')
print('m3...')
m31 = Matrix3(a11=0,
a12=0,
a13=0,
a21=0,
a22=0,
a23=0,
a31=0,
a32=0,
a33=0)
print('1 ok')
m32 = Matrix3(elements=[0, 0, 0, 0, 0, 0, 0, 0, 0])
print('2 ok')
print('m3 ok')
print('m4...')
m41 = Matrix4(a11=0,
a12=0,
a13=0,
a14=0,
a21=0,
a22=0,
a23=0,
a24=0,
a31=0,
a32=0,
a33=0,
a34=0,
a41=0,
a42=0,
a43=0,
a44=0)
print('1 ok')
m42 = Matrix4(elements=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
print('2 ok')
print('m4 ok')
print('line...')
line1 = Line(pt_1=Point(0, 0, 0), pt_2=Point(1, 1, 1))
print('1 ok')
#line2 = Line(plane_1=None, plane_2=None)
print('line ok')
print('seg')
segment = Segment(beg=Point(0, 0, 0), end=Point(1, 1, 1))
print('seg ok')
print('plane')
plane1 = Plane(a=1, b=1, c=1, d=1)
print('1 ok')
plane2 = Plane(pt_1=Point(0, 0, 0),
pt_2=Point(0, 0, 1),
pt_3=Point(0, 1, 0))
print('2 ok')
plane3 = Plane(pt=Point(0, 0, 0), normal=Vector(1, 1, 1))
print('3 ok')
plane4 = Plane(pt=Point(0, 0, 0),
segment=Segment(beg=Point(0, 0, 1), end=Point(1, 1, 1)))
print('4 ok')
print('plane ok')
print('poly_reg')
polygon_reg1 = PolygonRegular(vertices=(Point(0, 0, 0), Point(1, 0, 0),
Point(0, 1, 0), Point(1, 1, 0)))
print('poly_reg ok')
print('prism_reg')
prism_reg1 = PrismRegular(
top_facet=polygon_reg1,
bot_facet=PolygonRegular(vertices=(Point(0, 0, 1), Point(1, 0, 1),
Point(0, 1, 1), Point(1, 1, 1))))
print('prism_reg ok')