def calc_phi_points(points, laserpos, lasertheta): """Given an array of triples points that should be in the plane generated by a laser at laserpos with theta lasertheta, calculate the inclination of the laser plane's normal vector.""" plane_line = ddd.coord(-np.sin(lasertheta), np.cos(lasertheta), 0) normals = np.cross(np.array(plane_line.T)[0], points - np.array(laserpos.T)[0]) return calc_phi_norm(np.average((normals.T / npl.norm(normals, axis = 1)).T, axis = 0), lasertheta)
def calc_phi(xys, ref_half_plane, view, cameraposor, laserpos, lasertheta): """Given an array of pixel pairs xys from a camera with view and cameraposor and laser with laserpos and lasertheta, calculate the laser inclination based on a known half-plane ref_half_plane. Throws a NoReferenceException if no pixels are in the reference half-plane.""" cref_pos = ddd.unrotate(ref_half_plane.pos - cameraposor.pos, cameraposor) cref_side = ddd.unrotate(ref_half_plane.side, cameraposor) cref_line = np.cross(cref_side, ddd.unrotate(ref_half_plane.normal, cameraposor), axis = 0) # TODO less copy-pasta cpos = np.array([cref_pos[1, 0], -cref_pos[2, 0]]) / cref_pos[0, 0] * ddd.view_number(view) \ + np.array([view.centerx, view.centery]) cline_ = cref_pos / cref_pos[0, 0] - cref_line / cref_line[0, 0] cside_ = np.array([cref_side[1, 0], -cref_side[2, 0]]) cside = np.array([cline_[2, 0], cline_[1, 0]]) if np.dot(cside, cside_) < 0: cside = - cside dxys = xys - cpos dot_products = np.array(np.mat([cside]) * np.mat(dxys).T)[0] good_xys = xys[dot_products >= 0] print("say "+str(np.average(good_xys[:,1]))) if len(good_xys) == 0: raise NoReferenceException() threepoints = ddd.threedize_plane(good_xys, view, cameraposor, ref_half_plane) return calc_phi_points(threepoints, laserpos, lasertheta)
if len(sys.argv) > 4: fnameout=sys.argv[4] if len(sys.argv) > 5: ratiox=float(sys.argv[5]) if len(sys.argv) > 6: ratioy=float(sys.argv[6]) ww=1.*ratiox hh=ww*h/w*ratioy e=np.array(camera['loc'],dtype=float) f=np.array(camera['front'],dtype=float) u=np.array(camera['up'],dtype=float) sphs=[] for sph in objects: sphs += [np.array(sph['data'],dtype=float)] u/=np.linalg.norm(u) r=np.cross(f,u) u=np.cross(r,f) u/=np.linalg.norm(u) r/=np.linalg.norm(r) def SolveTri(a,b,c): d=b*b-4*a*c t1,t2 = 0., 0. sol=0 if d>0: sd=math.sqrt(d) t1=(-b-sd)/2/a t2=(-b+sd)/2/a sol=2 elif d==0: t1=-b/2/a
def cross_product(x, y): """ Returns a vector which is the cross product of x and y (both vector) """ return Vect(npmat.cross(x.T, y.T))
sphs = [] sphs += [np.array([0, -0.1, 0, 0.05, 0.8, 0.8, 0.8], dtype=float)] sphs += [np.array([0, 0, 0, 0.05, 0.8, 0.8, 0.8], dtype=float)] sphs += [np.array([0, 0.1, 0, 0.05, 0.8, 0.8, 0.8], dtype=float)] sphs += [np.array([0.1, -0.05, 0, 0.05, 0.8, 0, 0], dtype=float)] sphs += [np.array([0.1, 0.05, 0, 0.05, 0, 0, 0.8], dtype=float)] sphs += [np.array([0.2, 0, 0, 0.05, 0, 0.8, 0], dtype=float)] sphs += [np.array([0.05, -0.05, 0.1, 0.05, 0.8, 0.8, 0.8], dtype=float)] sphs += [np.array([0.05, 0.05, 0.1, 0.05, 0.8, 0.8, 0.8], dtype=float)] sphs += [np.array([0, -0.5, 0.5, 0.02, 1, 1, 0], dtype=float)] e = np.array([0.4, 0, 0.4], dtype=float) f = np.array([-1, 0, -1], dtype=float) u = np.array([-0.707107, 0, 0.707107], dtype=float) u /= np.linalg.norm(u) r = np.cross(f, u) u = np.cross(r, f) u /= np.linalg.norm(u) r /= np.linalg.norm(r) def SolveTri(a, b, c): d = b * b - 4 * a * c t1, t2 = 0., 0. sol = 0 if d > 0: sd = math.sqrt(d) t1 = (-b - sd) / 2 / a t2 = (-b + sd) / 2 / a sol = 2 elif d == 0: