def _configureGeom(self): self._loadGeom() color = self.cfg['color'] light = vec4(color[0] / 255.0, color[1] / 255.0, color[2] / 255.0, 1.0) material = GLMaterial(ambient=light, diffuse=light) self.child('Box').setMaterial(material, 0) self.setMaterial(material, 0)
def setUp(self): a = 0, 1.6, 2, 3, 4.977, 5, 6, .007, 8, 0, 1.6, 2, 3, 4.977, 5, 6 b = 0.000921, 10, 20.5, -34.5 self.cym = cycg.mat4(*a) self.cgm = cg.mat4(*a) self.cyvec4 = cycg.vec4(*b) self.cgvec4 = cg.vec4(*b) self.cyvec3 = cycg.vec3(2, -4, .8) self.cgvec3 = cg.vec3(2, -4, .8)
def _createLight(self, objName, objData): radius = objData['radius'] pos = vec3(objData['position']) color = objData.get('color') if color is not None: light = vec4(color[0] / 255.0, color[1] / 255.0, color[2] / 255.0, 1.0) material = GLMaterial(ambient=light, diffuse=light) else: material = None obj = Sphere(name=objName, radius=radius, pos=pos, dynamics=False, material=material) logger.info("using sphere radius: %s" % radius) logger.info("using sphere pos: %s" % pos) return obj
def renderGeoms(self): log.debug('rendering %d geoms', self.sim.space.getNumGeoms()) if self.wireframe: gluQuadricDrawStyle(self.quadratic, GLU_SILHOUETTE) else: gluQuadricDrawStyle(self.quadratic, GLU_FILL) for geom in self.sim.space: glPushMatrix() if type(geom) is ode.GeomSphere: log.debug('draw sphere') glColor(0, 0, 1) x, y, z = geom.getPosition() glTranslate(x, y, z) glutSolidSphere(geom.getRadius(), 20, 20) elif type(geom) is ode.GeomPlane: pass elif type(geom) is ode.GeomBox: log.debug('draw box(%s) @(%s)', str(geom.getLengths()), str(geom.getPosition())) glColor(0.8, 0, 0) x, y, z = geom.getPosition() # create openGL 4x4 transform matrix from ODE 3x3 rotation matrix R = geom.getRotation() log.debug('ROTATE = %s', str(R)) # R is a 3x3 matrix T = mat4() T.setMat3(mat3(R)) T.setColumn(3, vec4(x, y, z, 1.0)) glMultMatrixd(T.toList()) (sx, sy, sz) = geom.getLengths() log.debug('size (%f,%f,%f)', sx, sy, sz) glScale(sx, sy, sz) if self.wireframe: glutWireCube(1) else: glutSolidCube(1) elif type(geom) is ode.GeomCCylinder: log.debug('draw ccylinder') def red(): glColor(1, 0, 0) def green(): glColor(0, 1, 0) def blue(): glColor(0, 0, 1) def plot_axes(ax, ay, az): for axis, colour in (ax, red), (ay, blue), (az, green): if axis: x, y, z = axis colour() glBegin(GL_LINES) glVertex(0, 0, 0) glVertex(x * 5, y * 5, z * 5) glEnd() if self.render_axes and hasattr(geom, 'motor'): glPushMatrix() glDisable(GL_LIGHTING) m = geom.motor x, y, z = m.joint.getAnchor() glTranslate(x, y, z) ax = None ay = None az = None if isinstance(m.joint, ode.HingeJoint): ax = m.joint.getAxis() elif isinstance(m.joint, ode.UniversalJoint): ax = m.joint.getAxis1() ay = m.joint.getAxis2() plot_axes(ax, ay, az) # plot motor axes # these should be aligned with the joint axes above ax, ay, az = m.getAxis(0), m.getAxis(1), m.getAxis(2) plot_axes(ax, ay, az) glEnable(GL_LIGHTING) glPopMatrix() # construct transformation matrix from position and rotation x, y, z = geom.getPosition() rotmat = mat3(geom.getRotation()) log.debug('r=%s', geom.getRotation()) # ode ccylinders are aligned along z axis by default T = mat4() T.setMat3(rotmat) T.setColumn(3, vec4(x, y, z, 1.0)) log.debug('geom matrix T is %s', str(T)) glMultMatrixd(T.toList()) (radius, length) = geom.getParams() log.debug('geom len=%f xyz=%f,%f,%f', length, x, y, z) # plot the geom self.plotAxes() if self.render_bps: glTranslate(0, 0, -length / 2) b = 0.8 if not geom.parent: b = 0.0 glColor(0, 0, b) gluCylinder(self.quadratic, radius, radius, length, 16, 16) if geom.left == 'internal': glColor(0, 1, 0) else: glColor(1, 0, 0) gluSphere(self.quadratic, radius, 10, 10) glTranslate(0, 0, length) if geom.right == 'internal': glColor(0, 1, 0) else: glColor(1, 0, 0) gluSphere(self.quadratic, radius, 10, 10) else: log.critical('dont know how to render geom %s', str(geom)) glPopMatrix()
def transformPoint( point, matrix ): v = vec4(point.x, point.y, point.z, 1 ) r = matrix*v return vec3( r.x, r.y, r.z )
def MatrixFrontVector( aMatrix ): return vec4(aMatrix.transpose().getColumn( 0 ))
def run(): f_fts = open(sys.argv[1], 'r') f_tfs = open(sys.argv[2], 'r') fn_output = sys.argv[3] DL = '\t' tfs = [] tfs_file = "" for l in f_tfs: s = l.split(DL) if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1] if len(s) > 0 and s[0] == "tf": tfs.append(s[1:]) if len(s) > 0 and s[0] == "file": tfs_file = s[1] fts_file = "" T = None keypoints = [] KPstart = 0 frames = 0 took = {} for l in f_fts: s = l.split(DL) if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1] if len(s) > 0 and s[0] == "file": fts_file = s[1] if len(s) > 0 and s[0] == "header": T = None ts = float(s[1]) best = None mi = 0.075 #minimum for t in tfs: if abs(float(t[0]) - ts) < mi: mi = abs(float(t[0]) - ts) best = t if best == None: #print "skipping frame as no valid tf was found" continue #print mi T = [ vec4(float(best[1]), float(best[2]), float(best[3]), 1.), quat(float(v) for v in best[4:]) ] KPstart = len(keypoints) KPtype = "" frames += 1 if len(s) > 0 and s[0] == "took": if not s[1] in took: took[s[1]] = [] took[s[1]].append(reduce(lambda x, y: float(x) + float(y), s[2:])) if len(s) == 4 and s[0] == "keypoint" and T != None: pt = vec4(float(s[1]), float(s[2]), float(s[3]), 1.) TT = T[1].toMat4().setColumn(3, T[0]) #pt = TT.inverse()*pt pt = TT * pt keypoints.append({"pt": pt}) #print pt if len(s) > 4 and s[0] == "keypoint" and T != None: if KPtype != s[1]: KPtype = s[1] KP = KPstart keypoints[KP][s[1]] = [float(v) for v in s[2:]] KP += 1 print "read input file ", len(keypoints) f_out = file(fn_output + "_timing.csv", "w") i = 1 for ft in took: aa, bb, cc = mquantiles(took[ft]) f_out.write(str(i) + "\t") f_out.write(str(min(took[ft])) + "\t") f_out.write(str(aa) + "\t") f_out.write(str(bb) + "\t") f_out.write(str(cc) + "\t") f_out.write(str(max(took[ft])) + "\t") f_out.write( str(reduce(lambda x, y: x + y, took[ft]) / len(took[ft])) + "\t") f_out.write(ft) f_out.write("\n") i += 1 f_out.close() print "written timing output" borders = [x / 20. for x in range(1, 31)] avg = {} fts = [] for i in xrange(len(keypoints)): if len(keypoints) > 1000: if (i) % (len(keypoints) / 1000) == 0: print "#", sys.stdout.flush() #if i==3: exit() kp1 = keypoints[i] for j in range(i + 1, len(keypoints)): kp2 = keypoints[j] L = (kp1["pt"] - kp2["pt"]).length() #print L,"\t", for ft in kp1: if ft == "pt" or not ft in kp2: continue #D=dist(kp1[ft], kp2[ft], ft) #if D==0: # #print i,j, kp1["pt"], kp2["pt"] # if i+1==j: continue #print D,"\t", if not ft in avg: avg[ft] = 0. avg[ft + "num"] = 0 avg[ft + "b"] = 0. avg[ft + "bnum"] = 0 #avg[ft+"ar"]=[] #avg[ft+"bar"]=[] avg[ft + "border"] = [[] for x in xrange(len(borders))] #avg[ft+"bborder"]=[[] for x in xrange(len(borders))] fts.append(ft) #p="" #if L>0.7/3: p="b" #avg[ft+p]+=D #avg[ft+p+"num"]+=1 #avg[ft+p+"ar"].append(D) D = False for b in range(int(L / 0.05), len(borders)): #for b in xrange(len(borders)): pp = "" if L > borders[b]: continue #pp="b" if D == False: D = dist(kp1[ft], kp2[ft], ft) avg[ft + pp + "border"][b].append(D) #print "" print "frames ", frames for ft in fts: #if avg[ft+"bnum"]>0 and avg[ft+"bnum"] and avg[ft]>0: # print ft, avg[ft+"num"], avg[ft+"bnum"], avg[ft]/avg[ft+"num"], " ", avg[ft+"b"]/avg[ft+"bnum"], " -> ", (avg[ft+"b"]/avg[ft+"bnum"])/(avg[ft]/avg[ft+"num"]) #else: # print ft," is emtpy" print ft #print min(avg[ft+"ar"]), mquantiles(avg[ft+"ar"]), max(avg[ft+"ar"]) #print min(avg[ft+"bar"]), mquantiles(avg[ft+"bar"]), max(avg[ft+"bar"]) f_out = file(fn_output + "_" + ft + ".csv", "w") aa, bb, cc = mquantiles(avg[ft + "border"][len(borders) - 1]) FACT = 1 / bb for pp in [""]: #["", "b"]: for b in xrange(len(borders)): if len(avg[ft + pp + "border"][b]) < 1: continue f_out.write(str(borders[b]) + "\t") aa, bb, cc = mquantiles(avg[ft + pp + "border"][b]) f_out.write(str(min(avg[ft + pp + "border"][b]) * FACT) + "\t") f_out.write(str(aa * FACT) + "\t") f_out.write(str(bb * FACT) + "\t") f_out.write(str(cc * FACT) + "\t") f_out.write(str(max(avg[ft + pp + "border"][b]) * FACT) + "\t") f_out.write( str( reduce(lambda x, y: x + y, avg[ft + pp + "border"][b]) / len(avg[ft + pp + "border"][b]) * FACT)) f_out.write("\n") #print "" f_out.close() assert (tfs_file == fts_file)
def transformPoint(point, matrix): v = vec4(point.x, point.y, point.z, 1) r = matrix * v return vec3(r.x, r.y, r.z)
def MatrixFrontVector(aMatrix): return vec4(aMatrix.transpose().getColumn(0))
def run(): f_fts = open(sys.argv[1], 'r') f_tfs = open(sys.argv[2], 'r') fn_output = sys.argv[3] DL = '\t' tfs = [] tfs_file = "" for l in f_tfs: s = l.split(DL) if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1] if len(s) > 0 and s[0] == "tf": tfs.append(s[1:]) if len(s) > 0 and s[0] == "file": tfs_file = s[1] fts_file = "" T = None frames = [] took = {} radius = 0 for l in f_fts: s = l.split(DL) if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1] if len(s) > 0 and s[0] == "file": fts_file = s[1] if len(s) > 0 and s[0] == "header": T = None ts = float(s[1]) radius = None best = None mi = 0.075 #minimum for t in tfs: if abs(float(t[0]) - ts) < mi: mi = abs(float(t[0]) - ts) best = t if best == None: #print "skipping frame as no valid tf was found" continue T = [ vec4(float(best[1]), float(best[2]), float(best[3]), 1.), quat(float(v) for v in best[4:]) ] frames.append({}) elif radius == None: radius = float(s[1]) if len(s) > 0 and s[0] == "took": name = '_'.join(s[1:4]) if not name in took: took[name] = [] if not name in frames[len(frames) - 1]: frames[len(frames) - 1][name] = [] took[name].append( float(reduce(lambda x, y: float(x) + float(y), s[4:]))) if len(s) == 7 and s[0] == "eval_keypoint" and T != None: name = '_'.join(s[1:4]) pt = vec4(float(s[4]), float(s[5]), float(s[6]), 1.) TT = T[1].toMat4().setColumn(3, T[0]) #pt = TT.inverse()*pt pt = TT * pt frames[len(frames) - 1][name].append({"pt": pt}) print "read input file " f_out = file(fn_output + "_timing.csv", "w") i = 1 for ft in took: aa, bb, cc = mquantiles(took[ft]) f_out.write(str(i) + "\t") f_out.write(str(min(took[ft])) + "\t") f_out.write(str(aa) + "\t") f_out.write(str(bb) + "\t") f_out.write(str(cc) + "\t") f_out.write(str(max(took[ft])) + "\t") f_out.write( str(reduce(lambda x, y: x + y, took[ft]) / len(took[ft])) + "\t") f_out.write(ft) f_out.write("\n") i += 1 f_out.close() print "written timing output" print "frames ", len(frames) names = [n for n in frames[0]] f_out = file(fn_output + ".csv", "a") for name in names: print name dist = [] gtp = 0 gfp = 0 ges = 0 for i in xrange(len(frames)): ges += len(frames[i][name]) for ii in xrange(len(frames[i][name])): kp1 = frames[i][name][ii] for j in range(i + 1, len(frames)): tp = 0 fp = 0 mi = radius for jj in xrange(len(frames[j][name])): kp2 = frames[j][name][jj] L2 = (kp1["pt"] - kp2["pt"]).length() if L2 <= radius / 4: if tp > 0: fp += 1 else: tp += 1 mi = min(mi, L2) if tp > 0: dist.append(mi) gtp += tp gfp += fp print "" aa, bb, cc = mquantiles(dist) f_out.write(str(name) + "\t") f_out.write(str(gtp) + "\t") f_out.write(str(gfp) + "\t") f_out.write(str(ges) + "\t") f_out.write(str(len(frames)) + "\t") f_out.write(str(min(dist)) + "\t") f_out.write(str(aa) + "\t") f_out.write(str(bb) + "\t") f_out.write(str(cc) + "\t") f_out.write(str(max(dist)) + "\t") f_out.write(str(reduce(lambda x, y: x + y, dist) / len(dist)) + "\t") f_out.write(str(max(dist)) + "\n") f_out.flush() assert (tfs_file == fts_file)
def run(): global num_radii, num_angles f_fts = open(sys.argv[1], 'r') f_tfs = open(sys.argv[2], 'r') fn_output = sys.argv[3] DL = '\t' tfs = [] tfs_file = "" for l in f_tfs: s = l.split(DL) if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1] if len(s) > 0 and s[0] == "tf": tfs.append(s[1:]) if len(s) > 0 and s[0] == "file": tfs_file = s[1] fts_file = "" T = None keypoints = [] KPstart = 0 frames = 0 took = {} for l in f_fts: s = l.split(DL) if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1] if len(s) > 0 and s[0] == "file": fts_file = s[1] if len(s) > 0 and s[0] == "num_radii": num_radii = int(s[1]) if len(s) > 0 and s[0] == "num_angles": num_angles = int(s[1]) if len(s) > 0 and s[0] == "header": T = None ts = float(s[1]) best = None mi = 0.075 #minimum for t in tfs: if abs(float(t[0]) - ts) < mi: mi = abs(float(t[0]) - ts) best = t if best == None: #print "skipping frame as no valid tf was found" continue #print mi T = [ vec4(float(best[1]), float(best[2]), float(best[3]), 1.), quat(float(v) for v in best[4:]) ] KPstart = len(keypoints) KPtype = "" frames += 1 if len(s) > 0 and s[0] == "took": if not s[1] in took: took[s[1]] = [] took[s[1]].append(reduce(lambda x, y: float(x) + float(y), s[2:])) if len(s) == 4 and s[0] == "keypoint" and T != None: pt = vec4(float(s[1]), float(s[2]), float(s[3]), 1.) TT = T[1].toMat4().setColumn(3, T[0]) #pt = TT.inverse()*pt pt = TT * pt keypoints.append({"pt": pt}) #print pt if len(s) > 4 and s[0] == "keypoint" and T != None: if KPtype != s[1]: KPtype = s[1] KP = KPstart keypoints[KP][s[1]] = [float(v) for v in s[2:]] KP += 1 print "read input file ", len(keypoints) borders = [x / 20. for x in range(1, 31)] avg = {} avgDBG = {} for i in xrange(len(keypoints)): if len(keypoints) > 1000: if (i) % (len(keypoints) / 1000) == 0: print "#", sys.stdout.flush() if i > 10: break #if i==3: exit() fts = [] md = {} mL = {} mF = {} mdDBG = {} mLDBG = {} mFDBG = {} kp1 = keypoints[i] NNNN = 0 for j in range(i + 1, len(keypoints)): kp2 = keypoints[j] L = (kp1["pt"] - kp2["pt"]).length() #print L,"\t", for ft in kp1: if ft == "pt" or not ft in kp2: continue d = dist(kp1[ft], kp2[ft], ft) if not ft in md: md[ft] = None mL[ft] = None mdDBG[ft] = None mLDBG[ft] = None fts.append(ft) if md[ft] == None or d < md[ft]: md[ft] = d mL[ft] = L mF[ft] = [kp1[ft], kp2[ft]] if mdDBG[ft] == None or L < mLDBG[ft]: mdDBG[ft] = d mLDBG[ft] = L mFDBG[ft] = [kp1[ft], kp2[ft]] '''for ft in kp1: if ft=="pt" or not ft in kp2: continue d=dist(kp1[ft], kp2[ft], ft) if d<=mdDBG[ft]: NNNN+=1 print L''' for ft in fts: if md[ft] == None: continue if not ft in avg: avg[ft] = [] avgDBG[ft] = [] avg[ft].append([md[ft], mL[ft], mF[ft]]) avgDBG[ft].append([mdDBG[ft], mLDBG[ft], mFDBG[ft]]) print ft, NNNN print md[ft], mL[ft] print mdDBG[ft], mLDBG[ft] print mFDBG[ft][0] print mFDBG[ft][1] print mF[ft][1] print "frames ", frames print avg print avgDBG for b in xrange(len(borders)): for ft in avg: f1 = file(fn_output + "_" + ft + "_pr_" + str(borders[b]) + ".csv", "w") R = calc_pr(avg[ft], borders[b]) for r in R: f1.write(str(r[0])) for i in range(1, len(r)): f1.write("\t" + str(r[i])) f1.write("\n") f1.close() assert (tfs_file == fts_file)
print('# ---- det ----') for i in range(0, LEN): a = mat4(*mat4s[i]) print('# {0}'.format(a.determinant())) print('# ---- det end ----') print('') print('# ---- inv ----') for i in range(0, LEN): a = mat4(*mat4s[i]) inv = a.inverse() try: pr(inv) except: print('# 0') print('# ---- inv end ----') print('') print('# ---- mul vec ----') for i in range(0, LEN): a = mat4(*mat4s[i]) v = vec4(mat4s[i][1], mat4s[i][2], mat4s[i][3], mat4s[i][4]) w = a * v u = vec4(mat4s[i][3], mat4s[i][4], mat4s[i][5], 1) u = a * u u = u / u.w print('# vec4 {0} {1} {2} {3}'.format(w.x, w.y, w.z, w.w)) print('# vec3 {0} {1} {2}'.format(u.x, u.y, u.z)) print('# ---- mul vec end ----') print('')