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