## L3D.py (module 'L3D') Version 1.07

## Copyright (c) 2006 Bruce Vaughan, BV Detailing & Design, Inc.

## NOT FOR SALE. The software is provided "as is" without any warranty.

################################################################################

from math import sqrt

from point import Point

from param import Warning

################################################################################

"""

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 (function) - 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

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

Version 1.06 (12/7/06)  - Add function chk_type to check for valid arguments

Version 1.07 (2/16/07)  - Add __repr__ and __str__ methods

Reference 'The Shortest Line Between Two Lines in 3D' - Paul Bourke

"""

def chk_type(p_list):

for p in p_list:

if not isinstance(p, type(Point(0,0,0))):

return False

return True

class LineLineIntersect3D(object):

def __init__(self, p1, p2, p3, p4):

self.p1 = p1

self.p2 = p2

self.p3 = p3

self.p4 = p4

"""                                                                                                                       <-->     <-->

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 the location where the two lines 'appear' to intersect when viewed along the line segment PaPb.

Pa lies on the line connecting p1p2.

Pb lies on the line connecting p3p4.

Equation for each line:

Pa = p1 + ma(p2-p1)

Pb = p3 + mb(p4-p3)

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)

if chk_type([p1, p2, p3, p4]):

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

else:

raise TypeError, "All arguments passed to LineLineIntersect3D must be <type 'point'>"

# 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

def __repr__(self):

return 'LineLineIntersect3D(Point(%s), Point(%s), Point(%s), Point(%s))' % (self.p1, self.p2, self.p3, self.p4)

def __str__(self):

s1 = 'Instance of Class %s - End points of Shortest Line Segment:' % (repr(self.__class__).split('.')[:-2])

s2 = '  Point on line 1 (self.Pmem1): Point(%s)' % (self.Pmem1)

s3 = '  Point on line 2 (self.Pmem2): Point(%s)' % (self.Pmem2)

s4 = '  Distance between points: %0.6f' % (self.inters_dist)

return '%s\n%s\n%s\n%s' % (s1, s2, s3, s4)

def version(self):

"L3D.LineLineIntersect3D Version 1.07"

# end class definition

##############################################################################################

## Determine information about the relationship between a line segment and a point in 3D space

class DistancePointLine3D(object):

def __init__(self, p1, p2, Pb):

"""                                                                                                                  <-->

Calculate the points in 3D space Pa and Pb that define the line segment which is the shortest route between line p1p2 and point Pb.

Point Pb is closest to line p1p2 at an intersecting perpendicular line PaPb.

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.

"""

if chk_type([p1, p2, Pb]):

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.p1 = p1

self.p2 = p2

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)

else:

raise TypeError, "All arguments passed to DistancePointLine3D must be <type 'point'>"

def __repr__(self):

return 'DistancePointLine3D(Point(%s), Point(%s), Point(%s))' % (self.p1, self.p2, self.Pa)

def __str__(self):

s1 = 'Instance of Class %s - Closest point on Line Segment:' % (repr(self.__class__).split('.')[:-2])

s2 = '  Point on line segment p1-p2 (self.Pa): Point(%s)' % (self.Pa)

s3 = '  Distance between closest point on line segment and point: %0.6f' % (self.dist)

return '%s\n%s\n%s' % (s1, s2, s3)

def version(self):

"L3D.DistancePointLine3D Version 1.07"

# 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

print repr(a)

print

print formatDict("\nObject Attributes:", a.__dict__)

print

print dir(a)

## end test_scriptLL() ########################################################

def test_scriptLP():

from member import Member, MemberLocate

from point import Point, PointLocate

from param import dim_print

from macrolib.PrintDict import formatDict

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

print repr(a)

print

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

print

print dir(a)

print formatDict("\nObject Attributes:", a.__dict__)

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

test_scriptLP()

finally:

del test_scriptLL

del test_scriptLP

del test_ret_WP

del LineLineIntersect3D

del DistancePointLine3D

""" Output from test functions

Statement - Member 1 and Member 2 are not parallel - True or False? True

