## L3D.py (module 'L3D') Version 1.05
## Copyright (c) 2006 Bruce Vaughan, BV Detailing & Design, Inc.
## All rights reserved.
## NOT FOR SALE. The software is provided "as is" without any warranty.
################################################################################
from math import sqrt
from point import Point
################################################################################
"""
LineLineIntersect3D (class) - Determine information about the intersection of two line segments in 3D space
DistancePointLine3D (class) - Determine information about the relationship between a line segment and a point in 3D space
ret_WP (class) - Return the WP on member 1 and which end of member 2 coincides
Revision History:
Version 1.02 - Add attributes obj.Pa and obj.Pb to a DistancePointLine3D instance
Version 1.03 (10/30/06) - Rework calculation of self.position
Consolidate comments
add function ret_WP
Version 1.04 (11/16/06) - Rework LineLineIntersect3D - solve using like triangles
Version 1.05 (11/17/06) - Rework LineLineIntersect3D - solve for unknowns by substitution
Reference 'The Shortest Line Between Two Lines in 3D' - Paul Bourke
"""
class LineLineIntersect3D:
def __init__(self, p1, p2, p3, p4):
from param import Warning
""" <--> <-->
Calculate the points in 3D space Pa and Pb that define the line segment which is the shortest route between two lines p1p2 and p3p4.
Each point occurs at the apparent intersection of the 3D lines.
The apparent intersection is defined here as the location where the two lines 'appear' to intersect when viewed along the line segment PaPb.
Equation for each line:
Pa = p1 + ma(p2-p1)
Pb = p3 + mb(p4-p3)
Pa lies on the line connecting p1p2.
Pb lies on the line connecting p3p4.
The shortest line segment is perpendicular to both lines. Therefore:
(Pa-Pb).(p2-p1) = 0
(Pa-Pb).(p4-p3) = 0
Where:
'.' indicates the dot product
A = p1-p3
B = p2-p1
C = p4-p3
Substituting:
(A + ma(B) - mb(C)).B = 0 & (A + ma(B) - mb(C)).C = 0
-----------------------------------------------------------------
A.B + ma(B.B) - mb(C.B) = 0
A.B + ma(B.B) - (ma(C.B)-A.C)/C.C)(C.B) = 0
ma(B.B)(C.C) - ma(C.B)(C.B) = (A.C)(C.B)-(A.B)(C.C)
ma = ((A.C)(C.B)-(A.B)(C.C))/((B.B)(C.C) - (C.B)(C.B))
mb = (A.B + ma(B.B))/(C.B)
If the cross product magnitude of the two lines is equal to 0.0, the lines are parallel.
<-->
A line extends forever in both directions. The name of a line passing through two different points p1 and p2 would be "line p1p2" or p1p2.
The two-headed arrow over p1p2 signifies a line passing through points p1 and p2.
Two lines which have no actual intersection but are not parallel are called 'skew' or 'agonic' lines. Skew lines can only exist in
three or more dimensions.
Determine whether the apparent intersection point lies between the line segment end points or beyond one of the line segment end points.
This information is to be used to evaluate the framing condition of mem1 (p1p2).
Convention for members:
p1p2 - mem1.left.location, mem1.right.location
p3p4 - mem2.left.location, mem2.right.location
Set a keyword indicating the apparent intersection point position with respect to the line segment end points p1 and p2 as follows:
'LE' indicates the apparent intersection point occurs at p1 (within fudge_factor distance)
'RE' indicates the apparent intersection point occurs at p2 (within fudge_factor distance)
'Beyond LE' indicates the apparent intersection point occurs beyond p1
'Beyond RE' indicates the apparent intersection point occurs beyond p2
'Not Beyond LE' indicates the apparent intersection point occurs in between p1 and p2 and is closer to p1
'Not Beyond RE' indicates the apparent intersection point occurs in between p1 and p2 and is closer to p2
Calculate the magnitude and direction (beam member 'X' distance) the apparent intersection point occurs from line segment p1p2 end points.
"""
def cross_product(p1, p2):
return Point(p1.y*p2.z - p1.z*p2.y, p1.z*p2.x - p1.x*p2.z, p1.x*p2.y - p1.y*p2.x)
def dot_product(p1, p2):
return (p1.x*p2.x + p1.y*p2.y + p1.z*p2.z)
def mag(p):
return sqrt(p.x**2 + p.y**2 + p.z**2)
def normalise(p1, p2):
p = p2 - p1
m = mag(p)
if m == 0:
return Point(0.0, 0.0, 0.0)
else:
return Point(p.x/m, p.y/m, p.z/m)
def ptFactor(p, f):
return Point(p.x*f, p.y*f, p.z*f)
A = p1-p3
B = p2-p1
C = p4-p3
# Line p1p2 and p3p4 unit vectors
self.uv1 = normalise(p1, p2)
self.uv2 = normalise(p3, p4)
# Check for parallel lines
self.cp12 = cross_product(self.uv1, self.uv2)
self._cp12_ = mag(self.cp12)
if round(self._cp12_, 6) != 0.0:
ma = ((dot_product(A, C)*dot_product(C, B)) - (dot_product(A, B)*dot_product(C, C)))/ \
((dot_product(B, B)*dot_product(C, C)) - (dot_product(C, B)*dot_product(C, B)))
mb = (ma*dot_product(C, B) + dot_product(A, C))/ dot_product(C, C)
# Calculate the point on line 1 that is the closest point to line 2
Pa = p1 + ptFactor(B, ma)
self.Pmem1 = Pa
# Calculate the point on line 2 that is the closest point to line 1
Pb = p3 + ptFactor(C, mb)
self.Pmem2 = Pb
# Distance between lines
self.inters_dist = Pa.dist(Pb)
if round(ma, 3) >= 0.0 and round(ma, 3) <= 1.0:
self.on_segment1 = 1
xl_dir = 1
xr_dir = -1
if round(ma, 2) == 0.0:
self.position = "LE" # apparent intersection is at p1
elif round(ma, 2) == 1.0:
self.position = "RE" # apparent intersection is at p2
xr_dir = 1
xl_dir = 1
elif ma <= 0.5:
self.position = "Not Beyond LE" # apparent intersection is closer to p1
elif ma > 0.5:
self.position = "Not Beyond RE" # apparent intersection is closer to p2
else:
Warning('self.position calculation error, self.on_segment = 1')
raise ValueError
else:
self.on_segment1 = 0
if ma < 0.0:
self.position = "Beyond LE" # apparent intersection is beyond p1
xl_dir = -1
xr_dir = -1
elif ma > 0.0:
self.position = "Beyond RE" # apparent intersection is beyond p2
xl_dir = 1
xr_dir = 1
else:
Warning('self.position calculation error, self.on_segment = 0')
raise ValueError
# Set the member 'X' direction with respect to p1 and p2 - either '+' or '-'
self.left_dist = round(Pa.dist(p1)*xl_dir, 8)
self.right_dist = round(Pa.dist(p2)*xr_dir, 8)
if round(mb, 3) >= 0.0 and round(mb, 3) <= 1.0:
self.on_segment2 = 1
else:
self.on_segment2 = 0
# Calculate the unit vector of PaPb
if round(self.inters_dist, 4) > 0.0:
self.uv = normalise(Pb, Pa)
else:
self.uv = Point(0.0, 0.0, 0.0)
# Lines are parallel
else:
self.Pmem1 = None
self.Pmem2 = None
self.inters_dist = None
self.left_dist = None
self.right_dist = None
self.uv = None
# Return False if lines are parallel, and return True if lines are not parallel
def not_parallel(self):
if round(self._cp12_, 5) != 0.0:
return True
else:
return False
# end class definition
##############################################################################################
## Determine information about the relationship between a line segment and a point in 3D space
class DistancePointLine3D:
def __init__(self, p1, p2, Pb):
"""
Points p1 and p2 are the end points of member #1.
Point Pb is a point that lies on member #2.
Calculate the minimum distance in 3D space between point Pb and line p1p2.
Point Pb is closest to line p1p2 at an intersecting perpendicular line PaPb. Pa lies on line p1p2.
The dot product of two vectors, A and B, will equal the cosine of the angle between the vectors, times the length of each vector.
A dot B = A.x * B.x + A.y * B.y + A.z * B.z
If the vectors are unit vectors, the dot product is equal to the cosine of the angle between the vectors.
Since the angle between lines p1p2 and PaPb is 90 degrees, the dot product of vector p1p2 and vector PaPb is 0 (cos(90 deg)=0).
Determine location of point Pa and the scalar distance from point Pb.
If the calculation result for 'u' is between 0 and 1, Pa lies on line segment p1p2.
Determine whether point Pa lies between the line segment end points or beyond one of the line segment end points.
Set a keyword indicating point Pa position with respect to the line segment end points as follows:
'LE' indicates Pa occurs at p1 (within fudge_factor distance)
'RE' indicates Pa occurs at p2 (within fudge_factor distance)
'Beyond LE' indicates Pa occurs beyond p1
'Beyond RE' indicates Pa occurs beyond p2
'Not Beyond LE' indicates Pa occurs in between p1 and p2 and is closer to p1
'Not Beyond RE' indicates Pa occurs in between p1 and p2 and is closer to p2
Calculate the scalar distance and direction (beam member 'X' distance) Pa occurs from each line segment end point.
"""
Pa = Point(0.0, 0.0, 0.0)
u = (((Pb.x-p1.x)*(p2.x-p1.x))+((Pb.y-p1.y)*(p2.y-p1.y))+((Pb.z-p1.z)*(p2.z-p1.z)))/(((p2.x-p1.x)**2)+((p2.y-p1.y)**2)+((p2.z-p1.z)**2))
Pa.x = p1.x + u*(p2.x-p1.x)
Pa.y = p1.y + u*(p2.y-p1.y)
Pa.z = p1.z + u*(p2.z-p1.z)
self.Pmem1 = Pa
self.Pa = Pa
self.Pb = Pb
self.dist = round(Pa.dist(Pb),8)
if round(u, 3) >= 0.0 and round(u, 3) <= 1.0:
self.on_segment = 1
xl_dir = 1
xr_dir = -1
if round(u, 3) == 0.0:
self.position = "LE" # apparent intersection is at p1
elif round(u, 3) == 1.0:
self.position = "RE" # apparent intersection is at p2
xr_dir = 1
xl_dir = 1
elif u <= 0.5:
self.position = "Not Beyond LE" # apparent intersection is closer to p1
elif u > 0.5:
self.position = "Not Beyond RE" # apparent intersection is closer to p2
else:
Warning('self.position calculation error, self.on_segment = 1')
raise ValueError
else:
self.on_segment = 0
if u < 0.0:
self.position = "Beyond LE" # apparent intersection is beyond p1
xl_dir = -1
xr_dir = -1
elif u > 1.0:
self.position = "Beyond RE" # apparent intersection is beyond p2
xl_dir = 1
xr_dir = 1
else:
Warning('self.position calculation error, self.on_segment = 0')
raise ValueError
# Set the member 'X' direction with respect to p1 and p2 - either '+' or '-'
self.left_dist = round(Pa.dist(p1)*xl_dir, 8)
self.right_dist = round(Pa.dist(p2)*xr_dir, 8)
# Calculate the unit vector of PaPb
if self.dist > 0.0:
self.uv = Point((Pa.x-Pb.x)/self.dist, (Pa.y-Pb.y)/self.dist, (Pa.z-Pb.z)/self.dist)
else:
self.uv = Point(0.0, 0.0, 0.0)
# end class definition
###############################################################################
# Return the WP on member 1 and which end of member 2 is closer
def ret_WP(mem1, mem2):
a = LineLineIntersect3D(mem1.left.location, mem1.right.location, mem2.left.location, mem2.right.location)
b = LineLineIntersect3D(mem2.left.location, mem2.right.location, mem1.left.location, mem1.right.location)
# if intersection point is closer to left end, set which_end to 'left_end'
# if intersection point is closer to right end, set which_end to 'right_end'
if abs(b.left_dist) < abs(b.right_dist):
which_end = "left end"
else:
which_end = "right end"
return (a.Pmem1, which_end)
# end function definition
###############################################################################
def test_scriptLL():
from member import Member, MemberLocate
from param import dim_print
from point import Point, PointLocate
from macrolib.PrintDict import formatDict
mem1 = MemberLocate("Select a MEMBER 1")
mem2 = MemberLocate("Select a MEMBER 2")
a = LineLineIntersect3D(mem1.left.location, mem1.right.location, mem2.left.location, mem2.right.location)
print "Statement - Member 1 and Member 2 are not parallel - True or False? %s\n" % (a.not_parallel())
if a.not_parallel():
print "Member 1 apparent intersection point = ", a.Pmem1
print "Member 2 apparent intersection point = ", a.Pmem2
print "Unit vector length of segment connecting apparent intersection points = ", a.uv
print "Absolute distance between apparent intersection points = ", a.inters_dist
print "Member 1 Left End 'X' distance to apparent intersection point = ", a.left_dist
print "Member 1 Right End 'X' distance to apparent intersection point = ", a.right_dist
print "Apparent intersection point position with respect to Member 1 = ", a.position
if a.on_segment1 == 1:
print "Point Pa lies on Member 1"
else:
print "Point Pa does not lie on Member 1"
if a.on_segment2 == 1:
print "Point Pb lies on Member 2"
else:
print "Point Pb does not lie on Member 2"
else:
Warning("The members are parallel and do not intersect")
print "\nObject:"
print a
print formatDict("\nObject Attributes:", a.__dict__)
## end test_scriptLL() ########################################################
def test_scriptLP():
from member import Member, MemberLocate
from point import Point, PointLocate
from param import dim_print
mem1 = MemberLocate("Select a MEMBER 1")
pt1 = PointLocate("Pick point")
a = DistancePointLine3D(mem1.left.location, mem1.right.location, pt1)
print "Object: %s/n" % (a)
print "Closest point on Member 1 (Pa) from selected point = %s, %s, %s" % (dim_print(a.Pmem1.x), dim_print(a.Pmem1.y), dim_print(a.Pmem1.z))
print "Unit vector of segment connecting Pb and Pa = ", a.uv
print "Scalar distance between points Pb and Pa = ", a.dist
if a.on_segment == 1:
print "Point Pa lies on Member 1"
else:
print "Point Pa does not lie on Member 1"
print "Left end 'X' distance to Pa = ", a.left_dist
print "Right end 'X' distance to Pa = ", a.right_dist
print "Point Pa position = ", a.position, "\n"
listCopy = list(a.__dict__.keys())
listCopy.sort()
for i in listCopy:
print i, "=", a.__dict__[i]
## end test_scriptLP() ########################################################
def test_ret_WP():
from member import Member, MemberLocate
from param import dim_print
from point import Point, PointLocate
mem1 = MemberLocate("Select a MEMBER 1")
mem2 = MemberLocate("Select a MEMBER 2")
a, b = ret_WP(mem1, mem2)
print('Work point at member 1 = %s, %s, %s at the %s of member 2' % (dim_print(a.x), dim_print(a.y), dim_print(a.z), b))
## end test_ret_WP ############################################################
if __name__ == '__main__':
try:
test_scriptLL()
finally:
del test_scriptLL
del test_scriptLP
del test_ret_WP
del LineLineIntersect3D
del DistancePointLine3D