<reponame>calcdude84se/lasersandbox
import threedize as ddd
import numpy.matlib as np
import numpy.linalg as npl

# Prepare data from the camera for use with threedize.threedize_phi_angles
def threedize_phi_angles(data, ref_half_plane, view, cameraposor, laserpos, lasertheta):
    """Using ref_half_plane, tag data and pass it to threedize.threedize_phi_angles"""
    return ddd.threedize_phi_angles(tag_data(data, ref_half_plane, view, cameraposor, laserpos, lasertheta),
                                 view, cameraposor, laserpos, lasertheta)

# x is in the half-plane if, in addition to dot(n,x)=0, we have dot(side,x)>=0
class HalfPlane(object):
    def __init__(self, pos, normal, side):
        self.pos = pos
        self.normal = normal
        self.side = side

class NoReferenceException(Exception): pass

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)

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_norm(norm, lasertheta):
    """Given norm, the normal vector to the plane of the laser, and lasertheta, the
angle it is rotated along z from the positive x-axis, calculate phi, the angle
of inclination of the norm."""
    return np.arctan2(norm[2], norm[0]*np.cos(lasertheta) + norm[1]*np.sin(lasertheta))

def tag_data(data, ref_half_plane, view, cameraposor, laserpos, lasertheta):
    """Given a list of arrays of pixels data, return a list of tuples whose first
element is the calculated laser phi and whose second element is the associated
original data.  The other parameters are passed to calc_phi.  Pixel arrays that
cause NoReferenceException to be thrown are ignored."""
    result = []
    for xys in data:
        if len(xys) == 0:
            continue
        try:
            result.append((calc_phi(xys, ref_half_plane, view,
                                    cameraposor, laserpos, lasertheta),
                           xys))
            print("say "+str(result[-1][0]*180/np.pi))
        except NoReferenceException:
            pass
    return result

import threedize as ddd
import numpy.matlib as np
import numpy.linalg as npl

# Prepare data from the camera for use with threedize.threedize_phi_angles
def threedize_phi_angles(data, ref_half_plane, view, cameraposor, laserpos, lasertheta):
    """Using ref_half_plane, tag data and pass it to threedize.threedize_phi_angles"""
    return ddd.threedize_phi_angles(tag_data(data, ref_half_plane, view, cameraposor, laserpos, lasertheta),
                                 view, cameraposor, laserpos, lasertheta)

# x is in the half-plane if, in addition to dot(n,x)=0, we have dot(side,x)>=0
class HalfPlane(object):
    def __init__(self, pos, normal, side):
        self.pos = pos
        self.normal = normal
        self.side = side

class NoReferenceException(Exception): pass

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)

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_norm(norm, lasertheta):
    """Given norm, the normal vector to the plane of the laser, and lasertheta, the
angle it is rotated along z from the positive x-axis, calculate phi, the angle
of inclination of the norm."""
    return np.arctan2(norm[2], norm[0]*np.cos(lasertheta) + norm[1]*np.sin(lasertheta))

def tag_data(data, ref_half_plane, view, cameraposor, laserpos, lasertheta):
    """Given a list of arrays of pixels data, return a list of tuples whose first
element is the calculated laser phi and whose second element is the associated
original data.  The other parameters are passed to calc_phi.  Pixel arrays that
cause NoReferenceException to be thrown are ignored."""
    result = []
    for xys in data:
        if len(xys) == 0:
            continue
        try:
            result.append((calc_phi(xys, ref_half_plane, view,
                                    cameraposor, laserpos, lasertheta),
                           xys))
            print("say "+str(result[-1][0]*180/np.pi))
        except NoReferenceException:
            pass
    return result
# Prepare data from the camera for use with threedize.threedize_phi_angles Using ref_half_plane, tag data and pass it to threedize.threedize_phi_angles # x is in the half-plane if, in addition to dot(n,x)=0, we have dot(side,x)>=0 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. # TODO less copy-pasta 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. Given norm, the normal vector to the plane of the laser, and lasertheta, the angle it is rotated along z from the positive x-axis, calculate phi, the angle of inclination of the norm. Given a list of arrays of pixels data, return a list of tuples whose first element is the calculated laser phi and whose second element is the associated original data.  The other parameters are passed to calc_phi.  Pixel arrays that cause NoReferenceException to be thrown are ignored.