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