Exemplo n.º 1
0
def compute_linear_velocity(R1, omega1, c0_points, c1_points):
    """
    Solve time-derivative of constraint equation for linear velocity.

    Parameters
    ----------

    R1 : ndarray
         Rotation matrix matching the points in step ``k`` of the algorithm.

    omega1 : scalar
             Angular velocity to update rotation from step ``k`` to ``k+1``.

    c0_points, c1_points: array_like
                          Tuples of ndarrays, representing the points to be
                          matched at steps ``k`` and ``k+1``.

    Returns
    -------

    v : ndarray
        Linear velocity satisfying the (time-derivative of the) constraint.

    """
    
    c0_current, c0_next = c0_points
    c1_current, c1_next = c1_points

    b = (np.dot(R1.T, c1_next - c1_current) + c0_current - 
         np.dot(cayley_so2(omega1), c0_next))
    B = np.array([[1, omega1/2], [-omega1/2, 1]])
    
    return np.dot(B, b)
Exemplo n.º 2
0
def integrate_one_step(R1, omega0, v0, c0_points, c1_points, m):
    """
    Perform one step in the integration algorithm for the curve matching 
    equations.

    Parameters
    ----------

    R1 : ndarray
         Rotation matrix matching the points in step ``k`` of the algorithm.

    omega0 : scalar
             Angular velocity from previous iteration of algorithm.

    v0 : ndarray
         Linear velocity from previous iteration of algorithm.

    c0_points, c1_points: array_like
                          Tuples of ndarrays, representing the points to be
                          matched at steps ``k`` and ``k+1``.
                          
    m : scalar
        Relative weight for angular deformation.

    Returns
    -------

    R2 : ndarray
         Rotation matrix matching the points in step ``k+1`` of the algorithm.

    omega1, v1 : scalar, ndarray
                 Components of angular and linear velocity in step ``k+1``.

    """

    # Unpack points
    # 
    # c0_current has already been matched to c1_current,
    # c0_next is the one we're trying to match to c1_next
    c0_current, c0_next = c0_points
    c1_current, c1_next = c1_points

    rhs = projected_momentum(-omega0, -v0, m*omega0, v0, c0_current)
    def optimization_function(omega1):
        # Scalar function to find next omega
        v1 = compute_linear_velocity(R1, omega1, c0_points, c1_points)
        return projected_momentum(omega1, v1, m*omega1, v1, c0_current) - rhs

    # Determine components of Lie algebra update element
    omega1 = so.newton(optimization_function, omega0)
    v1 = compute_linear_velocity(R1, omega1, c0_points, c1_points)

    # Determine next group matching element
    # Only the rotational part is needed, since the translational part 
    # can be determined from the constraints, if necessary.

    #th0 = math.atan2(R1[1,0], R1[0,0])

    #print R1 
    #print cayley_so2(omega1)
    #print "theta before: ", th0

    R1 = np.dot(R1, cayley_so2(omega1))
    #th1 = math.atan2(R1[1,0], R1[0,0])

    #print "theta after: ", th1 
    #print "res: ", math.tan((th1-th0)/2) - omega1/2

    #print

    return R1, omega1, v1
Exemplo n.º 3
0
def compute_first_variation(omega0, v0, omega1, v1, R1, 
                            delta_omega0, delta_v0, delta_theta1,
                            c0_points, c1_points, m):

    """
    Solve the first-variation equations. This amounts to solving a 
    (complicated) system of linear equations.

    Parameters
    ----------

    omega0, v0 : scalar, ndarray
                 Angular and linear velocity at step ``k-1``.

    omega1, v1 : scalar, ndarray
                 Angular and linear velocity at step ``k``.

    R1 : ndarray
         Rotation matrix at step ``k``.

    delta_omega0, delta_v0: scalar, ndarray
                            Components of first variation at step ``k-1``.

    delta_theta1 : scalar
                   First variation of angle at step ``k``.

    c0_points, c1_points: array_like
                          Tuples of ndarrays, representing the points to be
                          matched at steps ``k`` and ``k+1``.
                          
    m : scalar
        Relative weight for angular deformation.

    Returns
    -------

    delta_omega1, delta_v1, delta_theta2 : scalar, array_like, scalar

    """

    c0_current, c0_next = c0_points
    c1_current, c1_next = c1_points

    c_plus, d_plus = coefficients_first_variation(omega1, v1, c0_current, m)
    c_neg, d_neg = coefficients_first_variation(-omega0, -v0, c0_current, m)

    b = (np.dot(R1.T, c1_next - c1_current) + c0_current - 
         np.dot(cayley_so2(omega1), c0_next))

    B = np.array([[1, omega1/2], [-omega1/2, 1]])
    A = np.linalg.inv(B)

    C = np.dot(J, b/2 + np.dot(A, c0_next))
    D = np.dot(B, np.dot(R1.T, np.dot(J, c1_next - c1_current)))

    delta_omega1 = 1./(c_plus + np.dot(d_plus, C))*(
        c_neg*delta_omega0 + np.dot(d_neg, delta_v0) - 
        np.dot(d_plus, D)*delta_theta1)

    delta_v1 = C*delta_omega1 + D*delta_theta1

    delta_theta2 = (delta_theta1 + 
                    (1-omega1**2/4)/(1+omega1**2/4)*delta_omega1)
    
    return delta_omega1, delta_v1, delta_theta2