def solvePointToLine(xe,ye,ze,x1,y1,z1,x2,y2,z2,r,debugPrint=False):
	a = (x1*x1-2*x1*x2+x2*x2) + (y1*y1-2*y1*y2+y2*y2) + (z1*z1-2*z1*z2+z2*z2)
	b = 2.0*(x1*x2-x1*xe-x2*x2+x2*xe + y1*y2-y1*ye-y2*y2+y2*ye + z1*z2-z1*ze-z2*z2+z2*ze) 
	c = x2*x2-2*x2*xe+xe*xe + y2*y2-2*y2*ye+ye*ye + z2*z2-2*z2*ze+ze*ze - r*r
	if debugPrint:
		print 'x1:%f,x2:%f,y1:%f,y2:%f,z1:%f,z2:%f,xe:%f,ye:%f,ze:%f,r:%f,a:%f,b:%f,c:%f'%(x1,x2,y1,y2,z1,z1,xe,ye,ze,r,a,b,c)
	
	#It's possible for A to become nearly zero but not quite, giving totally bad solutions as things are divided by a very small number
	if jgh.isEssentiallyZero(a) or jgh.tolerantSqrt(b*b - 4.0*a*c,handleExceptions=True) == 'NULL':
		if debugPrint:
			print "Endpts: Unable to make ptA or ptB"
		return ['NULL','NULL']
	na = (-b + jgh.tolerantSqrt(b*b - 4.0*a*c))/(2.0*a)	
	nb = (-b - jgh.tolerantSqrt(b*b - 4.0*a*c))/(2.0*a)
	#STEP 2:	
	ptA = location(na*(x1-x2)+x2,na*(y1-y2)+y2,na*(z1-z2)+z2)
	ptB = location(nb*(x1-x2)+x2,nb*(y1-y2)+y2,nb*(z1-z2)+z2)
	return ptA,ptB
def computeLocationAs3D(j,debugPrint=False):
	#debugPrint = True
	#print "NAME IS: %s"%j.name
	#STEP 0: Copy initial points so they never drift with repeated transformations
	#STEP 1: Translate to make point 1 the origin
	#STEP 2: Rotate around Y axis till point 2 lies in Z=0 plane
	#STEP 3: Rotate around Z axis till point 2 lies in Y=0 plane (now must be on the X-axis)
	#STEP 4: Solve for point 4(new point)'s to find the X and Y. The X here will end up being the real X in this space. The Y is simply the radius.
	#STEP 5: The problem now starts over, remaking a new point A and B
	#STEP 6: Translate to make point newA's X=0
	#STEP 7: Rotate around Y axis 90 degrees to make the solution circle lie at Z=0
	#STEP 8: Rotate around Z axis till point newB liest on the Y=0 plane (it now must be on the X-axis)
	#STEP 9: Solve for point 4(new point)'s to find the new X and Y locations. The transformations we've already done will fill out all the other data found along the way.
	#STEP 10: Build the solution
	#STEP 11: Back out all transformations

	#STEP 0:	
	locA,locB,locC = copyAllJoints(j)	
	#STEP 1: 
	step1Inverse = jgh.moveToOrigin(locA,[locA,locB,locC])
	if(debugPrint):
		print "step 1"	
		print locA.toString()
		print locB.toString()
		print locC.toString()
	#STEP 2:
	step2Inverse = jgh.rotateYToXYPlane(locB,[locA,locB,locC])	 
	if(debugPrint):
		print "step 2"	
		print locA.toString()
		print locB.toString()
		print locC.toString()

	#STEP 3:
	step3Inverse = jgh.rotateZToXZPlane(locB,[locA,locB,locC])
	if(debugPrint):
		print "step 3"	
		print locA.toString()
		print locB.toString()
		print locC.toString()

	#STEP 4:
	x,y = jgh.solveForXY(locA,j.spanA.l,locB,j.spanB.l)	
	if(debugPrint):
		print "step 4"
		print "x:%f, y:%f"%(x,y)
	#STEP 5: REMAKE PROBLEM:
	#  remake A
	newSpanA = math.fabs(y)
	newLocA = location(x,0.0,0.0)
	#  remake B: it's projection onto the X=whatever plane
	newSpanB = jgh.tolerantSqrt(math.pow(j.spanC.l,2) - math.pow(locC.x - x,2))
	newLocB = location(x,locC.y, locC.z)
	if(debugPrint):
		print "step 5"	
		print newLocA.toString()
		print newLocB.toString()
		print locC.toString()
		print newSpanA
		print newSpanB

	#STEP 6:
	step6Inverse = jgh.moveToOrigin(newLocA,[newLocA,newLocB])
	if(debugPrint):
		print "step 6"	
		print newLocA.toString()
		print newLocB.toString()
		print locC.toString()

	#STEP 7:
	step7Inverse = jgh.rotateYToXYPlane(location(0.0,0.0,-1.0),[newLocA,newLocB])#newLocB,[newLocA,newLocB])
	if(debugPrint):
		print "step 7"	
		print newLocA.toString()
		print newLocB.toString()
		print locC.toString()
	#STEP 8:
	step8Inverse = jgh.rotateZToXZPlane(newLocB,[newLocA,newLocB])
	if(debugPrint):
		print "step 8"	
		print newLocA.toString()
		print newLocB.toString()
		print locC.toString()
	#STEP 9:
	newX,newY = jgh.solveForXY(newLocA,newSpanA,newLocB,newSpanB)
	if(debugPrint):
		print "X: %f,  Y: %f"%(newX,newY)
	#BUILD SOLUTION	
	locD = location(newX,-newY,0)#Why is this negative? check your right hand rules when building, the solve function goes in the negative Y when we want it positive	
	#STEP 8-inverse:
	jgh.rotateZToXZPlane_reverse(locD,step8Inverse)
	if(debugPrint):
		print "step 8 inverse"	
		print locD.toString()
	#STEP 7-inverse:	
	jgh.rotateYToXYPlane_reverse(locD,step7Inverse)	
	if(debugPrint):
		print "step 7 inverse"
		print locD.toString()
	#STEP 6-inverse:
	jgh.moveToOrigin_reverse(locD,step6Inverse)	
	if(debugPrint):
		print "step 6 inverse"
		print locD.toString()
	#STEP 3-inverse:
	jgh.rotateZToXZPlane_reverse(locD,step3Inverse)	
	if(debugPrint):
		print "step 3 inverse"
		print locD.toString()
	#STEP 2-inverse:
	jgh.rotateYToXYPlane_reverse(locD,step2Inverse)	
	if(debugPrint):
		print "step 2 inverse"
		print locD.toString()
	#STEP 1-inverse:
	jgh.moveToOrigin_reverse(locD,step1Inverse)	
	if(debugPrint):
		print "step 1 inverse"
		print locD.toString()
	#RETURN: The main function will set this object to have the location of locD
	return locD