def computeIntersection(locator, rayOrigin, rayEnd): tolerance = 0.0 # intersection tolerance pt = [0.0, 0.0, 0.0] # data coordinate where intersection occurs lineT = vtk.mutable(0.0) # parametric distance along line segment where intersection occurs pcoords = [0.0, 0.0, 0.0] # parametric location within cell (triangle) where intersection occurs subId = vtk.mutable(0) # sub id of cell intersection result = locator.IntersectWithLine(rayOrigin, rayEnd, tolerance, lineT, pt, pcoords, subId) return pt if result else None
def newWalkingGoal(displayPoint, view): footFrame = footstepsDriver.getFeetMidPoint(robotModel) worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint) groundOrigin = footFrame.GetPosition() groundNormal = [0.0, 0.0, 1.0] selectedGroundPoint = [0.0, 0.0, 0.0] t = vtk.mutable(0.0) vtk.vtkPlane.IntersectWithLine(worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint) footFrame.Translate(np.array(selectedGroundPoint) - np.array(footFrame.GetPosition())) footstepsdriverpanel.panel.onNewWalkingGoal(footFrame)