Advertisement
Guest User

Untitled

a guest
Jan 8th, 2022
65
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Rust 1.47 KB | None | 0 0
  1. fn calculate_angular_acceleration(&self, dt: f64) -> Vector3 {
  2.         println!("---");
  3.         let orientation = get_next_orientation(self.orientation, self.angular_velocity, dt * 3.0);
  4.  
  5.         let delta_q = self.requested_orientation * orientation.get_inverse();
  6.         let delta_angle = delta_q.angle();
  7.  
  8.         // we will attempt to accelerate or decelerate on this axis
  9.         let accel_axis = delta_q.axis();
  10.  
  11.         let angular_velocity = self.angular_velocity.len();
  12.         // self.angular_velocity.projection_on_vector3(accel_axis);
  13.         //
  14.         let deaccel_angle = angular_velocity.powi(2) / (2.0 * self.max_angular_acceleration);
  15.  
  16.         println!(
  17.             "delta_angle: {:?}, deaccel_angle: {:?}, angular velocity: {:?}, accel_axis: {:?}",
  18.             delta_angle, deaccel_angle, angular_velocity, accel_axis
  19.         );
  20.  
  21.         if delta_angle <= angular_velocity {
  22.             // Remove all angular velocity
  23.             let result = self.angular_velocity.get_normalized()
  24.                 * -f64::min(self.max_angular_acceleration, delta_angle / dt);
  25.             println!("Near the end: {:?}", result);
  26.             return result;
  27.         }
  28.  
  29.         let result = accel_axis
  30.             * if delta_angle > deaccel_angle + angular_velocity * dt {
  31.                 self.max_angular_acceleration
  32.             } else {
  33.                 -self.max_angular_acceleration
  34.             };
  35.         println!("angular_acceleration: {:?}", result);
  36.         result
  37. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement