Initial commit of gyro-integrating function.

qpoint/_libqpoint.py

@@ -541,6 +541,24 @@ def setargs(fname, arg=None, res=None):
         ct.c_int,
     ),
 )
+setargs(
+    "qp_omega2azelpa",
+    arg=(
+        qp_memory_t_p,  # params
+        ct.c_double,  # init_az
+        ct.c_double,  # init_el
+        ct.c_double,  # init_roll
+        arr,  # omega_x
+        arr,  # omega_y
+        arr,  # omega_z
+        warr,  # azimuth (output)
+        warr,  # elevation (output)
+        warr,  # psi (output)
+        ct.c_double,  # delta_t
+        ct.c_int,  # n_samples
+        ct.c_int,  # fast_rot
+    ),
+)
 setargs(
     "qp_azel2bore",
     arg=(

qpoint/qpoint_class.py

@@ -1324,6 +1324,81 @@ def quat2radecpa(self, quat, **kwargs):
             return ra[0], dec[0], pa[0]
         return ra, dec, pa
 
+    def omega2azelpa(
+        self,
+        init_az,
+        init_el,
+        init_roll,
+        omega_x,
+        omega_y,
+        omega_z,
+        delta_t,
+        fast_rot=False,
+        **kwargs,
+    ):
+        """
+        Compute azimuth, elevation, and roll from angular velocity data.
+
+        Parameters
+        ----------
+        init_az : float
+            Initial azimuth in degrees.
+        init_el : float
+            Initial elevation in degrees.
+        init_roll : float
+            Initial roll (boresight rotation) in degrees.
+        omega_x, omega_y, omega_z : np.ndarray
+            Angular velocity components in the AZ/EL/ROLL frame.
+        delta_t : float
+            Time interval between samples.
+        fast_rot : bool, optional
+            If True, uses fast quaternion rotation (default: False).
+
+        Returns
+        -------
+        azimuth, elevation, roll : np.ndarray
+            Arrays of azimuth, elevation, and roll values for each sample.
+        """
+        # self.set(**kwargs)
+
+        omega_x = check_input("omega_x", omega_x, dtype=np.double)
+        omega_y = check_input("omega_y", omega_y, dtype=np.double)
+        omega_z = check_input("omega_z", omega_z, dtype=np.double)
+
+        # omega_x, omega_y, omega_z = check_inputs(omega_x, omega_y, omega_z)
+
+        n_samples = omega_x.size
+
+        azimuth = check_output("azimuth", shape=(n_samples,), dtype=np.double)
+        elevation = check_output("elevation", shape=(n_samples,), dtype=np.double)
+        roll = check_output("roll", shape=(n_samples,), dtype=np.double)
+
+        # print(f"Addresses in Python - Azimuth: {azimuth.__array_interface__['data'][0]}, "
+        # f"Elevation: {elevation.__array_interface__['data'][0]}, "
+        # f"Roll: {roll.__array_interface__['data'][0]}")
+
+        qp.qp_omega2azelpa(
+            self._memory,
+            init_az,
+            init_el,
+            init_roll,
+            omega_x,
+            omega_y,
+            omega_z,
+            azimuth,
+            elevation,
+            roll,
+            delta_t,
+            int(n_samples),
+            int(fast_rot),
+        )
+
+        # print(f"Addresses in Python - Azimuth: {hex(id(azimuth))}, "
+        #   f"Elevation: {hex(id(elevation))}, "
+        # f"Roll: {hex(id(roll))}")
+
+        return azimuth, elevation, roll
+
     def quat2pixpa(self, quat, nside=256, **kwargs):
         """
         Calculate ra/dec/pa for input quaternion(s).

src/qpoint.c

@@ -1126,3 +1126,98 @@ void qp_azel2rasindec_hwp(qp_memory_t *mem,
                    az, el, 0, pitch, roll, lon, lat, ctime,
                    hwp, ra, sindec, sin2psi, cos2psi, n);
 }
+
+void 
+qp_quat_azelpsi(const quat_t q, double *az, double *el, double *psi) 
+{
+    double x, y, z, w;
+    x = q[1];
+    y = q[2];
+    z = q[3];
+    w = q[0];
+
+    double sin_el_sq = x * x + y * y;   
+    double cos_el_sq = w * w + z * z;   
+
+    double s = atan2(z, w);            
+    double d = atan2(x, y);          
+
+    *az = (s - d);               
+    *psi = (s + d);               
+
+    if (cos_el_sq > 1e-12) {
+        *el = 2.0 * atan(sqrt(sin_el_sq / cos_el_sq));
+    } else {
+        *el = (sin_el_sq < 0.5) ? 0.0 : M_PI;
+    }
+    *el = M_PI_2 - *el;                 // el = π/2 - el
+    *psi = M_PI - *psi;                 // psi = π - psi              // psi = π - psi
+
+    *az = -rad2deg(*az);
+    *el = rad2deg(*el);
+    *psi = rad2deg(*psi);
+
+    if (*psi > 180.0) {
+      *psi -= 360.0;
+    } else if (*psi < -180.0) {
+      *psi += 360.0;
+    }
+
+    if (fabs(*az) < 1e-12) *az = 0.0;
+    if (fabs(*el) < 1e-12) *el = 0.0;
+    if (fabs(*psi) < 1e-12) *psi = 0.0;
+}
+
+// Test Quaternion_rot_vector_fast()
+// void print_vector(const char *label, const double v[3]) {
+//     printf("%s: [%f, %f, %f]\n", label, v[0], v[1], v[2]);
+// }
+
+//     double angle = M_PI / 4;  // 90 degrees in radians
+//     Quaternion q_rotation;
+//     double axis[3] = {0, 0, 1};  // Rotation around the Z-axis
+//     Quaternion_rot(q_rotation, angle, axis);
+//     // Define the vector to rotate (unit vector along X-axis)
+//     double vector[3] = {1, 0, 0};
+//     double rotated_vector[3];
+//     // Rotate the vector
+//     Quaternion_rot_vector_fast(q_rotation, vector, rotated_vector);
+//     // Print the original and rotated vectors
+//     print_vector("Original vector", vector);
+//     print_vector("Rotated vector", rotated_vector);
+
+
+// Compute attitude quaternion from angular velocity. 
+void 
+qp_omega2azelpa(qp_memory_t *mem, double init_az, double init_el, double init_roll,
+                double *omega_x, double *omega_y, double *omega_z,
+                double *azimuth, double *elevation, double *psi,
+                double dt, int n_samples, int fast_rot)
+{
+    Quaternion attitude;
+    qp_azelpsi_quat(init_az, init_el, init_roll, 0.0, 0.0, attitude);
+    // printf("Initial quaternion: [%f, %f, %f, %f]\n", attitude[0], attitude[1], attitude[2], attitude[3]);
+
+    for (int i = 0; i < n_samples; i++) {
+        double omega[3] = {omega_x[i], omega_y[i], omega_z[i]};
+        double rotated_omega[3];
+
+        // Rotate the gyro signal into the new frame of the instrument
+        // if (fast_rot) {
+        //     Quaternion_rot_vector_fast(attitude, omega, rotated_omega);
+        // } else {
+        //     Quaternion_rot_vector(attitude, omega, rotated_omega);
+        // }
+
+        // apply_angular_velocity(attitude, rotated_omega[2], rotated_omega[1], rotated_omega[0], dt);
+        apply_angular_velocity(attitude, omega[0], omega[1], omega[2], dt);
+        // apply_angular_velocity(attitude, rotated_omega[0], rotated_omega[1], rotated_omega[2], dt);
+
+        double az_rad, el_rad, psi_rad;
+        qp_quat_azelpsi(attitude, &az_rad, &el_rad, &psi_rad);
+
+        azimuth[i] = az_rad;
+        elevation[i] = el_rad;
+        psi[i] = psi_rad;
+    }
+}

src/qpoint.h

@@ -606,6 +606,14 @@ extern "C" {
 			    double *ra, double *sindec, double *sin2psi,
 			    double *cos2psi, int n);
 
+  /* Calculate az/el/bs roll (psi) from a starting attitude value and gyro rates.
+  This function integrates gyro readings to get the new attitude. 
+  It uses Horizontal-frame quaternions internally. */
+  void qp_omega2azelpa(qp_memory_t *mem, double init_az, double init_el, double init_roll,
+                     double *omega_x, double *omega_y, double *omega_z,
+                     double *azimuth, double *elevation, double *psi,
+                     double dt, int n_samples, int fast_rot);
+
 #ifndef ENABLE_LITE
 
   /* *************************************************************************

src/quaternion.c

@@ -205,3 +205,69 @@ QuaternionSlerp_interpolate(const QuaternionSlerp *slerp, double t, Quaternion q
     q[i] = s0*slerp->q0[i] + s1*slerp->q1[i];
 }
 
+
+// Standard vector rotation using quaternions. 32 multiplications.
+void 
+Quaternion_rot_vector(Quaternion q, const double v[3], double v_rot[3]) 
+{
+    Quaternion vector_quat = {0, v[0], v[1], v[2]};
+    Quaternion temp, rotated_quat;
+
+    Quaternion_mul(temp, q, vector_quat);
+    Quaternion_conj(q);
+    Quaternion_mul(rotated_quat, temp, q);
+    Quaternion_conj(q); // Restore original quaternion
+
+    v_rot[0] = rotated_quat[1];
+    v_rot[1] = rotated_quat[2];
+    v_rot[2] = rotated_quat[3];
+}
+
+/* Vector rotation using quaternions with fewer operations. 15 multiplcations.
+this computes v' = v + q_0 x t + (q_v  x t)
+where t = 2 x (q_v x v_v) */
+void 
+Quaternion_rot_vector_fast(Quaternion q, const double v[3], double v_rot[3])
+{
+    double t[3];
+    t[0] = 2.0 * (q[1] * v[2] - q[3] * v[1]);
+    t[1] = 2.0 * (q[2] * v[0] - q[1] * v[2]);
+    t[2] = 2.0 * (q[3] * v[0] - q[2] * v[1]);
+    v_rot[0] = v[0] + q[0] * t[0] + (q[2] * t[2] - q[3] * t[1]);
+    v_rot[1] = v[1] + q[0] * t[1] + (q[3] * t[0] - q[1] * t[2]);
+    v_rot[2] = v[2] + q[0] * t[2] + (q[1] * t[1] - q[2] * t[0]);
+}
+
+
+void 
+apply_angular_velocity(Quaternion attitude, double omega_x, double omega_y, double omega_z, double delta_t) 
+{
+    double omega_mag = sqrt(omega_x * omega_x + omega_y * omega_y + omega_z * omega_z);
+    double angle = omega_mag * delta_t;
+
+    if (omega_mag == 0.0 || angle == 0.0) {
+        return; // No change to attitude, so return early
+    }
+
+    double axis[3] = {omega_x / omega_mag, omega_y / omega_mag, omega_z / omega_mag};
+    // printf("Axis: [%f, %f, %f]\n", axis[0], axis[1], axis[2]);
+    Quaternion q_delta;
+    Quaternion_rot(q_delta, angle, axis);
+    // printf("Quaternion delta: [%f, %f, %f, %f]\n", q_delta[0], q_delta[1], q_delta[2], q_delta[3]);
+
+    Quaternion temp;
+    Quaternion_mul(temp, attitude, q_delta);
+    attitude[0] = temp[0];
+    attitude[1] = temp[1];
+    attitude[2] = temp[2];
+    attitude[3] = temp[3];
+
+    double norm = Quaternion_norm(attitude);
+    attitude[0] /= norm;
+    attitude[1] /= norm;
+    attitude[2] /= norm;
+    attitude[3] /= norm;
+    // printf("Updated quaternion: [%f, %f, %f, %f]\n", attitude[0], attitude[1], attitude[2], attitude[3]);
+}
+
+

src/quaternion.h

@@ -96,6 +96,9 @@ extern "C" {
   void Quaternion_r2_mul(double angle, Quaternion q);
   void Quaternion_r3_mul(double angle, Quaternion q);
 
+  void Quaternion_rot_vector(Quaternion q, const double v[3], double v_rot[3]);
+  void Quaternion_rot_vector_fast(Quaternion q, const double v[3], double v_rot[3]);
+
   // q = q * scale
   static inline void
   Quaternion_scale(Quaternion q, double scale)
@@ -168,6 +171,10 @@ extern "C" {
   void
   QuaternionSlerp_interpolate(const QuaternionSlerp *slerp, double t, Quaternion z);
 
+  void 
+  apply_angular_velocity(Quaternion attitude, double omega_x, double omega_y, double omega_z, double delta_t);
+
+
 #ifdef __cplusplus
 }
 #endif