def draw_glider(self):
_rot = coin.SbRotation()
_rot.setValue(coin.SbVec3f(0, 1, 0), coin.SbVec3f(1, 0, 0))
rot = coin.SoRotation()
rot.rotation.setValue(_rot)
self.task_separator += rot
draw_glider(self.parametric_glider.get_glider_3d(),
self.task_separator, hull=None, ribs=True,
fill_ribs=False)
draw_lines(self.parametric_glider.get_glider_3d(), vis_lines=self.task_separator, line_num=1)
def __init__(self, points, dynamic=False, arrow_size=0.04, length=2):
super(Arrow, self).__init__(points, dynamic)
self.arrow_sep = coin.SoSeparator()
self.arrow_rot = coin.SoRotation()
self.arrow_scale = coin.SoScale()
self.arrow_translate = coin.SoTranslation()
self.arrow_scale.scaleFactor.setValue(arrow_size, arrow_size, arrow_size)
self.cone = coin.SoCone()
arrow_length = coin.SoScale()
arrow_length.scaleFactor = (1, length, 1)
arrow_origin = coin.SoTranslation()
arrow_origin.translation = (0, -1, 0)
self.arrow_sep += [self.arrow_translate, self.arrow_rot, self.arrow_scale]
self.arrow_sep += [arrow_length, arrow_origin, self.cone]
self += [self.arrow_sep]
self.set_arrow_direction()
def get_scene_graph_transform(self):
"""
Create the Coin3D transform to translate and rotate this frame, in
accordance with the self._rot and self._loc matrices held in this class.
"""
n = coin.SoSeparator()
if N.any(self._loc != N.array((0, 0, 0))):
tr = coin.SoTranslation()
x, y, z = self._loc
tr.translation = coin.SbVec3f((x, y, z))
#print "tr.translation = ",tr.translation.getValue().getValue()
n.addChild(tr)
if not N.all(N.equal(self._rot, N.eye(3))):
m = self._rot
# http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
tr = m[0, 0] + m[1, 1] + m[2, 2]
if tr > 0:
S = N.sqrt(tr + 1.0) * 2
qw = 0.25 * S
qx = (m[2, 1] - m[1, 2]) / S
qy = (m[0, 2] - m[2, 0]) / S
qz = (m[1, 0] - m[0, 1]) / S
elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
S = N.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2
qw = (m[2, 1] - m[1, 2]) / S
qx = 0.25 * S
qy = (m[0, 1] + m[1, 0]) / S
qz = (m[0, 2] + m[2, 0]) / S
elif m[1, 1] > m[2, 2]:
S = N.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2
qw = (m[0, 2] - m[2, 0]) / S
qx = (m[0, 1] + m[1, 0]) / S
qy = 0.25 * S
qz = (m[1, 2] + m[2, 1]) / S
else:
S = N.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2
qw = (m[1, 0] - m[0, 1]) / S
qx = (m[0, 2] + m[2, 0]) / S
qy = (m[1, 2] + m[2, 1]) / S
qz = 0.25 * S
ro = coin.SoRotation()
ro.rotation = (qx, qy, qz, qw)
n.addChild(ro)
return n
def makeFrame(self, frame_labels):
"""
Method which makes a Coin3D frame to show a current pose in a RobRotation.
A frame is made from 3 red, green and blue arrows representing X, Y and Z.
Arrows are each constructed from a shaft and an arrowhead. Their dimensions
and other attributes are unassigned as they are extracted from appropriate
`RobRotation` properties.
Returns:
A SoSeparator with the frame shown in the FreeCAD View.
"""
# make a generic shaft from 0 in Y direction
shaft_vertices = coin.SoVertexProperty()
shaft_vertices.vertex.setNum(2)
shaft_vertices.vertex.set1Value(0, 0, 0, 0)
self.frame_shaft = coin.SoLineSet()
self.frame_shaft.vertexProperty.setValue(shaft_vertices)
self.frame_shaft.numVertices.setNum(1)
self.frame_shaft.numVertices.setValue(2)
# make a generic conic arrowhead oriented in Y axis direction and
# move it at the end of the shaft
self.frame_arrowhead_translation = coin.SoTranslation()
self.frame_arrowhead_cone = coin.SoCone()
self.frame_arrowhead = coin.SoSwitch()
self.frame_arrowhead.addChild(self.frame_arrowhead_translation)
self.frame_arrowhead.addChild(self.frame_arrowhead_cone)
# make rotations to rotate prepared shaft and arrowhead for Y axis
# direction also to X and Z
rot_y2x = coin.SoRotation()
rot_y2x.rotation.setValue(coin.SbRotation(coin.SbVec3f(0, 1, 0),
coin.SbVec3f(1, 0, 0)))
rot_y2z = coin.SoRotation()
rot_y2z.rotation.setValue(coin.SbRotation(coin.SbVec3f(0, 1, 0),
coin.SbVec3f(0, 0, 1)))
# prepare colors for X,Y,Z which will correspond to R,G,B as customary
self.frame_color_x = coin.SoPackedColor()
self.frame_color_y = coin.SoPackedColor()
self.frame_color_z = coin.SoPackedColor()
# make complete colored and rotated arrows
x_arrow = coin.SoSeparator()
x_arrow.addChild(rot_y2x)
x_arrow.addChild(self.frame_color_x)
x_arrow.addChild(self.frame_shaft)
x_arrow.addChild(self.frame_arrowhead)
x_arrow.addChild(frame_labels[0])
y_arrow = coin.SoSeparator()
y_arrow.addChild(self.frame_color_y)
y_arrow.addChild(self.frame_shaft)
y_arrow.addChild(self.frame_arrowhead)
y_arrow.addChild(frame_labels[1])
z_arrow = coin.SoSeparator()
z_arrow.addChild(rot_y2z)
z_arrow.addChild(self.frame_color_z)
z_arrow.addChild(self.frame_shaft)
z_arrow.addChild(self.frame_arrowhead)
z_arrow.addChild(frame_labels[2])
# prepare draw style to control shaft width
self.frame_drawstyle = coin.SoDrawStyle()
# make complete frame and it to shaded display mode
separated_frame = coin.SoSeparator()
separated_frame.addChild(self.frame_drawstyle)
separated_frame.addChild(x_arrow)
separated_frame.addChild(y_arrow)
separated_frame.addChild(z_arrow)
return separated_frame
def makeFrame(self, frame_labels):
# make a generic shaft from 0 in Y direction
shaft_vertices = coin.SoVertexProperty()
shaft_vertices.vertex.setNum(2)
shaft_vertices.vertex.set1Value(0, 0, 0, 0)
self.frame_shaft = coin.SoLineSet()
self.frame_shaft.vertexProperty.setValue(shaft_vertices)
self.frame_shaft.numVertices.setNum(1)
self.frame_shaft.numVertices.setValue(2)
# make a generic conic arrowhead oriented in Y axis direction and
# move it at the end of the shaft
self.frame_arrowhead_translation = coin.SoTranslation()
self.frame_arrowhead_cone = coin.SoCone()
self.frame_arrowhead = coin.SoSwitch()
self.frame_arrowhead.addChild(self.frame_arrowhead_translation)
self.frame_arrowhead.addChild(self.frame_arrowhead_cone)
# make rotations to rotate prepared shaft and arrowhead for Y axis
# direction also to X and Z
rot_y2x = coin.SoRotation()
rot_y2x.rotation.setValue(
coin.SbRotation(coin.SbVec3f(0, 1, 0), coin.SbVec3f(1, 0, 0)))
rot_y2z = coin.SoRotation()
rot_y2z.rotation.setValue(
coin.SbRotation(coin.SbVec3f(0, 1, 0), coin.SbVec3f(0, 0, 1)))
# prepare colors for X,Y,Z which will correspond to R,G,B as customary
self.frame_color_x = coin.SoPackedColor()
self.frame_color_y = coin.SoPackedColor()
self.frame_color_z = coin.SoPackedColor()
# make complete colored and rotated arrows
x_arrow = coin.SoSeparator()
x_arrow.addChild(rot_y2x)
x_arrow.addChild(self.frame_color_x)
x_arrow.addChild(self.frame_shaft)
x_arrow.addChild(self.frame_arrowhead)
x_arrow.addChild(frame_labels[0])
y_arrow = coin.SoSeparator()
y_arrow.addChild(self.frame_color_y)
y_arrow.addChild(self.frame_shaft)
y_arrow.addChild(self.frame_arrowhead)
y_arrow.addChild(frame_labels[1])
z_arrow = coin.SoSeparator()
z_arrow.addChild(rot_y2z)
z_arrow.addChild(self.frame_color_z)
z_arrow.addChild(self.frame_shaft)
z_arrow.addChild(self.frame_arrowhead)
z_arrow.addChild(frame_labels[2])
# prepare draw style to control shaft width
self.frame_drawstyle = coin.SoDrawStyle()
# make complete frame and it to shaded display mode
separated_frame = coin.SoSeparator()
separated_frame.addChild(self.frame_drawstyle)
separated_frame.addChild(x_arrow)
separated_frame.addChild(y_arrow)
separated_frame.addChild(z_arrow)
return separated_frame