CALCULATE THE LENGTH OR SHORTEST LINE BETWEEN TWO LINES IN 3D

3d_line_intersection.py

#!/usr/bin/env python3
# 
# CALCULATE THE LENGTH OR SHORTEST LINE BETWEEN TWO LINES IN 3D
#
# See "The shortest line between two lines in 3D" of 
# http://paulbourke.net/geometry/pointlineplane/ written by Paul Bourke

from optparse import OptionParser
import sys
import numpy as np

usage="usage: %prog [options] arg1 (arg1 is PDB file)"
parser = OptionParser(usage=usage)
parser.add_option("-v", "--verbose",
	action="store_true", dest="verbose", default=False,
	help="print status messages to stdout")
(options, args) = parser.parse_args()

if len(args)!=12:
	parser.error("incorrect number of arguments")

P1 = np.array([int(p) for p in args[0:3]],float)
P2 = np.array([int(p) for p in args[3:6]],float)
P3 = np.array([int(p) for p in args[6:9]],float)
P4 = np.array([int(p) for p in args[9:12]],float)


def d(a,b,c,d):
	return np.dot(a-b,c-d)

def main():
	print("P1",P1)
	print("P2",P2)
	print("P3",P3)
	print("P4",P4)
	d_1343 = d(P1,P3,P4,P3)
	d_4321 = d(P4,P3,P2,P1)
	d_1321 = d(P1,P3,P2,P1)
	d_4343 = d(P4,P3,P4,P3)
	d_2121 = d(P2,P1,P2,P1)
	if (d_2121*d_4343 - d_4321*d_4321) != 0:
		mu_a = (d_1343*d_4321 - d_1321*d_4343) / (d_2121*d_4343 - d_4321*d_4321)
	else:
		print("Two lines are parallel. mu_a and mu_b cannot be calculated.")
		sys.exit(1)

	if d_4343 != 0:
		mu_b = (d_1343 + mu_a * d_4321) / d_4343
	else:
		print("mu_b cannot be calculated because d_4343 = 0.")
		print("mu_a =", mu_a)
		sys.exit(1)

	print("mu_a = ", mu_a, "mu_b = ", mu_b)

	P_a = P1 + mu_a * (P2 - P1)
	P_b = P3 + mu_b * (P4 - P3)

	D = np.sqrt(np.dot(P_a - P_b, P_a - P_b))
	print("length of the shortest line:", D)

if __name__ == "__main__":
	main()