# Path: lfd/rapprentice/math_utils.py
# def interp2d(x,xp,yp):
# def interp_mat(x, xp):
# def normalize(x):
# def normr(x):
# def normc(x):
# def norms(x,ax):
# def intround(x):
# def deriv(x):
# def linspace2d(start,end,n):
# def remove_duplicate_rows(mat):
# def invertHmat(hmat):
#     T = len(x)
#     R = hmat[:3,:3]


import numpy as np
from lfd.rapprentice import svds, math_utils
        return x_ma.dot(self.lin_ag) + self.trans_g[None,:]  
    def compute_jacobian(self, x_ma):
        return np.repeat(self.lin_ag.T[None,:,:],len(x_ma), axis=0)
        
class Composition(Transformation):
    def __init__(self, fs):
        "applied from first to last (left to right)"
        self.fs = fs
    def transform_points(self, x_ma):
        for f in self.fs: x_ma = f.transform_points(x_ma)
        return x_ma
    def compute_jacobian(self, x_ma):
        grads = []
        for f in self.fs:
            grad_mga = f.compute_jacobian(x_ma)
            grads.append(grad_mga)
            x_ma = f.transform_points(x_ma)
        totalgrad = grads[0]
        for grad in grads[1:]:
            totalgrad = (grad[:,:,:,None] * totalgrad[:,None,:,:]).sum(axis=-2)
        return totalgrad

def orthogonalize3_cross(mats_n33):
    "turns each matrix into a rotation"

    x_n3 = mats_n33[:,:,0]
    # y_n3 = mats_n33[:,:,1]
    z_n3 = mats_n33[:,:,2]

    znew_n3 = math_utils.normr(z_n3)

# Path: lfd/rapprentice/math_utils.py
# def interp2d(x,xp,yp):
# def interp_mat(x, xp):
# def normalize(x):
# def normr(x):
# def normc(x):
# def norms(x,ax):
# def intround(x):
# def deriv(x):
# def linspace2d(start,end,n):
# def remove_duplicate_rows(mat):
# def invertHmat(hmat):
#     T = len(x)
#     R = hmat[:3,:3]


# Path: lfd/registration/transformation.py
import numpy as np
from lfd.rapprentice import svds, math_utils

        return x_ma.dot(self.lin_ag) + self.trans_g[None,:]  
    def compute_jacobian(self, x_ma):
        return np.repeat(self.lin_ag.T[None,:,:],len(x_ma), axis=0)
        
class Composition(Transformation):
    def __init__(self, fs):
        "applied from first to last (left to right)"
        self.fs = fs
    def transform_points(self, x_ma):
        for f in self.fs: x_ma = f.transform_points(x_ma)
        return x_ma
    def compute_jacobian(self, x_ma):
        grads = []
        for f in self.fs:
            grad_mga = f.compute_jacobian(x_ma)
            grads.append(grad_mga)
            x_ma = f.transform_points(x_ma)
        totalgrad = grads[0]
        for grad in grads[1:]:
            totalgrad = (grad[:,:,:,None] * totalgrad[:,None,:,:]).sum(axis=-2)
        return totalgrad

def orthogonalize3_cross(mats_n33):
    "turns each matrix into a rotation"

    x_n3 = mats_n33[:,:,0]
    # y_n3 = mats_n33[:,:,1]
    z_n3 = mats_n33[:,:,2]

    znew_n3 = math_utils.normr(z_n3)

    ynew_n3 = math_utils.normr(np.cross(znew_n3, x_n3))