Member 1 apparent intersection point =  470.25, 264.625, 11850.75

Member 2 apparent intersection point =  470.25, 264.625, 11845.75

Unit vector length of segment connecting apparent intersection points =  0, 0, 1

Absolute distance between apparent intersection points =  5.0

Member 1 Left End 'X' distance to apparent intersection point =  0.0

Member 1 Right End 'X' distance to apparent intersection point =  -91.375

Apparent intersection point position with respect to Member 1 =  LE

Point Pa lies on Member 1

Point Pb lies on Member 2

Object:

Instance of Class LineLineIntersect3D - End points of Shortest Line Segment:

Point on line 1 (self.Pmem1): Point(470.25, 264.625, 11850.75)

Point on line 2 (self.Pmem2): Point(470.25, 264.625, 11845.75)

Distance between points: 5.000000

LineLineIntersect3D(Point(470.25, 264.625, 11850.75), Point(470.25, 356, 11850.75), Point(358, 264.625, 11845.75), Point(630.25, 264.625, 11845.75))

Object Attributes:

Key = Pmem1          Value = 470.25, 264.625, 11850.75

Key = Pmem2          Value = 470.25, 264.625, 11845.75

Key = _cp12_         Value = 1.0

Key = cp12           Value = 0, 0, -1

Key = inters_dist    Value = 5.0

Key = left_dist      Value = 0.0

Key = on_segment1    Value = 1

Key = on_segment2    Value = 1

Key = p1             Value = 470.25, 264.625, 11850.75

Key = p2             Value = 470.25, 356, 11850.75

Key = p3             Value = 358, 264.625, 11845.75

Key = p4             Value = 630.25, 264.625, 11845.75

Key = position       Value = LE

Key = right_dist     Value = -91.375

Key = uv             Value = 0, 0, 1

Key = uv1            Value = 0, 1, 0

Key = uv2            Value = 1, 0, 0

['Pmem1', 'Pmem2', '__class__', '__delattr__', '__dict__', '__doc__', '__getattribute__', '__hash__', '__init__', '__module__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__str__', '__weakref__', '_cp12_', 'cp12', 'inters_dist', 'left_dist', 'not_parallel', 'on_segment1', 'on_segment2', 'p1', 'p2', 'p3', 'p4', 'position', 'right_dist', 'uv', 'uv1', 'uv2', 'version']

Object: Instance of Class DistancePointLine3D - Closest point on Line Segment:

Point on line segment p1-p2 (self.Pa): Point(468, 719, 11954.961413)

Distance between closest point on line segment and point: 94.087229

DistancePointLine3D(Point(468, 719, 11811.5), Point(468, 719, 12024), Point(468, 719, 11954.961413))

Closest point on Member 1 (Pa) from selected point = 39-0, 59-11, 996-2 15/16

Unit vector of segment connecting Pb and Pa =  -0.369371, 0.929282, 0

Scalar distance between points Pb and Pa =  94.08722851

Point Pa lies on Member 1

Left end 'X' distance to Pa =  143.4614125

Right end 'X' distance to Pa =  -69.0385875

Point Pa position =  Not Beyond RE

['Pa', 'Pb', 'Pmem1', '__class__', '__delattr__', '__dict__', '__doc__', '__getattribute__', '__hash__', '__init__', '__module__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__str__', '__weakref__', 'dist', 'left_dist', 'on_segment', 'p1', 'p2', 'position', 'right_dist', 'uv', 'version']

Object Attributes:

Key = Pa            Value = 468, 719, 11954.961413

Key = Pb            Value = 502.753075, 631.566423, 11954.961413

Key = Pmem1         Value = 468, 719, 11954.961413

Key = dist          Value = 94.08722851

Key = left_dist     Value = 143.4614125

Key = on_segment    Value = 1

Key = p1            Value = 468, 719, 11811.5

Key = p2            Value = 468, 719, 12024

Key = position      Value = Not Beyond RE

Key = right_dist    Value = -69.0385875

Key = uv            Value = -0.369371, 0.929282, 0

"""