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