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)
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
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