#!/usr/bin/python import math REAL_X=200 # mm REAL_Y=200 # mm ''' assumptions: - kamera jest pod katem 90deg do podkladki (linie nie sa przemieszane) #- 0-1 punkt to _KROTKA_ krawedz - obrot zgodnie z ruchem wskazowek zegara - wiimote sa poziomo (upside down) y up ^ ______ | /1 2\ | left | | right | \0______3/ | z |/ down +----------------------->x [0,1,2,3] - down [1,2,3,0] - left [3,0,1,2] - right [2,3,0,1] - up ''' from wiicountmacros import * transformation={ 0: 'down', 1: 'left', 2: 'up', 3: 'right', } class Camera: transformation = None thepoints = None min_tp = None max_tp = None real_distance = None see_x_axis = None midpoint = None def __init__(self, thepoints): self.thepoints = thepoints dists = dict( ( (dist(0.0,1.0,a,b), i) for i,(a,b,_) in enumerate(thepoints) ) ) downleft = dists[ sorted(dists.keys())[0] ] self.data = shift(map(lambda (a,b,_):(a,b),thepoints), downleft) self.pos = transformation[downleft] left = getline( *(self.data[0] + self.data[1]) ) right = getline( *(self.data[2] + self.data[3]) ) up = getline( *(self.data[1] + self.data[2]) ) down = getline( *(self.data[0] + self.data[3]) ) infzx=getpoint(*(left+right)) infzy=getpoint(*(up+down)) z1, x = 41.0 * (infzx[0] - 0.5),31.0 * (infzx[1] - 0.5) # .. -360...+360 z2, y = 41.0 * (infzy[0] - 0.5),31.0 * (infzy[1] - 0.5) print "inf: x=%6.1f y=%6.1f z=%6.1f" % (x,y,z1) self.alpha = x self.beta = y self.gamma = z1 def shorten(l, a): return math.cos(math.radians(z1))*float(l) #return l*((90.0 - abs(a))/90.0) def tozet(bok, gamma): return math.sqrt(2.0)* (bok/2.0) * math.sin(math.radians(45.0+gamma)) l = shorten(REAL_X, z1) zzz = tozet(REAL_X, z1) alp = (41.0)*( dist( *(self.data[0]+self.data[3]))) # good enough d1 = (l/2.0) / math.tan( math.radians( alp/2.0 ) ) +zzz alp = (41.0)*( dist( *(self.data[1]+self.data[2]))) # good enough d2 = (l/2.0) / math.tan( math.radians( alp/2.0 ) ) -zzz print "gamma=%f l=%f z=%f dist=%f %f (%fmm)" % (z1, l,zzz, d1, d2, abs(d1-d2)) b = (math.tan(math.radians(self.gamma)) * d1) self.coord = (b,d1) self.l = l self.k = zzz def get_b(self, a, b): x1,y1 = self.coord a, b = propointmap(*(self.data[0] + self.data[3] + (a,b))) #todo: dist on line x2 = self.l * ( (a - self.data[0][0])/(self.data[3][0] - self.data[0][0]) ) - self.l/2.0 y2 = self.k if self.pos == 'down': camera= (x1, -y1) point = (x2, -y2) elif self.pos == 'right': camera= (y1, x1) point = (y2, x2) elif self.pos == 'up': camera= (-x1, y1) point = (-x2, y2) elif self.pos == 'left': camera= (-y1, x1) point = (-y2, x2) print "camera:%5.1fx%5.1f point:%5.1fx%5.1f" % (camera + point) return getline( *(camera + point)) def llline(a,b,maxx,maxy): # maxy = a*x+b -> maxy - b = a*x -> x = (maxy-b)/a # 0 = a*x+b -> x = (0-b)/a # ax+b=y -> ax=y-b -> x = (y-b)/a # maxx = (y-b)/a -> maxx*a = y-b -> y = maxx*a - b # 0 = (y-b)/a -> y = 0*a - b ''' x1 = (0-b)/a x2 = (maxy-b)/a y1 = 0*a -b y2 = maxx*a - b if x1 >= 0 and x1 <= maxx: ix = x1/float(maxx), 0 else: ix = x2/float(maxx), maxy/float(maxy) if y1 >= 0 and y1 <=maxy: iy = 0, y1/float(maxy) else: iy = maxx/float(maxx), y2/float(maxy) ''' xl = -maxx/2.0 yl = a*xl + b xr = +maxx/2.0 yr = a*xr + b yd = -maxy/2.0 xd = (yd-b)/a yu = +maxy/2.0 xu = (yu-b)/a ps = [(xl,yl), (xr,yr), (xd, yd), (xu,yu)] ps2 = [] #print "a=%r" % (ps) for x,y in ps: if (x < -maxx/2.0 or x > maxx/2.0 or y < -maxy/2.0 or y > maxy/2.0): continue ps2.append( (x,y) ) #print "b=%r" % (ps2) x0, y0 = ps[0] x1, y1 = ps[1] y0 = -y0 y1 = -y1 x0 += maxx/2.0 y0 += maxy/2.0 x1 += maxx/2.0 y1 += maxy/2.0 ix = x0/float(maxx), y0/float(maxy) iy = x1/float(maxx), y1/float(maxy) #print "%r" % ((ix + iy),) return ix + iy class Cameras: cameras = None def __init__(self, thepoints_list): self.cameras = [] for thepoints in thepoints_list: self.cameras.append( Camera(thepoints) ) def get_xyzaaa(self, pps): c1, c2 = 0.0, 1.0 axes = [0,0] for j in range(4): for i, camera in enumerate(self.cameras): a, b = pps[i] c1, c2, axes[i] = camera.get_a(a,b, c1,c2) #print "%s %f %f" % (i*' ', c1, c2) return axes[0], axes[1] def get_xyz(self, pps): #return 0.5, 0.5 a1, b1= self.cameras[0].get_b(*pps[0]) a2, b2= self.cameras[1].get_b(*pps[1]) x, y = getpoint(a1,b1,a2,b2) print "POINT:%5.1fx%5.1f" % (x,y) y= -y x += REAL_X/2.0 y += REAL_Y/2.0 if x < 0:x=0 if x > REAL_X:x = REAL_X if y < 0:y=0 if y > REAL_Y:y = REAL_Y return x/float(REAL_X), y/float(REAL_Y), [ llline(a1,b1,REAL_X,REAL_Y), llline(a2,b2,REAL_X,REAL_Y) ] """ thepoint1 = [(0.71484375, 0.80859375, 1), (0.880859375, 0.87630208333333337, 1), (0.1806640625, 0.84895833333333337, 1), (0.29296875, 0.78776041666666663, 1)] thepoint2 = [(0.8828125, 0.91666666666666663, 1), (0.1181640625, 0.87890625, 2), (0.2421875, 0.828125, 1), (0.74609375, 0.85026041666666663, 1)] pp = {'p2': (0.5048828125, 0.67838541666666663, 1), 'p1': (0.5029296875, 0.64453125, 2)} #{'p2': (0.5048828125, 0.67838541666666663, 1), 'p1': (0.501953125, 0.64583333333333337, 1)} pps = [ (0.5029296875, 0.64453125), (0.5048828125, 0.67838541666666663) ] cs = Cameras( [thepoint1, thepoint2] ) print "%r" % (cs.get_xyz(pps), ) """ ''' a, b = cs.cameras[0].p(0.501953125, 0.64583333333333337) print "%r %r" % (a,b) a, b = cs.cameras[0].p(0.5029296875, 0.64453125, a, b) print "%r %r" % (a,b) a, b = cs.cameras[0].p(0.501953125, 0.64583333333333337, a, b) print "%r %r" % (a,b) '''