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)
Esempio n. 3
0
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
Esempio n. 4
0
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))
Esempio n. 5
0
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: