def createCellLocators(self, dataDict=None):
self.locatorData = {}
for linkName, cellCodeArray in dataDict.iteritems():
polyData = vtk.vtkPolyData()
self.robotStateModel.model.getLinkModelMesh(linkName, polyData)
cellCodeArrayVTK = numpy_support.numpy_to_vtk(cellCodeArray)
arrayName = "cellCodeArray"
cellCodeArrayVTK.SetName(arrayName)
polyData.GetCellData().AddArray(cellCodeArrayVTK)
thresholdRange = [1,1]
polyData = filterUtils.thresholdCells(polyData, arrayName, thresholdRange)
cellData = polyData.GetCellData()
normals = cellData.GetNormals()
if polyData.GetNumberOfCells() == 0:
continue
locator = vtk.vtkCellLocator()
locator.SetDataSet(polyData)
locator.BuildLocator()
meshToWorld = transformUtils.copyFrame(self.linkFrameContainer.getLinkFrame(linkName))
worldToMesh = meshToWorld.GetLinearInverse()
self.locatorData[linkName] = {'locator':locator, 'polyData': polyData, 'meshToWorld': meshToWorld,
'worldToMesh': worldToMesh, 'cellData': cellData, 'normals': normals}
def buildCellLocator(polyData):
#print "buidling cell locator"
loc = vtk.vtkCellLocator()
loc.SetDataSet(polyData)
loc.BuildLocator()
return loc
def clear_locator(self):
d = DebugData()
for ship, frame in self._commonships:
d.addPolyData(ship.to_positioned_polydata())
self.locator = vtk.vtkCellLocator()
self.locator.SetDataSet(d.getPolyData())
self.locator.BuildLocator()
def buildCellLocator(polyData):
#print "buidling cell locator"
loc = vtk.vtkCellLocator()
loc.SetDataSet(polyData)
loc.BuildLocator()
return loc
def update_locator(self):
"""Updates cell locator."""
d = DebugData()
d.addPolyData(self._world.to_polydata())
for obstacle, frame in self._obstacles:
d.addPolyData(obstacle.to_positioned_polydata())
self.locator = vtk.vtkCellLocator()
self.locator.SetDataSet(d.getPolyData())
self.locator.BuildLocator()
def update_locator(self):
"""Updates cell locator."""
d = DebugData()
d.addPolyData(self._world.to_polydata())
for obstacle, frame in self._obstacles:
d.addPolyData(obstacle.to_positioned_polydata())
self.locator = vtk.vtkCellLocator()
self.locator.SetDataSet(d.getPolyData())
self.locator.BuildLocator()
def computePointToSurfaceDistance(pointsPolyData, meshPolyData):
cl = vtk.vtkCellLocator()
cl.SetDataSet(meshPolyData)
cl.BuildLocator()
points = vnp.getNumpyFromVtk(pointsPolyData, 'Points')
dists = np.zeros(len(points))
closestPoint = np.zeros(3)
closestPointDist = vtk.mutable(0.0)
cellId = vtk.mutable(0)
subId = vtk.mutable(0)
for i in xrange(len(points)):
cl.FindClosestPoint(points[i], closestPoint, cellId, subId,
closestPointDist)
dists[i] = closestPointDist
return np.sqrt(dists)
def update_locator(self):
"""Updates cell locator."""
d = DebugData()
size = self._worldsize
#d.addPolyData(self._world.to_polydata())
for robot, frame in self._robots:
d.addPolyData(
self._get_line(
[robot._initPos[0], robot._initPos[1] + 51, 0],
[robot._target[0] + 30, robot._target[1] + 51, 0],
1,
color=[1, 1, 1]))
d.addPolyData(
self._get_line(
[robot._initPos[0], robot._initPos[1] - 51, 0],
[robot._target[0] + 30, robot._target[1] - 51, 0],
2,
color=[1, 1, 1]))
for ship, frame in self._commonships:
d.addPolyData(ship.to_positioned_polydata())
self.locator = vtk.vtkCellLocator()
self.locator.SetDataSet(d.getPolyData())
self.locator.BuildLocator()
def buildCellLocator(polyData):
loc = vtk.vtkCellLocator()
loc.SetDataSet(polyData)
loc.BuildLocator()
return loc
def buildCellLocator(polyData):
loc = vtk.vtkCellLocator()
loc.SetDataSet(polyData)
loc.BuildLocator()
return loc
