def __distance_point_polygon(self, point, polygon):
"""
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
"""
ac = AffinityChecker()
distance_to_plane = self.__distance_point_plane(point,
polygon.containing_plane)
if distance_to_plane < self.epsilon:
return self.__distance_point_polygon_edge(point, polygon)
vec_to_plane = Vector(polygon.containing_plane.a,
polygon.containing_plane.b,
polygon.containing_plane.c)
vec_to_plane.renorm(distance_to_plane)
if not ac.check(point, PrismRegular(polygon.translated(vec_to_plane),
polygon.translated(-vec_to_plane))):
return self.__distance_point_polygon_edge(point, polygon)
return distance_to_plane
def __distance_point_line(self, point, line):
"""
pt0, pt1, pt2 form a triangle where
pt0 = point,
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 = point
pt1 = line.point1
pt2 = line.point2
line_parallel_vector_length = self.__distance_point_point(pt1, pt2)
v01 = Vector(pt0, pt1)
v02 = Vector(pt0, 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 test_inits():
# vec inits: from 3 floats/ints +
vec = Vector(1, 1.0, 1)
# point inits: from pt +
# from vec +
# from 3 floats/ints +
pt1 = Point(0, 0, 0)
pt2 = Point(1, 0, 0)
pt3 = Point(0, 1, 0)
pt4 = Point(1, 1, 0)
pt5 = Point(0, 0, 1)
pt6 = Point(1, 0, 1)
pt7 = Point(0, 1, 1)
pt8 = Point(1, 1, 1)
pt9 = Point(vec)
pt10 = Point(1, 1.0, 1.0)
# line inits: from 2 pts +
# from seg +
# from pt, vec +
line = Line(pt1, pt2)
segment1 = Segment(pt1, pt2)
line3 = Line(segment1)
line1 = Line(pt1, vec)
# segment inits: from 2 pts +
# from vec +
# from 3 floats/ints +
segment = Segment(pt1, pt2)
segment3 = Segment(vec)
segment4 = Segment(1.0, 1, 1.0)
# plane inits: from 3 pts +
# from pt, vec +
# from 4 floats/ints +
# from 2 vecs +
# from 2 segs +
# from 2 lines -
# from vec / seg / line and vec / seg / line seem to be +
plane = Plane(pt1, pt2, pt3)
plane1 = Plane(1, 1.0, 1, 1)
plane2 = Plane(pt1, vec)
vec1 = Vector(2, 2, 0)
plane3 = Plane(vec, vec1)
plane4 = Plane(segment, segment4)
plane5 = Plane(line1, line3)
plane6 = Plane(vec1, line1)
plane7 = Plane(vec1, segment3)
# PolygonRegular inits: from vertices +
polygon = PolygonRegular([pt1, pt2, pt3, pt4])
polygon1 = PolygonRegular([pt5, pt6, pt7, pt8])
# PrismRegular intis
prism = PrismRegular(polygon, polygon1)
def __distance_line_plane(self, line, plane):
normal_to_plane = Vector(plane.a, plane.b, plane.c)
# Check if they are parallel;
# otherwise they surely cross.
if line.parallel_vector.product_with(normal_to_plane) > self.epsilon:
return 0
# Distances from all line points to plane are equal.
return self.__distance_point_plane(line.point1, plane)
def triangle_area(self, point1, point2, point3):
vec12 = Vector(point1, point2)
vec13 = Vector(point1, point3)
length12 = vec12.length()
length13 = vec13.length()
if 0 in (length12, length13):
return 0
cos_angle213 = vec12.product_with(vec13) / length12 / length13
sin_angle213 = (1 - cos_angle213**2)**0.5
area = 0.5 * sin_angle213 * length12 * length13
return area
def __distance_point_segment(self, point, segment):
"""
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 = point
pt1 = segment.begin
pt2 = segment.end
v01 = Vector(pt0, pt1)
v02 = Vector(pt0, pt2)
v12 = Vector(pt1, pt2)
if v01.product_with(v12) >= 0:
return v01.length()
elif v02.product_with(v12) <= 0:
return v02.length()
height = self.__distance_point_line(point, Line(pt1, pt2))
return height
def plane_from_brackets(self, brackets):
"""
Makes a plane from a line called 'brackets':
brackets == (pt.x, pt.y, pt.z; normal.x, normal.y, normal.z)
Returns a Plane that may be used in mypymath.
"""
pt_coords = brackets.split('(')[1].split(';')[0].split(', ')
ptx = float(pt_coords[0])
pty = float(pt_coords[1])
ptz = float(pt_coords[2])
pt = Point(ptx, pty, ptz)
normal_coords = brackets.split('(')[1].split(';')[1].split(')')[0]
normal_coords = normal_coords.split(', ')
nx = float(normal_coords[0])
ny = float(normal_coords[1])
nz = float(normal_coords[2])
normal = Vector(nx, ny, nz)
return Plane(pt, normal)
def __distance_segment_polygon(self, segment, polygon):
distance = self.__distance_segment_segment
edge = Segment(polygon.vertices[0], polygon.vertices[-1])
min_edge_distance = distance(segment, edge)
for i in range(1, len(polygon.vertices)):
edge = Segment(polygon.vertices[i], polygon.vertices[i - 1])
min_edge_distance = min(min_edge_distance,
distance(segment, edge))
pl = polygon.containing_plane
if self.__distance_segment_plane(segment, pl) != 0:
return min_edge_distance
#ptCross = begin + alpha*Vector(segment)
pt = segment.begin
vec = Vector(segment)
alpha = ((-pl.a*pt.x - pl.b*pt.y - pl.c*pt.z - pl.d) /
(vec.x + vec.y + vec.z))
ptCross = Point(pt.x + alpha*vec.x,
pt.y + alpha*vec.y,
pt.z + alpha*vec.z)
if self.__distance_point_polygon(ptCross, polygon) == 0:
return 0
return min_edge_distance
def __distance_segment_segment(self, segment1, segment2):
ptA = segment1.begin
ptB = segment1.end
ptC = segment2.begin
ptD = segment2.end
vecAB = Vector(ptA, ptB)
vecCD = Vector(ptC, ptD)
vecAC = Vector(ptA, ptC)
vecBD = Vector(ptB, ptD)
vecBC = Vector(ptB, ptC)
vecAD = Vector(ptA, ptD)
if (vecAC.length() == 0 or
vecAD.length() == 0 or
vecBD.length() == 0 or
vecBD.length() == 0):
return 0
cos_AB_CD = vecAB.product_with(vecCD) / vecAB.length() / vecCD.length()
if abs(abs(cos_AB_CD) - 1) < self.epsilon:
# parallel or at one line
cos_BAC = vecAB.product_with(vecAC) / vecAB.length() / vecAC.length()
cos_ABD = vecAB.product_with(vecBD) / vecAB.length() / vecBD.length()
if (abs(abs(cos_BAC) - 1) < self.epsilon and
abs(abs(cos_ABD) - 1) < self.epsilon):
# lie on one line
ac = AffinityChecker()
if (ac.check(ptA, segment2) or ac.check(ptB, segment2) or
ac.check(ptC, segment1) or ac.check(ptD, segment1)):
# vectors cross
return 0
return min(vecAD.length(),
vecAC.length(),
vecBD.length(),
vecBC.length())
if abs(abs(cos_BAC) - 1) < self.epsilon:
# ptC belongs to line AB, ptD does not
return min(vecBC.length(), vecAC.length())
if abs(abs(cos_ABD) - 1) < self.epsilon:
# ptD belongs to line AB, ptC does not
return min(vecAD.length(), vecBD.length())
distance = self.__distance_point_segment
return min(distance(ptA, segment2),
distance(ptB, segment2),
distance(ptC, segment1),
distance(ptD, segment1))
# lines are not parallel
c11 = -vecAB.length()**2
c12 = vecAB.product_with(vecCD)
c22 = vecCD.length()**2
# tmp vector for easy calculation procedure
b1 = -vecAC.product_with(vecAB)
b2 = -vecAC.product_with(vecCD)
"""
System to solve is:
|c11 c12 | b1 |
|-c12 c22 | b2 |
"""
det = c11*c22 + c12**2
if det == 0:
# Line AB and line CD cross (?)
# ptCross = ptA + fi * vecAB == ptC + xi * vecCD
d = Matrix2([-vecAD.x, vecCD.x,
-vecAB.y, vecCD.y]).det()
dfi = Matrix2([ptC.x - ptA.x, vecCD.x,
ptC.y - ptA.y, vecCD.y]).det()
dxi = Matrix2([-vecAB.x, ptC.x - ptA.x,
-vecAB.y, ptC.y - ptA.y]).det()
if d == 0:
# cross of segments (?)
return 0
else:
fi = dfi / d
xi = dxi / d
if fi < 0 :
ptE = ptA
elif fi > 1:
ptE = ptB
else:
vecAB.multiply_by_number(fi)
ptE = ptA.translated(vecAB)
if xi < 0:
ptF = ptC
elif xi > 1:
ptF = ptD
else:
vecCD.multiply_by_number(xi)
ptF = ptC.translated(vecCD)
return Vector(ptE, ptF).length()
else:
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)
if 0 <= alpha <= 1:
ptE = ptA.translated(vecAB)
elif alpha < 0:
ptE = ptA
else:
ptE = ptB
vecCD.multiply_by_number(gamma)
if 0 <= gamma <= 1:
ptF = ptC.translated(vecCD)
elif gamma < 0:
ptF = ptC
else:
ptF = ptD
return Vector(ptE, ptF).length()
def __distance_line_segment(self, line, segment):
ptA = line.point1
ptB = line.point2
ptC = segment.begin
ptD = segment.end
vecAB = Vector(ptA, ptB)
vecCD = Vector(ptC, ptD)
# check if these lines are parallel
param = abs(vecAB.product_with(vecCD) / vecAB.length() / vecCD.length())
if param - 1 < self.epsilon:
vecAD = Vector(ptA, ptD)
if vecAD.length() == 0:
return 0
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
# tmp vector for easy calculation procedure
vecBETA = Vector(ptC.x - ptA.x, ptC.y - ptA.y, ptC.z - ptA.z)
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:
# Line AB and line CD 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)
if 0 <= gamma <= 1:
ptF = ptC.translated(vecCD)
elif gamma < 0:
ptF = ptC
else:
ptF = ptD
return Vector(ptE, ptF).length()
def __checker_point_2prism(self, point, prism):
"""
Check whether volume of prism equlas to the sum of volumes
of pyramides composed by point and prism's facets.
"""
prism_volume = prism.height * prism.top_facet.area
pyramides_volume = 0
# top and bottom facets
vec1 = Vector(point, prism.top_facet.vertices[0])
vec2 = Vector(point, prism.top_facet.vertices[1])
vec3 = Vector(point, prism.top_facet.vertices[2])
vec4 = Vector(point, prism.top_facet.vertices[3])
elements = [
vec1.x, vec1.y, vec1.z, vec2.x, vec2.y, vec2.z, vec3.x, vec3.y,
vec3.z
]
pyramides_volume += abs(1 / 6 * Matrix3(elements).det())
elements = [
vec1.x, vec1.y, vec1.z, vec3.x, vec3.y, vec3.z, vec4.x, vec4.y,
vec4.z
]
pyramides_volume += abs(1 / 6 * Matrix3(elements).det())
vec1 = Vector(point, prism.bottom_facet.vertices[0])
vec2 = Vector(point, prism.bottom_facet.vertices[1])
vec3 = Vector(point, prism.bottom_facet.vertices[2])
vec4 = Vector(point, prism.bottom_facet.vertices[3])
elements = [
vec1.x, vec1.y, vec1.z, vec2.x, vec2.y, vec2.z, vec3.x, vec3.y,
vec3.z
]
pyramides_volume += abs(1 / 6 * Matrix3(elements).det())
elements = [
vec1.x, vec1.y, vec1.z, vec3.x, vec3.y, vec3.z, vec4.x, vec4.y,
vec4.z
]
pyramides_volume += abs(1 / 6 * Matrix3(elements).det())
# side facets
for i in range(len(prism.top_facet.vertices)):
vec1 = Vector(point, prism.top_facet.vertices[i])
vec2 = Vector(point, prism.top_facet.vertices[i - 1])
vec3 = Vector(point, prism.bottom_facet.vertices[i - 1])
vec4 = Vector(point, prism.bottom_facet.vertices[i])
elements = [
vec1.x, vec1.y, vec1.z, vec2.x, vec2.y, vec2.z, vec3.x, vec3.y,
vec3.z
]
pyramides_volume += abs(1 / 6 * Matrix3(elements).det())
elements = [
vec1.x, vec1.y, vec1.z, vec3.x, vec3.y, vec3.z, vec4.x, vec4.y,
vec4.z
]
pyramides_volume += abs(1 / 6 * Matrix3(elements).det())
if abs(prism_volume - pyramides_volume) < self.epsilon:
return True
return False
def test_distances(debug_flag=True):
dc = DistanceCalculator()
pt = Point(0, 0, 0)
pt1 = Point(1, 1, 1)
pt2 = Point(1, 2, 1)
pt3 = Point(2, 2, 1)
pt4 = Point(2, 1, 1)
pt5 = Point(1, 1, 2)
pt6 = Point(1, 2, 2)
pt7 = Point(2, 2, 2)
pt8 = Point(2, 1, 2)
segment = Segment(pt1, pt2)
line = Line(pt1, pt2)
line1 = Line(pt5, pt6)
line2 = Line(Point(0, 0, -1), Point(1, 1, -1))
plane = Plane(pt1, pt2, pt3)
plane1 = Plane(pt5, pt6, pt7)
plane2 = Plane(Point(0, 0, -1), Point(0, 1, -1), Point(1, 0, -1))
polygon = PolygonRegular([pt1, pt2, pt3, pt4])
polygon1 = PolygonRegular(
[Point(1, 1, -1),
Point(1, 2, -1),
Point(2, 2, -1),
Point(2, 1, -1)])
bottom_facet = PolygonRegular([pt5, pt6, pt7, pt8])
prism = PrismRegular(polygon, bottom_facet)
prism1 = PrismRegular(polygon.translated(Vector(0, 0, 10)),
polygon.translated(Vector(0, 0, 11)))
segment1 = Segment(pt1, pt3)
segment2 = Segment(pt2, pt4)
segment3 = Segment(pt5, pt7)
segment4 = Segment(Point(1, 1, -1), Point(1, 2, -1))
if debug_flag:
print()
# pt-ANY
print('pt-pt: ', dc.distance(pt, pt1)) # 1.0
print('pt-line: ', dc.distance(pt, line)) # sqrt(2)
print('pt-seg: ', dc.distance(pt, segment)) # sqrt(3)
print('pt-plane: ', dc.distance(pt, plane)) # 1.0
print('pt-poly: ', dc.distance(pt, polygon)) # sqrt(3)
print('pt-prism: ', dc.distance(pt, prism)) # sqrt(3)
# line-ANY
print('line-pt: ', dc.distance(line, pt)) # 1.0
print('line-line, 0: ', dc.distance(line, line)) # 0.0
print('line-line, 1: ', dc.distance(line, line1)) # 1.0
print('line-seg: ', dc.distance(line1, segment)) # 1.0
print('line-plane: ', dc.distance(line1, plane)) # 1.0
print('line-poly: ', dc.distance(line1, polygon)) # 1.0
print('line-prism, 0: ', dc.distance(line, prism)) # 0.0
print('line-prism, 0: ', dc.distance(line2, prism)) # 2.0
# segment-ANY
print('segment-pt: ', dc.distance(segment, pt)) # sqrt(3)
print('segment-line: ', dc.distance(segment, line)) # 0.0
print('segment-line: ', dc.distance(segment, line1)) # 1.0
print('segment-seg: ', dc.distance(segment1, segment2)) # 0.0
print('segment-seg: ', dc.distance(segment, segment3)) # 1.0
print('segment-plane: ', dc.distance(segment3, plane)) # 1.0
print('segment-poly: ', dc.distance(segment3, polygon)) # 1.0
print('segment-prism: ', dc.distance(segment4, prism)) # 2.0
# plane-ANY
print('plane-pt: ', dc.distance(plane, pt)) # 1.0
print('plane-line ', dc.distance(plane, line1)) # 1.0
print('plane-seg ', dc.distance(plane, segment4)) # 2.0
print('plane-plane ', dc.distance(plane, plane)) # 0.0
print('plane-plane ', dc.distance(plane, plane1)) # 1.0
print('plane-poly ', dc.distance(plane1, polygon)) # 1.0
print('plane-prism ', dc.distance(plane2, prism)) # 2.0
# polygon-ANY
print('polygon-pt: ', dc.distance(polygon, pt)) # sqrt(3)
print('polygon-line: ', dc.distance(polygon, line1)) # 1.0
print('polygon-seg: ', dc.distance(polygon, segment4)) # 2.0
print('polygon-plane: ', dc.distance(polygon, plane2)) # 2.0
print('polygon-polygon: ', dc.distance(polygon, polygon)) # 0.0
print('polygon-polygon: ', dc.distance(polygon, bottom_facet)) # 1.0
print('polygon-prism: ', dc.distance(polygon1, prism)) # 2.0
# prism-ANY
print('prism-pt ', dc.distance(prism, pt)) # sqrt(3)
print('prism-line ', dc.distance(prism, line2)) # 2.0
print('prism-seg ', dc.distance(prism, segment4)) # 2.0
print('prism-plane ', dc.distance(prism, plane2)) # 2.0
print('prism-polygon ', dc.distance(prism, polygon1)) # 2.0
print('prism-prism, 0: ', dc.distance(prism, prism)) # 0.0
print('prism-prism, 1: ', dc.distance(prism, prism1)) # 9.0