Python Quaternion.from_rotation方法代码示例

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import from_rotation [as 别名]
 def estimate_divergence(self):
   ''' 
   Estimate the divergence term with a deterministic
   finite difference approach. This is for a single quaternion.
   '''
   delta = 1e-6
   div_term = np.zeros(3)
   for k in range(3):
     omega = np.zeros(3)
     omega[k] = 1.
     quaternion_dt = Quaternion.from_rotation(omega*delta/2.)
     quaternion_tilde_1 = quaternion_dt*self.orientation[0]
     quaternion_dt = Quaternion.from_rotation(-1.*omega*delta/2.)
     quaternion_tilde_2 = quaternion_dt*self.orientation[0]
     div_term += np.inner((self.mobility([quaternion_tilde_1]) -
                           self.mobility([quaternion_tilde_2])),
                          omega/delta)
     
   return div_term

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import from_rotation [as 别名]
 def test_quaternion_from_rotation(self):
   ''' Test that we correctly create a quaternion from an angle '''
   # Generate a random rotation vector
   phi = np.random.rand(3)
   phi_norm = np.linalg.norm(phi)
   theta = Quaternion.from_rotation(phi)
   
   self.assertAlmostEqual(theta.entries[0], np.cos(phi_norm/2.))
   self.assertAlmostEqual(theta.entries[1], np.sin(phi_norm/2.)*phi[0]/phi_norm)
   self.assertAlmostEqual(theta.entries[2], np.sin(phi_norm/2.)*phi[1]/phi_norm)
   self.assertAlmostEqual(theta.entries[3], np.sin(phi_norm/2.)*phi[2]/phi_norm)

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import from_rotation [as 别名]
  def additive_em_time_step(self, dt):
    ''' 
    Take a simple Euler Maruyama step assuming that the mobility is
    constant.  This for testing and debugging.  We also use it to make sure
    that we need the drift for the correct distribution, etc.
    '''
    if self.has_location:
      mobility  = self.mobility(self.location, self.orientation)
      mobility_half = np.linalg.cholesky(mobility)
      noise = np.random.normal(0.0, 1.0, self.dim*6)
      force = self.force_calculator(self.location, self.orientation)
      torque = self.torque_calculator(self.location, self.orientation)
      velocity_and_omega = (np.dot(mobility, np.concatenate([force, torque])) +
                            np.sqrt(2.0*self.kT/dt)*
                            np.dot(mobility_half, noise))
      velocity = velocity_and_omega[0:(3*self.dim)]
      omega = velocity_and_omega[(3*self.dim):(6*self.dim)]
      new_location = self.location + dt*velocity
      new_orientation = []
      for i in range(self.dim):
        quaternion_dt = Quaternion.from_rotation((omega[(i*3):(i*3+3)])*dt)
        new_orientation.append(quaternion_dt*self.orientation[i])
      if self.check_new_state(new_location, new_orientation):
        self.successes += 1
        self.orientation = new_orientation
        self.location = new_location
    else:
      mobility = self.mobility(self.orientation)
      mobility_half = np.linalg.cholesky(mobility)
      torque = self.torque_calculator(self.orientation)
      noise = np.random.normal(0.0, 1.0, self.dim*3)
      omega = (np.dot(mobility, torque) + 
               np.sqrt(2.0*self.kT/dt)*np.dot(mobility_half, noise))
      new_orientation = []
      for i in range(self.dim):
        quaternion_dt = Quaternion.from_rotation((omega[(i*3):(i*3+3)])*dt)
        new_orientation.append(quaternion_dt*self.orientation[i])

      if self.check_new_state(None, new_orientation):
        self.successes += 1
        self.orientation = new_orientation

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import from_rotation [as 别名]
  def test_quaternion_rotation_angle(self):
    ''' Test generating rotation angle from quaternion. '''
    # First construct any random unit quaternion. Not uniform.
    s = 2*random.random() - 1.
    p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
    p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() - 
          (1. - np.abs(s) - np.abs(p1)))
    p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
    theta = Quaternion(np.array([s, p1, p2, p3]))

    rotation_angle = theta.rotation_angle()
    phi = Quaternion.from_rotation(rotation_angle)

    self.assertAlmostEqual(phi.s, theta.s)
    self.assertAlmostEqual(phi.p[0], theta.p[0])
    self.assertAlmostEqual(phi.p[1], theta.p[1])
    self.assertAlmostEqual(phi.p[2], theta.p[2])

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import from_rotation [as 别名]
  def test_rot_matrix_against_rodriguez(self):
    ''' 
    Test that given an angle of rotation, the quaternion
    rotation matrix matches Rodriguez formula.
    '''
    # Generate a random rotation, phi.
    phi = np.random.normal(0., 1., 3)
    phi = phi/np.linalg.norm(phi)
    magnitude = np.random.uniform(0., np.pi)

    theta = Quaternion.from_rotation(phi*magnitude)
    R = theta.rotation_matrix()
    omega = np.array([[0., -1.*phi[2], phi[1]],
                     [phi[2], 0., -1.*phi[0]],
                     [-1.*phi[1], phi[0], 0.]])
    R_rodriguez = (np.identity(3) + np.sin(magnitude)*omega +
                   np.inner(omega, omega.T)*(1. - np.cos(magnitude)))

    for j in range(3):
      for k in range(3):
        self.assertAlmostEqual(R[j, k], R_rodriguez[j, k])

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import from_rotation [as 别名]
  def fixman_time_step(self, dt):
    ''' Take a timestep of length dt using the Fixman method '''
    # Try to take steps until one is valid
    while True:
      if self.has_location:
        # Handle integrator with location as well.
        mobility  = self.mobility(self.location, self.orientation)
        mobility_half = np.linalg.cholesky(mobility)
        noise = np.random.normal(0.0, 1.0, self.dim*6)
        force = self.force_calculator(self.location, self.orientation)
        torque = self.torque_calculator(self.location, self.orientation)
        velocity_and_omega = (np.dot(mobility, np.concatenate([force, torque])) +
                              np.sqrt(4.0*self.kT/dt)*
                              np.dot(mobility_half, noise))
        velocity = velocity_and_omega[0:(3*self.dim)]
        omega = velocity_and_omega[(3*self.dim):(6*self.dim)]
      else:
        mobility  = self.mobility(self.orientation)
        mobility_half = np.linalg.cholesky(mobility)
        noise = np.random.normal(0.0, 1.0, self.dim*3)
        torque = self.torque_calculator(self.orientation)
        omega = (np.dot(mobility, torque) + 
               np.sqrt(4.0*self.kT/dt)*np.dot(mobility_half, noise))
        
      # Update each quaternion at a time.
      orientation_midpoint = []
      for i in range(self.dim):
        quaternion_dt = Quaternion.from_rotation((omega[(i*3):(i*3+3)])*dt/2.)
        orientation_midpoint.append(quaternion_dt*self.orientation[i])

      if self.has_location:
        location_midpoint = self.location + 0.5*dt*velocity
        if not self.check_new_state(location_midpoint, orientation_midpoint):
          # restart the step.
          continue

      if self.has_location:
        mobility_tilde = self.mobility(location_midpoint, orientation_midpoint)
        noise = noise + np.random.normal(0.0, 1.0, self.dim*6)
        force_tilde = self.force_calculator(location_midpoint, orientation_midpoint)
        torque_tilde = self.torque_calculator(location_midpoint, orientation_midpoint)
        mobility_half_inv = np.linalg.inv(mobility_half)
        velocity_and_omega_tilde = (
          np.dot(mobility_tilde, 
                 np.concatenate([force_tilde, torque_tilde])) + np.sqrt(self.kT/dt)*
          np.dot(mobility_tilde, np.dot(mobility_half_inv.T, noise)))
        velocity_tilde = velocity_and_omega_tilde[0:(3*self.dim)]
        self.avg_velocity += np.linalg.norm(velocity_tilde)
        omega_tilde = velocity_and_omega_tilde[(3*self.dim):(6*self.dim)]
        self.avg_omega += np.linalg.norm(omega_tilde)
      
      else:
        mobility_tilde = self.mobility(orientation_midpoint)
        noise = noise + np.random.normal(0.0, 1.0, self.dim*3)
        torque_tilde = self.torque_calculator(orientation_midpoint)
        mobility_half_inv = np.linalg.inv(mobility_half)
        omega_tilde = (
          np.dot(mobility_tilde, torque_tilde) + np.sqrt(self.kT/dt)*
          np.dot(mobility_tilde, np.dot(mobility_half_inv.T, noise)))
        
      new_orientation = []
      for i in range(self.dim):
        quaternion_dt = Quaternion.from_rotation((omega_tilde[(i*3):(i*3+3)])*dt)
        new_orientation.append(quaternion_dt*self.orientation[i])

      # Check that the new state is admissible. Re-take the step 
      # (with tail recursion) if the new state is invalid.
      if self.has_location:
        new_location = self.location + dt*velocity_tilde
        if self.check_new_state(new_location, new_orientation):
          self.location = new_location
          self.orientation = new_orientation
          self.successes += 1
          return
      else:
        if self.check_new_state(None, new_orientation):
          self.orientation = new_orientation
          self.successes += 1
          return

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import from_rotation [as 别名]
  def rfd_time_step(self, dt):
    ''' Take a timestep of length dt using the RFD method '''
    # Attempt steps until we find a valid endpoint.  
    while True:
      if self.has_location:
        # Handle integrator with location as well.
        mobility  = self.mobility(self.location, self.orientation)
        mobility_half = np.linalg.cholesky(mobility)
        rfd_noise = np.random.normal(0.0, 1.0, self.dim*6)
        # Calculate RFD location.
        rfd_location = self.location + self.rf_delta*rfd_noise[0:3*self.dim]
        # Update each quaternion at a time for RFD orientation.
        rfd_orientation = []
        for i in range(self.dim):
          quaternion_dt = Quaternion.from_rotation((self.rf_delta*
                                                    rfd_noise[(3*self.dim + i*3):
                                                              (3*self.dim + i*3+3)]))
          rfd_orientation.append(quaternion_dt*self.orientation[i])

        # divergence term d_x(N) : \Psi^T 
        divergence_term = self.kT*np.dot(
          (self.mobility(rfd_location, rfd_orientation) - mobility),
          rfd_noise/self.rf_delta)

        noise = np.random.normal(0.0, 1.0, self.dim*6)
        force = self.force_calculator(self.location, self.orientation)
        torque = self.torque_calculator(self.location, self.orientation)
        velocity_and_omega = (np.dot(mobility, np.concatenate([force, torque])) +
                              np.sqrt(2.0*self.kT/dt)*
                              np.dot(mobility_half, noise) +
                              divergence_term)
        velocity = velocity_and_omega[0:(3*self.dim)]
        self.avg_velocity += np.linalg.norm(velocity)
        omega = velocity_and_omega[(3*self.dim):(6*self.dim)]
        self.avg_omega += np.linalg.norm(omega)

        new_location = self.location + dt*velocity

      else:
        rfd_noise = np.random.normal(0.0, 1.0, self.dim*3)
        mobility  = self.mobility(self.orientation)
        mobility_half = np.linalg.cholesky(mobility)
        torque = self.torque_calculator(self.orientation)
        
        noise = np.random.normal(0.0, 1.0, self.dim*3)
        # Update each quaternion at a time for rfd orientation.
        rfd_orientation = []
        for i in range(self.dim):
          quaternion_dt = Quaternion.from_rotation((self.rf_delta*
                                                    rfd_noise[(i*3):(i*3+3)]))
          rfd_orientation.append(quaternion_dt*self.orientation[i])

        # divergence term d_x(M) : \Psi^T 
        divergence_term = self.kT*np.dot(
          (self.mobility(rfd_orientation) - mobility),
          rfd_noise/self.rf_delta)
        omega = (np.dot(mobility, torque) + 
                 np.sqrt(2.*self.kT/dt)*
                 np.dot(mobility_half, noise) +
                 divergence_term)

      # For with location and without location, we update orientation the same way.
      new_orientation = []
      for i in range(self.dim):
        quaternion_dt = Quaternion.from_rotation((omega[(i*3):(i*3+3)])*dt)
        new_orientation.append(quaternion_dt*self.orientation[i])
        
      # Check validity of new state.
      if self.has_location:
        if self.check_new_state(new_location, new_orientation):
          self.location = new_location
          self.orientation = new_orientation
          self.successes += 1
          return
      else:
        if self.check_new_state(None, new_orientation):
          self.orientation = new_orientation
          self.successes += 1
          return