Verified Commit 6bdbe5d3 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Justin Carpentier
Browse files

Update gepetto-viewer utility functions.

parent 88cc70a3
......@@ -54,3 +54,27 @@ def displayDistanceResult(gui, group_name, res, closest_points = True, normal =
gui.applyConfiguration(n,
res.getNearestPoint1().tolist() + hppfcl.Quaternion.FromTwoVectors(np.array([1,0,0]), res.normal).coeffs().tolist())
gui.refresh()
def displayCollisionResult(gui, group_name, res, color=Color.green):
if res.isCollision():
if gui.nodeExists(group_name):
gui.setVisibility(group_name, "ON")
else:
gui.createGroup(group_name)
for i in range(res.numContacts()):
contact = res.getContact(i)
n = group_name+"/contact"+str(i)
depth = contact.penetration_depth
if gui.nodeExists(n):
gui.setFloatProperty(n, 'Size', depth)
gui.setFloatProperty(n, 'Radius', 0.1*depth)
gui.setColor(n, color)
else:
gui.addArrow(n, depth*0.1, depth, color)
N = contact.normal
P = contact.pos
gui.applyConfiguration(n, (P-depth*N/2).tolist() +
hppfcl.Quaternion.FromTwoVectors(np.array([1,0,0]), N).coeffs().tolist())
gui.refresh()
elif gui.nodeExists(group_name):
gui.setVisibility(group_name, "OFF")
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment