vismach.py
1 # Copyright 2007 John Kasunich and Jeff Epler 2 # 3 # This program is free software; you can redistribute it and/or modify 4 # it under the terms of the GNU General Public License as published by 5 # the Free Software Foundation; either version 2 of the License, or 6 # (at your option) any later version. 7 # 8 # This program is distributed in the hope that it will be useful, 9 # but WITHOUT ANY WARRANTY; without even the implied warranty of 10 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 # GNU General Public License for more details. 12 # 13 # You should have received a copy of the GNU General Public License 14 # along with this program; if not, write to the Free Software 15 # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 16 17 import rs274.OpenGLTk, Tkinter, signal, hal 18 from minigl import * 19 from math import * 20 import glnav 21 import hal 22 23 class Collection(object): 24 def __init__(self, parts): 25 self.parts = parts 26 self.vol = 0 27 28 def traverse(self): 29 for p in self.parts: 30 if hasattr(p, "apply"): 31 p.apply() 32 if hasattr(p, "capture"): 33 p.capture() 34 if hasattr(p, "draw"): 35 p.draw() 36 if hasattr(p, "traverse"): 37 p.traverse() 38 if hasattr(p, "unapply"): 39 p.unapply() 40 41 def volume(self): 42 if hasattr(self, "vol") and self.vol != 0: 43 vol = self.vol 44 else: 45 vol = sum(part.volume() for part in self.parts) 46 #print "Collection.volume", vol 47 return vol 48 49 # a collection consisting of overlapping parts will have an incorrect 50 # volume, because overlapping volumes will be counted twice. If the 51 # correct volume is known, it can be set using this method 52 def set_volume(self,vol): 53 self.vol = vol; 54 55 class Translate(Collection): 56 def __init__(self, parts, x, y, z): 57 self.parts = parts 58 self.where = x, y, z 59 60 def apply(self): 61 glPushMatrix() 62 glTranslatef(*self.where) 63 64 def unapply(self): 65 glPopMatrix() 66 67 class Scale(Collection): 68 def __init__(self, parts, x, y, z): 69 self.parts = parts 70 self.scaleby = x, y, z 71 72 def apply(self): 73 glPushMatrix() 74 glScalef(*self.scaleby) 75 76 def unapply(self): 77 glPopMatrix() 78 79 class HalTranslate(Collection): 80 def __init__(self, parts, comp, var, x, y, z): 81 self.parts = parts 82 self.where = x, y, z 83 self.comp = comp 84 self.var = var 85 86 def apply(self): 87 x, y, z = self.where 88 v = self.comp[self.var] 89 90 glPushMatrix() 91 glTranslatef(x*v, y*v, z*v) 92 93 def unapply(self): 94 glPopMatrix() 95 96 97 class HalRotate(Collection): 98 def __init__(self, parts, comp, var, th, x, y, z): 99 self.parts = parts 100 self.where = th, x, y, z 101 self.comp = comp 102 self.var = var 103 104 def apply(self): 105 th, x, y, z = self.where 106 glPushMatrix() 107 glRotatef(th * self.comp[self.var], x, y, z) 108 109 def unapply(self): 110 glPopMatrix() 111 112 113 class Rotate(Collection): 114 def __init__(self, parts, th, x, y, z): 115 self.parts = parts 116 self.where = th, x, y, z 117 118 def apply(self): 119 th, x, y, z = self.where 120 glPushMatrix() 121 glRotatef(th, x, y, z) 122 123 def unapply(self): 124 glPopMatrix() 125 126 127 class Track(Collection): 128 '''move and rotate an object to point from one capture()'d 129 coordinate system to another. 130 we need "world" to convert coordinates from GL_MODELVIEW coordinates 131 to our coordinate system''' 132 def __init__(self, parts, position, target, world): 133 self.parts = parts 134 self.target = target 135 self.position = position 136 self.world2view = world 137 138 def angle_to(self,x,y,z): 139 '''returns polar coordinates in degrees to a point from the origin 140 a rotates around the x-axis; b rotates around the y axis; r is the distance''' 141 azimuth = atan2(y, x)*180/pi #longitude 142 elevation = atan2(z, sqrt(x**2 + y**2))*180/pi 143 radius = sqrt(x**2+y**2+z**2) 144 return((azimuth, elevation, radius)) 145 146 def map_coords(self,tx,ty,tz,transform): 147 # now we have to transform them to the world frame 148 wx = tx*transform[0]+ty*transform[4]+tz*transform[8]+transform[12] 149 wy = tx*transform[1]+ty*transform[5]+tz*transform[9]+transform[13] 150 wz = tx*transform[2]+ty*transform[6]+tz*transform[10]+transform[14] 151 return([wx,wy,wz]) 152 153 154 def apply(self): 155 #make sure we have something to work with first 156 if (self.world2view.t == []): 157 #something's borkled - give up 158 print "vismach.py: Track: why am i here? world is not in the scene yet" 159 glPushMatrix() 160 return 161 162 view2world = invert(self.world2view.t) 163 164 px, py, pz = self.position.t[12:15] 165 px, py, pz = self.map_coords(px,py,pz,view2world) 166 tx, ty, tz = self.target.t[12:15] 167 tx, ty, tz = self.map_coords(tx,ty,tz,view2world) 168 dx = tx - px; dy = ty - py; dz = tz - pz; 169 (az,el,r) = self.angle_to(dx,dy,dz) 170 if(hasattr(HUD, "debug_track") and HUD.debug_track == 1): 171 HUD.strs = [] 172 HUD.strs += ["current coords: %3.4f %3.4f %3.4f " % (px, py, pz)] 173 HUD.strs += ["target coords: %3.4f %3.4f %3.4f" % (tx, ty, tz)] 174 HUD.strs += ["az,el,r: %3.4f %3.4f %3.4f" % (az,el,r)] 175 glPushMatrix() 176 glTranslatef(px,py,pz) 177 glRotatef(az-90,0,0,1) 178 glRotatef(el-90,1,0,0) 179 180 181 def unapply(self): 182 glPopMatrix() 183 184 class CoordsBase(object): 185 def __init__(self, *args): 186 if args and isinstance(args[0], hal.component): 187 self.comp = args[0] 188 args = args[1:] 189 else: 190 self.comp = None 191 self._coords = args 192 self.q = gluNewQuadric() 193 194 def coords(self): 195 return map(self._coord, self._coords) 196 197 def _coord(self, v): 198 if isinstance(v, str): return self.comp[v] 199 return v 200 201 # give endpoint X values and radii 202 # resulting cylinder is on the X axis 203 class CylinderX(CoordsBase): 204 def draw(self): 205 x1, r1, x2, r2 = self.coords() 206 if x1 > x2: 207 tmp = x1 208 x1 = x2 209 x2 = tmp 210 tmp = r1 211 r1 = r2 212 r2 = tmp 213 glPushMatrix() 214 # GL creates cylinders along Z, so need to rotate 215 z1 = x1 216 z2 = x2 217 glRotatef(90,0,1,0) 218 # need to translate the whole thing to z1 219 glTranslatef(0,0,z1) 220 # the cylinder starts out at Z=0 221 gluCylinder(self.q, r1, r2, z2-z1, 32, 1) 222 # bottom cap 223 glRotatef(180,1,0,0) 224 gluDisk(self.q, 0, r1, 32, 1) 225 glRotatef(180,1,0,0) 226 # the top cap needs flipped and translated 227 glPushMatrix() 228 glTranslatef(0,0,z2-z1) 229 gluDisk(self.q, 0, r2, 32, 1) 230 glPopMatrix() 231 glPopMatrix() 232 233 def volume(self): 234 x1, r1, x2, r2 = self.coords() 235 # actually a frustum of a cone 236 vol = 3.1415927/3.0 * abs(x1-x2)*(r1*r1+r1*r2+r2*r2) 237 #print "CylinderX.volume", vol 238 return vol 239 240 241 # give endpoint Y values and radii 242 # resulting cylinder is on the Y axis 243 class CylinderY(CoordsBase): 244 def __init__(self, y1, r1, y2, r2): 245 self._coords = y1, r1, y2, r2 246 self.q = gluNewQuadric() 247 248 def draw(self): 249 y1, r1, y2, r2 = self.coords() 250 if y1 > y2: 251 tmp = y1 252 y1 = y2 253 y2 = tmp 254 tmp = r1 255 r1 = r2 256 r2 = tmp 257 glPushMatrix() 258 # GL creates cylinders along Z, so need to rotate 259 z1 = y1 260 z2 = y2 261 glRotatef(-90,1,0,0) 262 # need to translate the whole thing to z1 263 glTranslatef(0,0,z1) 264 # the cylinder starts out at Z=0 265 gluCylinder(self.q, r1, r2, z2-z1, 32, 1) 266 # bottom cap 267 glRotatef(180,1,0,0) 268 gluDisk(self.q, 0, r1, 32, 1) 269 glRotatef(180,1,0,0) 270 # the top cap needs flipped and translated 271 glPushMatrix() 272 glTranslatef(0,0,z2-z1) 273 gluDisk(self.q, 0, r2, 32, 1) 274 glPopMatrix() 275 glPopMatrix() 276 277 def volume(self): 278 y1, r1, y2, r2 = self.coords() 279 # actually a frustum of a cone 280 vol = 3.1415927/3.0 * abs(y1-y2)*(r1*r1+r1*r2+r2*r2) 281 #print "CylinderY.volume", vol 282 return vol 283 284 285 class CylinderZ(CoordsBase): 286 def draw(self): 287 z1, r1, z2, r2 = self.coords() 288 if z1 > z2: 289 tmp = z1 290 z1 = z2 291 z2 = tmp 292 tmp = r1 293 r1 = r2 294 r2 = tmp 295 # need to translate the whole thing to z1 296 glPushMatrix() 297 glTranslatef(0,0,z1) 298 # the cylinder starts out at Z=0 299 gluCylinder(self.q, r1, r2, z2-z1, 32, 1) 300 # bottom cap 301 glRotatef(180,1,0,0) 302 gluDisk(self.q, 0, r1, 32, 1) 303 glRotatef(180,1,0,0) 304 # the top cap needs flipped and translated 305 glPushMatrix() 306 glTranslatef(0,0,z2-z1) 307 gluDisk(self.q, 0, r2, 32, 1) 308 glPopMatrix() 309 glPopMatrix() 310 311 def volume(self): 312 z1, r1, z2, r2 = self.coords() 313 # actually a frustum of a cone 314 vol = 3.1415927/3.0 * abs(z1-z2)*(r1*r1+r1*r2+r2*r2) 315 #print "CylinderZ.volume", vol 316 return vol 317 318 # give center and radius 319 class Sphere(CoordsBase): 320 def draw(self): 321 x, y, z, r = self.coords() 322 # need to translate the whole thing to x,y,z 323 glPushMatrix() 324 glTranslatef(x,y,z) 325 # the sphere starts out at the origin 326 gluSphere(self.q, r, 32, 16) 327 glPopMatrix() 328 329 def volume(self): 330 x, y, z, r = self.coords() 331 vol = 1.3333333*3.1415927*r*r*r 332 #print "Sphere.volume", vol 333 return vol 334 335 336 # triangular plate in XY plane 337 # specify the corners Z values for each side 338 class TriangleXY(CoordsBase): 339 def draw(self): 340 x1, y1, x2, y2, x3, y3, z1, z2 = self.coords() 341 x12 = x1-x2 342 y12 = y1-y2 343 x13 = x1-x3 344 y13 = y1-y3 345 cross = x12*y13 - x13*y12 346 if cross < 0: 347 tmp = x2 348 x2 = x3 349 x3 = tmp 350 tmp = y2 351 y2 = y3 352 y3 = tmp 353 if z1 > z2: 354 tmp = z1 355 z1 = z2 356 z2 = tmp 357 x12 = x1-x2 358 y12 = y1-y2 359 x23 = x2-x3 360 y23 = y2-y3 361 x31 = x3-x1 362 y31 = y3-y1 363 glBegin(GL_QUADS) 364 # side 1-2 365 h = hypot(x12,y12) 366 glNormal3f(-y12/h,x12/h,0) 367 glVertex3f(x1, y1, z1) 368 glVertex3f(x2, y2, z1) 369 glVertex3f(x2, y2, z2) 370 glVertex3f(x1, y1, z2) 371 # side 2-3 372 h = hypot(x23,y23) 373 glNormal3f(-y23/h,x23/h,0) 374 glVertex3f(x2, y2, z1) 375 glVertex3f(x3, y3, z1) 376 glVertex3f(x3, y3, z2) 377 glVertex3f(x2, y2, z2) 378 # side 3-1 379 h = hypot(x31,y31) 380 glNormal3f(-y31/h,x31/h,0) 381 glVertex3f(x3, y3, z1) 382 glVertex3f(x1, y1, z1) 383 glVertex3f(x1, y1, z2) 384 glVertex3f(x3, y3, z2) 385 glEnd() 386 glBegin(GL_TRIANGLES) 387 # upper face 388 glNormal3f(0,0,1) 389 glVertex3f(x1, y1, z2) 390 glVertex3f(x2, y2, z2) 391 glVertex3f(x3, y3, z2) 392 # lower face 393 glNormal3f(0,0,-1) 394 glVertex3f(x1, y1, z1) 395 glVertex3f(x3, y3, z1) 396 glVertex3f(x2, y2, z1) 397 glEnd() 398 399 def volume(self): 400 x1, y1, x2, y2, x3, y3, z1, z2 = self.coords() 401 # compute pts 2 and 3 relative to 1 (puts pt1 at origin) 402 x2 = x2-x1 403 x3 = x3-x1 404 y2 = y2-y1 405 y3 = y3-y1 406 # compute area of triangle 407 area = 0.5*abs(x2*y3 - x3*y2) 408 thk = abs(z1-z2) 409 vol = area*thk 410 #print "TriangleXY.volume = area * thickness)",vol, area, thk 411 return vol 412 413 # triangular plate in XZ plane 414 class TriangleXZ(TriangleXY): 415 def coords(self): 416 x1, z1, x2, z2, x3, z3, y1, y2 = TriangleXY.coords(self) 417 return x1, z1, x2, z2, x3, z3, -y1, -y2 418 419 def draw(self): 420 glPushMatrix() 421 glRotatef(90,1,0,0) 422 # create the triangle in XY plane 423 TriangleXY.draw(self) 424 # bottom cap 425 glPopMatrix() 426 427 def volume(self): 428 vol = TriangleXY.volume(self) 429 #print " TriangleXZ.volume",vol 430 return vol 431 432 # triangular plate in YZ plane 433 class TriangleYZ(TriangleXY): 434 def coords(self): 435 y1, z1, y2, z2, y3, z3, x1, x2 = TriangleXY.coords(self) 436 return z1, y1, z2, y2, z3, y3, -x1, -x2 437 438 def draw(self): 439 glPushMatrix() 440 glRotatef(90,0,-1,0) 441 # create the triangle in XY plane 442 TriangleXY.draw(self) 443 # bottom cap 444 glPopMatrix() 445 446 def volume(self): 447 vol = TriangleXY.volume(self) 448 #print " TriangleYZ.volume",vol 449 return vol 450 451 452 class ArcX(CoordsBase): 453 def draw(self): 454 x1, x2, r1, r2, a1, a2, steps = self.coords() 455 if x1 > x2: 456 tmp = x1 457 x1 = x2 458 x2 = tmp 459 if r1 > r2: 460 tmp = r1 461 r1 = r2 462 r2 = tmp 463 while a1 > a2: 464 a2 = a2 + 360 465 astep = ((a2-a1)/steps)*(pi/180) 466 a1rads = a1 * (pi/180) 467 # positive X end face 468 glBegin(GL_QUAD_STRIP) 469 glNormal3f(1,0,0) 470 n = 0 471 while n <= steps: 472 angle = a1rads+n*astep 473 s = sin(angle) 474 c = cos(angle) 475 glVertex3f(x2, r1*s, r1*c) 476 glVertex3f(x2, r2*s, r2*c) 477 n = n + 1 478 479 glEnd() 480 # negative X end face 481 glBegin(GL_QUAD_STRIP) 482 glNormal3f(-1,0,0) 483 n = 0 484 while n <= steps: 485 angle = a1rads+n*astep 486 s = sin(angle) 487 c = cos(angle) 488 glVertex3f(x1, r1*s, r1*c) 489 glVertex3f(x1, r2*s, r2*c) 490 n = n + 1 491 glEnd() 492 # inner diameter 493 glBegin(GL_QUAD_STRIP) 494 n = 0 495 while n <= steps: 496 angle = a1rads+n*astep 497 s = sin(angle) 498 c = cos(angle) 499 glNormal3f(0,-s, -c) 500 glVertex3f(x1, r1*s, r1*c) 501 glVertex3f(x2, r1*s, r1*c) 502 n = n + 1 503 glEnd() 504 # outer diameter 505 glBegin(GL_QUAD_STRIP) 506 n = 0 507 while n <= steps: 508 angle = a1rads+n*astep 509 s = sin(angle) 510 c = cos(angle) 511 glNormal3f(0, s, c) 512 glVertex3f(x1, r2*s, r2*c) 513 glVertex3f(x2, r2*s, r2*c) 514 n = n + 1 515 glEnd() 516 # end plates 517 glBegin(GL_QUADS) 518 # first end plate 519 angle = a1 * (pi/180) 520 s = sin(angle) 521 c = cos(angle) 522 glNormal3f(0, -c, s) 523 glVertex3f(x1, r2*s, r2*c) 524 glVertex3f(x2, r2*s, r2*c) 525 glVertex3f(x2, r1*s, r1*c) 526 glVertex3f(x1, r1*s, r1*c) 527 # other end 528 angle = a2 * (pi/180) 529 s = sin(angle) 530 c = cos(angle) 531 glNormal3f(0, c, -s) 532 glVertex3f(x1, r2*s, r2*c) 533 glVertex3f(x2, r2*s, r2*c) 534 glVertex3f(x2, r1*s, r1*c) 535 glVertex3f(x1, r1*s, r1*c) 536 glEnd() 537 538 def volume(self): 539 x1, x2, r1, r2, a1, a2, steps = self.coords() 540 if x1 > x2: 541 tmp = x1 542 x1 = x2 543 x2 = tmp 544 if r1 > r2: 545 tmp = r1 546 r1 = r2 547 r2 = tmp 548 while a1 > a2: 549 a2 = a2 + 360 550 height = x2 - x1 551 angle = a2 - a1 552 area = (angle/360.0)*pi*(r2*r2-r1*r1) 553 vol = area * height 554 #print "Arc.volume = angle * area * height",vol, angle, area, height 555 return vol 556 557 558 559 560 # six coordinate version - specify each side of the box 561 class Box(CoordsBase): 562 def draw(self): 563 x1, y1, z1, x2, y2, z2 = self.coords() 564 if x1 > x2: 565 tmp = x1 566 x1 = x2 567 x2 = tmp 568 if y1 > y2: 569 tmp = y1 570 y1 = y2 571 y2 = tmp 572 if z1 > z2: 573 tmp = z1 574 z1 = z2 575 z2 = tmp 576 577 glBegin(GL_QUADS) 578 # bottom face 579 glNormal3f(0,0,-1) 580 glVertex3f(x2, y1, z1) 581 glVertex3f(x1, y1, z1) 582 glVertex3f(x1, y2, z1) 583 glVertex3f(x2, y2, z1) 584 # positive X face 585 glNormal3f(1,0,0) 586 glVertex3f(x2, y1, z1) 587 glVertex3f(x2, y2, z1) 588 glVertex3f(x2, y2, z2) 589 glVertex3f(x2, y1, z2) 590 # positive Y face 591 glNormal3f(0,1,0) 592 glVertex3f(x1, y2, z1) 593 glVertex3f(x1, y2, z2) 594 glVertex3f(x2, y2, z2) 595 glVertex3f(x2, y2, z1) 596 # negative Y face 597 glNormal3f(0,-1,0) 598 glVertex3f(x2, y1, z2) 599 glVertex3f(x1, y1, z2) 600 glVertex3f(x1, y1, z1) 601 glVertex3f(x2, y1, z1) 602 # negative X face 603 glNormal3f(-1,0,0) 604 glVertex3f(x1, y1, z1) 605 glVertex3f(x1, y1, z2) 606 glVertex3f(x1, y2, z2) 607 glVertex3f(x1, y2, z1) 608 # top face 609 glNormal3f(0,0,1) 610 glVertex3f(x1, y2, z2) 611 glVertex3f(x1, y1, z2) 612 glVertex3f(x2, y1, z2) 613 glVertex3f(x2, y2, z2) 614 glEnd() 615 616 def volume(self): 617 x1, y1, z1, x2, y2, z2 = self.coords() 618 vol = abs((x1-x2)*(y1-y2)*(z1-z2)) 619 #print "Box.volume", vol 620 return vol 621 622 623 # specify the width in X and Y, and the height in Z 624 # the box is centered on the origin 625 class BoxCentered(Box): 626 def __init__(self, xw, yw, zw): 627 Box.__init__(self, -xw/2.0, -yw/2.0, -zw/2.0, xw/2.0, yw/2.0, zw/2.0) 628 629 # specify the width in X and Y, and the height in Z 630 # the box is centered in X and Y, and runs from Z=0 up 631 # (or down) to the specified Z value 632 class BoxCenteredXY(Box): 633 def __init__(self, xw, yw, zw): 634 Box.__init__(self, -xw/2.0, -yw/2.0, 0, xw/2.0, yw/2.0, zw) 635 636 # capture current transformation matrix 637 # note that this tranforms from the current coordinate system 638 # to the viewport system, NOT to the world system 639 class Capture(object): 640 def __init__(self): 641 self.t = [] 642 643 def capture(self): 644 self.t = glGetDoublev(GL_MODELVIEW_MATRIX) 645 646 def volume(self): 647 return 0.0 648 649 # function to invert a transform matrix 650 # based on http://steve.hollasch.net/cgindex/math/matrix/afforthinv.c 651 # with simplifications since we don't do scaling 652 653 # This function inverts a 4x4 matrix that is affine and orthogonal. In 654 # other words, the perspective components are [0 0 0 1], and the basis 655 # vectors are orthogonal to each other. In addition, the matrix must 656 # not do scaling 657 658 def invert(src): 659 # make a copy 660 inv=src[:] 661 # The inverse of the upper 3x3 is the transpose (since the basis 662 # vectors are orthogonal to each other. 663 inv[1],inv[4] = inv[4],inv[1] 664 inv[2],inv[8] = inv[8],inv[2] 665 inv[6],inv[9] = inv[9],inv[6] 666 # The inverse of the translation component is just the negation 667 # of the translation after dotting with the new upper3x3 rows. */ 668 inv[12] = -(src[12]*inv[0] + src[13]*inv[4] + src[14]*inv[8]) 669 inv[13] = -(src[12]*inv[1] + src[13]*inv[5] + src[14]*inv[9]) 670 inv[14] = -(src[12]*inv[2] + src[13]*inv[6] + src[14]*inv[10]) 671 return inv 672 673 class Hud(object): 674 '''head up display - draws a semi-transparent text box. 675 use HUD.strs for things that must be updated constantly, 676 and HUD.show("stuff") for one-shot things like error messages''' 677 def __init__(self, showme=1): 678 self.app = [] 679 self.strs = [] 680 self.messages = [] 681 self.showme = 0 682 683 def show(self, string="xyzzy"): 684 self.showme = 1 685 if string != "xyzzy": 686 self.messages += [str(string)] 687 688 def hide(self): 689 self.showme = 0 690 691 def clear(self): 692 self.messages = [] 693 694 def draw(self): 695 drawtext = self.strs + self.messages 696 self.lines = len(drawtext) 697 #draw head-up-display 698 #see axis.py for more font/color configurability 699 if ((self.showme == 0) or (self.lines == 0)): 700 return 701 702 glMatrixMode(GL_PROJECTION) 703 glPushMatrix() 704 glLoadIdentity() 705 706 #pointer to font? 707 fontbase = int(self.app.loadbitmapfont("9x15")) 708 char_width, char_height = 9, 15 709 xmargin,ymargin = 5,5 710 ypos = float(self.app.winfo_height()) 711 712 glOrtho(0.0, self.app.winfo_width(), 0.0, ypos, -1.0, 1.0) 713 glMatrixMode(GL_MODELVIEW) 714 glPushMatrix() 715 glLoadIdentity() 716 717 #draw the text box 718 maxlen = max([len(p) for p in drawtext]) 719 box_width = maxlen * char_width 720 glDepthFunc(GL_ALWAYS) 721 glDepthMask(GL_FALSE) 722 glDisable(GL_LIGHTING) 723 glEnable(GL_BLEND) 724 glEnable(GL_NORMALIZE) 725 glBlendFunc(GL_ONE, GL_CONSTANT_ALPHA) 726 glColor3f(0.2,0,0) 727 glBlendColor(0,0,0,0.5) #rgba 728 glBegin(GL_QUADS) 729 glVertex3f(0, ypos, 1) #upper left 730 glVertex3f(0, ypos - 2*ymargin - char_height*len(drawtext), 1) #lower left 731 glVertex3f(box_width+2*xmargin, ypos - 2*ymargin - char_height*len(drawtext), 1) #lower right 732 glVertex3f(box_width+2*xmargin, ypos , 1) #upper right 733 glEnd() 734 glDisable(GL_BLEND) 735 glEnable(GL_LIGHTING) 736 737 #fill the box with text 738 maxlen = 0 739 ypos -= char_height+ymargin 740 i=0 741 glDisable(GL_LIGHTING) 742 glColor3f(0.9,0.9,0.9) 743 for string in drawtext: 744 maxlen = max(maxlen, len(string)) 745 # if i < len(homed) and homed[i]: 746 # glRasterPos2i(6, ypos) 747 # glBitmap(13, 16, 0, 3, 17, 0, homeicon) 748 glRasterPos2i(xmargin, int(ypos)) 749 for char in string: 750 glCallList(fontbase + ord(char)) 751 # if i < len(homed) and limit[i]: 752 # glBitmap(13, 16, -5, 3, 17, 0, limiticon) 753 ypos -= char_height 754 i = i + 1 755 glDepthFunc(GL_LESS) 756 glDepthMask(GL_TRUE) 757 glEnable(GL_LIGHTING) 758 759 glPopMatrix() 760 glMatrixMode(GL_PROJECTION) 761 glPopMatrix() 762 glMatrixMode(GL_MODELVIEW) 763 764 765 class O(rs274.OpenGLTk.Opengl): 766 def __init__(self, *args, **kw): 767 rs274.OpenGLTk.Opengl.__init__(self, *args, **kw) 768 self.r_back = self.g_back = self.b_back = 0 769 #self.q1 = gluNewQuadric() 770 #self.q2 = gluNewQuadric() 771 #self.q3 = gluNewQuadric() 772 self.plotdata = [] 773 self.plotlen = 16000 774 #does not show HUD by default 775 self.hud = Hud() 776 777 def basic_lighting(self): 778 self.activate() 779 glLightfv(GL_LIGHT0, GL_POSITION, (1, -1, .5, 0)) 780 glLightfv(GL_LIGHT0, GL_AMBIENT, (.2,.2,.2,0)) 781 glLightfv(GL_LIGHT0, GL_DIFFUSE, (.6,.6,.4,0)) 782 glLightfv(GL_LIGHT0+1, GL_POSITION, (-1, -1, .5, 0)) 783 glLightfv(GL_LIGHT0+1, GL_AMBIENT, (.0,.0,.0,0)) 784 glLightfv(GL_LIGHT0+1, GL_DIFFUSE, (.0,.0,.4,0)) 785 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, (1,1,1,0)) 786 glDisable(GL_CULL_FACE) 787 glEnable(GL_LIGHTING) 788 glEnable(GL_LIGHT0) 789 glEnable(GL_LIGHT0+1) 790 glDepthFunc(GL_LESS) 791 glEnable(GL_DEPTH_TEST) 792 glMatrixMode(GL_MODELVIEW) 793 glLoadIdentity() 794 795 796 797 798 def redraw(self, *args): 799 if self.winfo_width() == 1: return 800 self.model.traverse() 801 # current coords: world 802 # the matrices tool2view, work2view, and world2view 803 # transform from tool/work/world coords to viewport coords 804 # if we want to draw in tool coords, we need to do 805 # "tool -> view -> world" (since the current frame is world) 806 # and if we want to draw in work coords, we need 807 # "work -> view -> world". For both, we need to invert 808 # the world2view matrix to do the second step 809 view2world = invert(self.world2view.t) 810 # likewise, for backplot, we want to transform the tooltip 811 # position from tool coords (where it is [0,0,0]) to work 812 # coords, so we need tool -> view -> work 813 # so lets also invert the work2view matrix 814 view2work = invert(self.work2view.t) 815 816 # since backplot lines only need vertexes, not orientation, 817 # and the tooltip is at the origin, getting the tool coords 818 # is easy 819 tx, ty, tz = self.tool2view.t[12:15] 820 # now we have to transform them to the work frame 821 wx = tx*view2work[0]+ty*view2work[4]+tz*view2work[8]+view2work[12] 822 wy = tx*view2work[1]+ty*view2work[5]+tz*view2work[9]+view2work[13] 823 wz = tx*view2work[2]+ty*view2work[6]+tz*view2work[10]+view2work[14] 824 # wx, wy, wz are the values to use for backplot 825 # so we save them in a buffer 826 if len(self.plotdata) == self.plotlen: 827 del self.plotdata[:self.plotlen / 10] 828 point = [ wx, wy, wz ] 829 if not self.plotdata or point != self.plotdata[-1]: 830 self.plotdata.append(point) 831 832 # now lets draw something in the tool coordinate system 833 #glPushMatrix() 834 # matrixes take effect in reverse order, so the next 835 # two lines do "tool -> view -> world" 836 #glMultMatrixd(view2world) 837 #glMultMatrixd(self.tool2view.t) 838 839 # do drawing here 840 # cylinder normally goes to +Z, we want it down 841 #glTranslatef(0,0,-60) 842 #gluCylinder(self.q1, 20, 20, 60, 32, 16) 843 844 # back to world coords 845 #glPopMatrix() 846 847 848 # we can also draw in the work coord system 849 glPushMatrix() 850 # "work -> view -> world" 851 glMultMatrixd(view2world) 852 glMultMatrixd(self.work2view.t) 853 # now we can draw in work coords, and whatever we draw 854 # will move with the work, (if the work is attached to 855 # a table or indexer or something that moves with 856 # respect to the world 857 858 # just a test object, sitting on the table 859 #gluCylinder(self.q2, 40, 20, 60, 32, 16) 860 861 #draw head up display 862 if(hasattr(self.hud, "draw")): 863 self.hud.draw() 864 865 # draw backplot 866 glDisable(GL_LIGHTING) 867 glLineWidth(2) 868 glColor3f(1.0,0.5,0.5) 869 870 glBegin(GL_LINE_STRIP) 871 for p in self.plotdata: 872 glVertex3f(*p) 873 glEnd() 874 875 glEnable(GL_LIGHTING) 876 glColor3f(1,1,1) 877 glLineWidth(1) 878 glDisable(GL_BLEND) 879 glDepthFunc(GL_LESS) 880 881 # back to world again 882 glPopMatrix() 883 884 def plotclear(self): 885 del self.plotdata[:self.plotlen] 886 887 class Color(Collection): 888 def __init__(self, color, parts): 889 self.color = color 890 Collection.__init__(self, parts) 891 892 def apply(self): 893 glPushAttrib(GL_LIGHTING_BIT) 894 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, self.color) 895 896 def unapply(self): 897 glPopAttrib() 898 899 class AsciiSTL: 900 def __init__(self, filename=None, data=None): 901 if data is None: 902 data = open(filename, "r") 903 elif isinstance(data, str): 904 data = data.split("\n") 905 self.list = None 906 t = [] 907 n = [0,0,0] 908 self.d = d = [] 909 for line in data: 910 if line.find("normal") != -1: 911 line = line.split() 912 x, y, z = map(float, line[-3:]) 913 n = [x,y,z] 914 elif line.find("vertex") != -1: 915 line = line.split() 916 x, y, z = map(float, line[-3:]) 917 t.append([x,y,z]) 918 if len(t) == 3: 919 if n == [0,0,0]: 920 dx1 = t[1][0] - t[0][0] 921 dy1 = t[1][1] - t[0][1] 922 dz1 = t[1][2] - t[0][2] 923 dx2 = t[2][0] - t[0][0] 924 dy2 = t[2][1] - t[0][1] 925 dz2 = t[2][2] - t[0][2] 926 n = [dy1*dz2 - dy2*dz1, dz1*dx2 - dz2*dx1, dy1*dx2 - dy2*dx1] 927 d.append((n, t)) 928 t = [] 929 n = [0,0,0] 930 931 def draw(self): 932 if self.list is None: 933 # OpenGL isn't ready yet in __init__ so the display list 934 # is created during the first draw 935 self.list = glGenLists(1) 936 glNewList(self.list, GL_COMPILE) 937 glBegin(GL_TRIANGLES) 938 for n, t in self.d: 939 glNormal3f(*n) 940 glVertex3f(*t[0]) 941 glVertex3f(*t[1]) 942 glVertex3f(*t[2]) 943 glEnd() 944 glEndList() 945 del self.d 946 glCallList(self.list) 947 948 class AsciiOBJ: 949 def __init__(self, filename=None, data=None): 950 if data is None: 951 data = open(filename, "r") 952 elif isinstance(data, str): 953 data = data.split("\n") 954 955 self.v = v = [] 956 self.vn = vn = [] 957 self.f = f = [] 958 for line in data: 959 if line.startswith("#"): continue 960 if line.startswith("vn"): 961 vn.append([float(w) for w in line.split()[1:]]) 962 elif line.startswith("v"): 963 v.append([float(w) for w in line.split()[1:]]) 964 elif line.startswith("f"): 965 f.append(self.parse_face(line)) 966 967 # print v[:5] 968 # print vn[:5] 969 # print f[:5] 970 971 self.list = None 972 973 974 def parse_int(self, i): 975 if i == '': return None 976 return int(i) 977 978 def parse_slash(self, word): 979 return [self.parse_int(i) for i in word.split("/")] 980 981 def parse_face(self, line): 982 return [self.parse_slash(w) for w in line.split()[1:]] 983 984 def draw(self): 985 if self.list is None: 986 # OpenGL isn't ready yet in __init__ so the display list 987 # is created during the first draw 988 self.list = glGenLists(1) 989 glNewList(self.list, GL_COMPILE) 990 glDisable(GL_CULL_FACE) 991 glBegin(GL_TRIANGLES) 992 #print "obj", len(self.f) 993 for f in self.f: 994 for v, t, n in f: 995 if n: 996 glNormal3f(*self.vn[n-1]) 997 glVertex3f(*self.v[v-1]) 998 glEnd() 999 glEndList() 1000 del self.v 1001 del self.vn 1002 del self.f 1003 glCallList(self.list) 1004 1005 1006 old_plotclear = False 1007 1008 def main(model, tool, work, size=10, hud=0, rotation_vectors=None, lat=0, lon=0): 1009 app = Tkinter.Tk() 1010 1011 t = O(app, double=1, depth=1) 1012 # set which axes to rotate around 1013 if rotation_vectors: t.rotation_vectors = rotation_vectors 1014 # we want to be able to see the model from all angles 1015 t.set_latitudelimits(-180, 180) 1016 # set starting viewpoint if desired 1017 t.after(100, lambda: t.set_viewangle(lat, lon)) 1018 1019 vcomp = hal.component("vismach") 1020 vcomp.newpin("plotclear",hal.HAL_BIT,hal.HAL_IN) 1021 vcomp.ready() 1022 1023 #there's probably a better way of doing this 1024 global HUD 1025 HUD = 0 1026 if(hud != 0 and hasattr(hud, "app")): 1027 HUD = hud 1028 #point our app at the global 1029 t.hud = HUD 1030 1031 t.hud.app = t #HUD needs to know where to draw 1032 1033 # need to capture the world coordinate system 1034 world = Capture() 1035 1036 t.model = Collection([model, world]) 1037 t.distance = size * 3 1038 t.near = size * 0.01 1039 t.far = size * 10.0 1040 t.tool2view = tool 1041 t.world2view = world 1042 t.work2view = work 1043 1044 t.pack(fill="both", expand=1) 1045 1046 def update(): 1047 global old_plotclear 1048 t.tkRedraw() 1049 new_plotclear = vcomp["plotclear"] 1050 if new_plotclear and not old_plotclear: 1051 t.plotclear() 1052 old_plotclear=new_plotclear 1053 t.after(100, update) 1054 update() 1055 1056 def quit(*args): 1057 raise SystemExit 1058 1059 signal.signal(signal.SIGTERM, quit) 1060 signal.signal(signal.SIGINT, quit) 1061 1062 app.mainloop()