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)
