def computeLocationAs2D(j): #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: Rotate around X axis till point 3 lies in Z=0 plane #STEP 4: Solve for X and Y #STEP 5: Build the solution #STEP 6: Back out all transformations #STEP 0: locA,locB,locC = copyAllJoints(j) #STEP 1: step1Inverse = jgh.moveToOrigin(locA,[locA,locB,locC]) #STEP 2: step2Inverse = jgh.rotateYToXYPlane(locB,[locA,locB,locC]) #STEP 3: step3Inverse = jgh.rotateZToXZPlane(locB,[locA,locB,locC]) #STEP 4: if(j.farOrNear == 'n'): yIsPositive = False else: yIsPositive = True step4Inverse = jgh.rotateXToXYPlane(locC,[locA,locB,locC],yIsPos=yIsPositive) #STEP 5: x,y = jgh.solveForXY(locA,j.spanA.l,locB,j.spanB.l) #BUILD SOLUTION locD = location(x,y,0) #STEP 4-inverse: jgh.rotateXToXYPlane_reverse(locD,step4Inverse) #STEP 3-inverse: jgh.rotateZToXZPlane_reverse(locD,step3Inverse) #STEP 2-inverse: jgh.rotateYToXYPlane_reverse(locD,step2Inverse) #STEP 1-inverse: jgh.moveToOrigin_reverse(locD,step1Inverse) #RETURN: The main function will set this object to have the location of locD return locD
def checkForCollisionEdgeCylinder(line,otherLine,minDistance,debugPrint=False): #debugPrint = True #STEP 1: Copy joints #STEP 2: Move so that plane orig is 0,0,0 #STEP 3: Rotate around Y axis till line is in XY-plane #STEP 4: Rotate around Z axis till line is the X-axis # -- Span/span collision -- (if the other line penetrates the cylinder around this line, it will be caught here. But it can still come in through the ends...) #STEP 5: Check X1 and Y1 to assure it's square is not zero-zero. If it is, the quad formula we'll use wont work so we'll need to give it just a quick translation #STEP 6: Find the two possible collision points (care for the 1-point case) #STEP 7: Check each point to see if it's actually on the other line #STEP 8: Check each point to see if it's actually on line la,lb,ola,olb = jgh.copyPoints(line[0],line[1],otherLine[0],otherLine[1]) allPoints = [la,lb,ola,olb] if(debugPrint): print "step 1" for pt in allPoints: print pt.toString() #STEP 2 step2Inverse = jgh.moveToOrigin(la,allPoints) if(debugPrint): print "step 2" for pt in allPoints: print pt.toString() #STEP 3: step3Inverse = jgh.rotateYToXYPlane(lb,allPoints) if(debugPrint): print "step 3" for pt in allPoints: print pt.toString() #STEP 4: step4Inverse = jgh.rotateZToXZPlane(lb,allPoints) if(debugPrint): print "step 4" for pt in allPoints: print pt.toString() #STEP 5: if ola.z*ola.z+ola.y*ola.y == 0.0: jgh.movePoints(location(0,1,1),allPoints) if debugPrint: print "step 5: had to move points" #STEP 6: z1 = ola.z y1 = ola.y x1 = ola.x z2 = olb.z y2 = olb.y x2 = olb.x ye = la.y #could pick either A or B since they should now be equal ze = la.z #Move everything onto x=0 since we're not acting like line is a point ptA,ptB = solvePointToLine(0,ye,ze,0,y1,z1,0,y2,z2,minDistance,debugPrint=debugPrint) if ptA == 'NULL' and ptB == 'NULL': if debugPrint: print "step 6: there's no point to be found here" return False else: if debugPrint: print "step 6: found points: %s\t%s"%(ptA.toString(),ptB.toString()) # Fill in the X values we ignored earlier if y1 != y2: ptA.x = ((ptA.y-y2)/(y1-y2))*(x1-x2)+x2 ptB.x = ((ptB.y-y2)/(y1-y2))*(x1-x2)+x2 elif z1 != z2: #if y is parallel, use z ptA.x = ((ptA.z-z2)/(z1-z2))*(x1-x2)+x2 ptB.x = ((ptB.z-z2)/(z1-z2))*(x1-x2)+x2 else: #Only ever get here if it's a perfect hit on a parallel line, which will also get picked up by the end caps if debugPrint: print "step 6: no solution for x" return False #STEP 7: if (ola.findLength(ptA) > ola.findLength(olb) or olb.findLength(ptA) > olb.findLength(ola)): ptA = 'NULL' if debugPrint: print "step 7: eliminating pta" if (ola.findLength(ptB) > ola.findLength(olb) or olb.findLength(ptB) > olb.findLength(ola)): ptB = 'NULL' if debugPrint: print "step 7: eliminating ptb" #STEP 8: if ptA != 'NULL': if (ptA.x < 0.0 or ptA.x > lb.x): if debugPrint: print "step 8: eliminating pta" ptA = 'NULL' if ptB != 'NULL': if (ptB.x < 0.0 or ptB.x > lb.x): if debugPrint: print "step 8: eliminating ptb" ptB = 'NULL' #RETURN if ptA != 'NULL' or ptB != 'NULL': if debugPrint: print "found an edge point that collides" return True return False
def checkForCollisionPlane(lineA,lineB,colLine,minDistance,debugPrint=False): if lineA == 'NULL' or lineB == 'NULL' or colLine == 'NULL': return False #STEP 1: Copy joints #STEP 2: Move so that plane orig is 0,0,0 #STEP 3: Rotate around Y axis till plane pointA is in XY-plane #STEP 4: Rotate around Z axis till plane pointA in X-axis #STEP 5: Rotate around X axis till plane pointB is in XY-plane #STEP 6: Near edge: For each line end, check distance to Z, if less than min distance, check the point there #STEP 7: Clear cross: If the line begins and end on opposite sides of Z=0, solve for Z=0 and get the X,Y #STEP 8: Check if the angle from origin to this new point is smaller than the angle to plane pointB #STEP 9: Check if the angle from planePointA to this new point is smaller than the angle from plane pointA to plane pointB #STEP 1 plo,pla,plb,la,lb = jgh.copyPoints(lineA[0],lineA[1],lineB[1],colLine[0],colLine[1]) allPoints = [plo,pla,plb,la,lb] #STEP 2 step2Inverse = jgh.moveToOrigin(plo,allPoints) if(debugPrint): print "step 2" for pt in allPoints: print pt.toString() #STEP 3: step3Inverse = jgh.rotateYToXYPlane(pla,allPoints) if(debugPrint): print "step 3" for pt in allPoints: print pt.toString() #STEP 4: step4Inverse = jgh.rotateZToXZPlane(pla,allPoints) if(debugPrint): print "step 4" for pt in allPoints: print pt.toString() #STEP 5: step5Inverse = jgh.rotateXToXYPlane(plb,allPoints,yIsPos=True) if(debugPrint): print "step 5" for pt in allPoints: print pt.toString() ptsToCheck = [] #STEP 6: if (math.fabs(la.z)-minDistance) < 0.0: #No need to handle near-miss/hit with a fluff factor since we already have the standoff distance. tmp = la.copy() tmp.z = 0.0 ptsToCheck.append(tmp) if debugPrint: print "end point was close to plane" if (math.fabs(lb.z)-minDistance) < 0.0: #No need to handle near-miss/hit with a fluff factor since we already have the standoff distance. tmp = lb.copy() tmp.z = 0.0 ptsToCheck.append(tmp) if debugPrint: print "end point was close to plane" #STEP 7: if (la.z < 0.0 and lb.z > 0.0) or (la.z > 0.0 and lb.z < 0.0): #N(X1-X2)+X2 = X #N(Y1-Y2)+Y2 = Y #N(Z1-Z2)+Z2 = 0 tmpX = lb.x + (-lb.z/(la.z-lb.z))*(la.x-lb.x) tmpY = lb.y + (-lb.z/(la.z-lb.z))*(la.y-lb.y) tmp = location(tmpX,tmpY,0.0) ptsToCheck.append(tmp) if debugPrint: print "line goes clear through the plane" #STEP 8,9: for pt in ptsToCheck: origAngleMax = math.atan2(plb.y,plb.x) origAngleFound = math.atan2(pt.y,pt.x) farAngleMax = math.atan2(plb.y,(pla.x-plb.x)) farAngleFound = math.atan2(pt.y,(pla.x-pt.x)) if(origAngleFound > 0.0 and origAngleFound < origAngleMax and farAngleFound > 0.0 and farAngleFound < farAngleMax): if debugPrint: "line goes through the surface" return True if debugPrint: print "STEP 8,9: Not a hit. point:%s, origAngleMax:%f, origAngleFound:%f, farAngleMax:%f, farAngleFound:%f"%(pt.toString(),origAngleMax,origAngleFound,farAngleMax,farAngleFound) return False
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