Example #1
0
    def rotate(self, angle):
        """
        Rotate curve over a given angle.

        """
        R = rotation(-angle)
        self.points = np.dot(self.points, R)
def compute_linear_displacement(c0_current, c1_current, theta):
    """
    Solve constraint equation for linear displacement.

    Parameters
    ----------

    c0_current, c1_current : ndarray
                             Corresponding points on matching curves.
                
    theta : scalar
            Angle matching ``c0_current`` to ``c0_current``.

    """
    return c1_current - np.dot(rotation(theta), c0_current)
    def __init__(self, c0, c1, g):

        self.N = c0.length()
        self.c0_3_raw = np.empty((self.N, 3))
        self.c1_3_raw = np.empty((self.N, 3))
        self.xi = np.empty((3, 3, self.N))

        # Copy c0, c1 into Nx3 arrays with ones in the last column
        self.c0_3_raw[:, 0:2] = c0.points
        self.c1_3_raw[:, 0:2] = c1.points
        self.c0_3_raw[:, 2] = np.ones(self.N)
        self.c1_3_raw[:, 2] = np.ones(self.N)

        # Compute infinitesimal transformations
        for k in xrange(0, self.N):
            gmat = np.empty((3, 3))
            gmat[0:2, 0:2] = rotation(g.theta[k])
            gmat[0:2, 2] = g.x[k, :]
            gmat[2, 2] = 1

            # Obtain Lie algebra element
            self.xi[:, :, k] = logm(gmat)
def integrate(c0, c1, theta0, m):
    """
    Integrate curve matching equations for two given equations.

    Parameters
    ----------

    c0, c1 : ndarray
             Curves to match. Must have the same number of points

    theta0 : scalar
             Initial value for matching angle.

    m : scalar
        Relative weight for angular deformation.
       
    Returns
    -------

    theta : ndarray
            Array of angles along the curve.

    omega, v : ndarray, ndarray
               Components of angular and linear velocity along the curve.

    delta_theta, delta_omega, delta_v : ndarray, ndarray, ndarray
                                        Components of first variation along 
                                        the curve.

    """

    # Book-keeping
    numpoints = c0.length()

    theta = np.zeros(numpoints)
    omega = np.zeros(numpoints-1)
    v = np.zeros((numpoints-1, 2))

    delta_theta = np.zeros(numpoints)
    delta_omega = np.zeros(numpoints-1)
    delta_v = np.zeros((numpoints-1, 2))

    # Set up initial conditions
    R0 = rotation(theta0); R1 = R0
    omega0 = 0
    v0 = compute_linear_velocity(R0, omega0, 
                                 (c0[0, :], c0[1, :]), 
                                 (c1[0, :], c1[1, :]))

    delta_theta0 = 1
    delta_theta1 = 1
    
    delta_omega0 = 0
    delta_v0 = np.dot(R0.T, np.dot(J, c1[1]-c1[0]))*delta_theta0

    # Record initial values
    theta[0] = theta0; theta[1] = theta0
    omega[0] = omega0 # Zero
    v[0, :] = v0

    delta_theta[0] = delta_theta0
    delta_theta[1] = delta_theta1
    delta_omega[0] = delta_omega0
    delta_v[0, :] = delta_v0

    # Main integration loop 
    for s in xrange(1, numpoints-1):

        c0_points = (c0[s, :], c0[s+1, :])
        c1_points = (c1[s, :], c1[s+1, :])

        # Compute next element of solution trajectory
        R2, omega1, v1 = integrate_one_step(R1, omega0, v0, 
                                            c0_points, c1_points, m)

        # Compute next step of first-variation equation
        delta_omega1, delta_v1, delta_theta2 = \
            compute_first_variation(omega0, v0, omega1, v1, R1, 
                                    delta_omega0, delta_v0, delta_theta1,
                                    c0_points, c1_points, m)

        # Record computed values
        omega[s] = omega1
        theta[s+1] = math.atan2(R2[1,0], R2[0,0]) 

        v[s, :] = v1

        delta_omega[s] = delta_omega1
        delta_theta[s+1] = delta_theta2 

        delta_v[s, :] = delta_v1

        # Move along
        R1 = R2
        omega0 = omega1
        v0 = v1

        delta_theta1 = delta_theta2
        delta_omega0 = delta_omega1
        delta_v0 = delta_v1

    return theta, omega, v, delta_theta, delta_omega, delta_v