## 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