def drawArrow(self, startPos, endPos, vector=None, lineWidth=.02):
if vector is None:
vector = [endPos[i] - startPos[i] for i in range(3)]
elif startPos is None:
startPos = [endPos[i] - vector[i] for i in range(3)]
length = mm.length(vector)
if length == 0.: return
glPushMatrix()
arrowT = mm.r_p_to_t(mm.getSO3FromVectors((length, 0, 0), vector),
startPos)
glMultMatrixf(arrowT.transpose())
triWidth = lineWidth * 3
triLength = triWidth * 1.2
# line + cone all parts
glePolyCone(
((0, 0, 0), (0, 0, 0), (length - triLength, 0, 0),
(length - triLength, 0, 0), (length, 0, 0), (length, 0, 0)), None,
(lineWidth / 2., lineWidth / 2., lineWidth / 2., triWidth / 2., 0,
0))
glPopMatrix()
def render(self):
for i in range(len(self.Rs)):
if self.Rs[i] != None and self.ps[i] != None:
T = mm.r_p_to_t(self.Rs[i], self.ps[i])
glPushMatrix()
glMultMatrixf(T.transpose())
ygh.drawCoordinate(self.totalColor, self.axisLength)
glPopMatrix()
def drawCircularArrow(self,
startPos,
endPos,
rotVec=None,
lineWidth=.02,
radius=.1):
if rotVec is None:
rotVec = [endPos[i] - startPos[i] for i in range(3)]
elif startPos is None:
startPos = [endPos[i] - rotVec[i] for i in range(3)]
length = mm.length(rotVec)
if length == 0.: return
glPushMatrix()
axisT = mm.r_p_to_t(mm.getSO3FromVectors((0, 0, length), rotVec),
startPos)
glMultMatrixf(axisT.transpose())
triWidth = lineWidth * 3
triLength = triWidth * 1.2
# axis
# self.drawLine((0,0,0), (0,0,length))
glePolyCylinder(((0, 0, 0), (0, 0, 0), (0, 0, length), (0, 0, length)),
None, lineWidth / 4.)
# circular line part
# gleHelicoid( rToroid , startRadius , drdTheta , startZ , dzdTheta ,
# startXform , dXformdTheta , startTheta , sweepTheta )
sweepTheta = 2 * math.pi * length * mm.DEG
gleHelicoid(lineWidth / 2., radius, 0., 0., radius, None, None, 0.,
sweepTheta)
# cone part
glPushMatrix()
glRotatef(sweepTheta, 0, 0, 1)
glTranslatef(radius, 0, radius * (sweepTheta / 360.))
glRotatef(-90, 1, 0, 0)
glePolyCone(
((0, 0, 0), (0, 0, 0), (0, 0, triLength), (0, 0, triLength)), None,
(triWidth / 2., triWidth / 2., 0, 0))
glPopMatrix()
glPopMatrix()
def test_FramesRenderer_OrientationsRenderer():
frame0 = mm.i_se3()
frame1 = mm.r_p_to_t(mm.exp(mm.vec3(0, 1, 0), math.pi / 8.), (1, 0, 0))
viewer = ysv.SimpleViewer()
viewer.doc.addRenderer('frame0', FramesRenderer([frame0], (255, 0, 0)))
viewer.doc.addRenderer('frame1', FramesRenderer([frame1], (255, 0, 0)))
viewer.doc.addRenderer(
'orientation0',
OrientationsRenderer([mm.t_to_r(frame0)], [mm.t_to_p(frame0)],
(0, 255, 0)))
viewer.doc.addRenderer(
'orientation1',
OrientationsRenderer([mm.t_to_r(frame1)], [mm.t_to_p(frame1)],
(0, 255, 0)))
viewer.show()
Fl.run()
def draw2DArrow(self, startPos, endPos, vector=None, lineWidth=.02):
if vector is None:
vector = [endPos[i] - startPos[i] for i in range(3)]
elif startPos is None:
startPos = [endPos[i] - vector[i] for i in range(3)]
# glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
glDisable(GL_CULL_FACE)
glPushMatrix()
length = mm.length(vector)
arrowT = mm.r_p_to_t(mm.getSO3FromVectors((length, 0, 0), vector),
startPos)
glMultMatrixf(arrowT.transpose())
triWidth = lineWidth * 3
triLength = triWidth * 1.2
angles = [0, 90]
for angle in angles:
glRotatef(angle, 1, 0, 0)
# line part
glBegin(GL_QUADS)
glVertex3f(0, 0, lineWidth / 2)
glVertex3f(0, 0, -lineWidth / 2)
glVertex3f(length - triLength, 0, -lineWidth / 2)
glVertex3f(length - triLength, 0, +lineWidth / 2)
glEnd()
# triangle part
glBegin(GL_TRIANGLES)
glVertex3f(length - triLength, 0, triWidth / 2)
glVertex3f(length - triLength, 0, -triWidth / 2)
glVertex3f(length, 0, 0)
glEnd()
glPopMatrix()
glEnable(GL_CULL_FACE)