Example #1
0
 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)
Example #3
0
 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
Example #4
0
    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()
Example #5
0
def transformPoint( point, matrix ):
    v = vec4(point.x, point.y, point.z, 1 )
    r = matrix*v
    return vec3( r.x, r.y, r.z )
Example #6
0
def MatrixFrontVector( aMatrix ):
	return vec4(aMatrix.transpose().getColumn( 0 ))
Example #7
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)
Example #8
0
def transformPoint(point, matrix):
    v = vec4(point.x, point.y, point.z, 1)
    r = matrix * v
    return vec3(r.x, r.y, r.z)
Example #9
0
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)
Example #11
0
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)
Example #12
0
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('')