def draw_axes(axes_l=10, number=''):
glLineWidth(5.0)
glBegin(GL_LINES)
# x axis, red
if config.enable_lighting:
material.red()
else:
glColor3f(1.0, 0, 0)
glVertex3f(0, 0, 0)
glVertex3f(axes_l, 0, 0)
# y axis, green
if config.enable_lighting:
material.green()
else:
glColor3f(0, 1.0, 0)
glVertex3f(0, 0, 0)
glVertex3f(0, axes_l, 0)
# z axis
if config.enable_lighting:
material.blue()
else:
glColor3f(0, 0, 1.0)
glVertex3f(0, 0, 0)
glVertex3f(0, 0, axes_l)
glEnd()
glLineWidth(1.0)
offset = axes_l / 7
text_at_pos(offset + axes_l, 0, 0, 'X' + number)
text_at_pos(0, offset + axes_l, 0, 'Y' + number)
text_at_pos(0, 0, offset + axes_l, 'Z' + number)
def draw_axes(axes_l = 10, number=''):
glLineWidth(5.0)
glBegin(GL_LINES)
# x axis, red
if config.enable_lighting:
material.red()
else:
glColor3f(1.0, 0, 0)
glVertex3f(0, 0, 0)
glVertex3f(axes_l, 0, 0)
# y axis, green
if config.enable_lighting:
material.green()
else:
glColor3f(0, 1.0, 0)
glVertex3f(0, 0, 0)
glVertex3f(0, axes_l, 0)
# z axis
if config.enable_lighting:
material.blue()
else:
glColor3f(0, 0, 1.0)
glVertex3f(0, 0, 0)
glVertex3f(0, 0, axes_l)
glEnd()
glLineWidth(1.0)
offset = axes_l/7
text_at_pos(offset + axes_l, 0, 0, 'X' + number)
text_at_pos(0, offset + axes_l, 0, 'Y' + number)
text_at_pos(0, 0, offset + axes_l, 'Z' + number)
def render(self):
for link in self.links:
R = link.R
P = link.P
h = link.h
if config.enable_lighting:
material.black()
else:
glColor3f(0, 0, 0)
glPushMatrix()
glLineWidth(15)
glBegin(GL_LINES)
if not link.is_prismatic():
glVertex3f(0, 0, 0)
glVertex3f(P[0], P[1], P[2])
else:
glVertex3f(0, 0, 0)
glVertex3f(P[0] - link.q * link.h[0],
P[1] - link.q * link.h[1],
P[2] - link.q * link.h[2])
glEnd()
glTranslate(P[0], P[1], P[2])
if config.enable_axis:
if link.index != self.N:
display.draw_axes(20, str(link.index))
else:
display.draw_axes(20, 'T')
# draw joint
if link.is_prismatic(): # prismatic joint
display.draw_prismatic_joint([[0], [0], [0]], link.q * link.h,
10)
elif link.is_rotational():
if config.enable_lighting:
material.green()
else:
glColor3f(0, 0.6, 0)
display.draw_rotational_joint(h * 10, h * -10, 8,
link.q * 180 / PI)
if config.enable_lighting:
material.grey()
else:
glColor3f(0.3, 0.3, 0.3)
glRotate(link.q * 180 / PI, link.h[0], link.h[1], link.h[2])
elif (R == eye(3)).all(): # link - no joint
pass
# pop all joints off
for matPop in self.links:
glPopMatrix()
glLineWidth(5)
glPointSize(10)
# only save the last whatever points
self.trace = self.trace[-config.max_trace:]
if config.enable_ghost:
if config.enable_lighting:
material.grey()
else:
glColor3f(0.4, 0.4, 0.4)
glPointSize(8)
for verts, i in zip(self.trace, range(len(self.trace))):
if i % config.ghost_interval == 1:
glBegin(GL_POINTS)
for vert in verts:
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
if config.enable_lighting:
material.grey()
else:
glColor3f(0.7, 0.7, 0.7)
for verts, i in zip(self.trace, range(len(self.trace))):
if i % config.ghost_interval == 1:
glBegin(GL_LINE_STRIP)
for vert in verts:
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
if config.enable_trace:
# saved tool positions
if config.enable_lighting:
material.red()
else:
glColor3f(1.0, 0.0, 0)
glBegin(GL_LINE_STRIP)
for verts in self.trace:
vert = verts[-1]
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
def render(self):
for link in self.links:
R = link.R
P = link.P
h = link.h
if config.enable_lighting:
material.black()
else:
glColor3f(0, 0, 0)
glPushMatrix()
glLineWidth(15)
glBegin(GL_LINES)
if not link.is_prismatic():
glVertex3f(0, 0, 0)
glVertex3f(P[0], P[1], P[2])
else:
glVertex3f(0, 0, 0)
glVertex3f(P[0]-link.q*link.h[0], P[1] - link.q*link.h[1], P[2] - link.q*link.h[2])
glEnd()
glTranslate(P[0], P[1], P[2])
if config.enable_axis:
if link.index != self.N:
display.draw_axes(20, str(link.index))
else:
display.draw_axes(20, 'T')
# draw joint
if link.is_prismatic(): # prismatic joint
display.draw_prismatic_joint([[0], [0], [0]], link.q*link.h, 10)
elif link.is_rotational():
if config.enable_lighting:
material.green()
else:
glColor3f(0, 0.6, 0)
display.draw_rotational_joint(h*10, h*-10, 8, link.q*180/PI)
if config.enable_lighting:
material.grey()
else:
glColor3f(0.3, 0.3, 0.3)
glRotate(link.q * 180 / PI, link.h[0], link.h[1], link.h[2])
elif (R == eye(3)).all(): # link - no joint
pass
# pop all joints off
for matPop in self.links:
glPopMatrix()
glLineWidth(5)
glPointSize(10)
# only save the last whatever points
self.trace = self.trace[-config.max_trace:]
if config.enable_ghost:
if config.enable_lighting:
material.grey()
else:
glColor3f(0.4, 0.4, 0.4)
glPointSize(8)
for verts, i in zip(self.trace, range(len(self.trace))):
if i % config.ghost_interval == 1:
glBegin(GL_POINTS)
for vert in verts:
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
if config.enable_lighting:
material.grey()
else:
glColor3f(0.7, 0.7, 0.7)
for verts, i in zip(self.trace, range(len(self.trace))):
if i % config.ghost_interval == 1:
glBegin(GL_LINE_STRIP)
for vert in verts:
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
if config.enable_trace:
# saved tool positions
if config.enable_lighting:
material.red()
else:
glColor3f(1.0, 0.0, 0)
glBegin(GL_LINE_STRIP)
for verts in self.trace:
vert = verts[-1]
glVertex3f(vert[0], vert[1], vert[2])
glEnd()