added main.c Makefile

main.c

@@ -0,0 +1,156 @@
+/*
+ *
+ * Copyright 2022 Oleg Borodin  <borodin@unix7.org>
+ */
+
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <time.h>
+#include <unistd.h>
+#include <math.h>
+
+#define NS_MULTI (1000 * 1000 * 1000)
+
+typedef struct timespec timespec_t;
+
+typedef struct angle_s {
+    double z;   // yaw
+    double y;   // pitch
+    double x;   // roll
+} angle_t;
+
+
+typedef struct quaternion_s {
+    double w;
+    double x;
+    double y;
+    double z;
+} quaternion_t;
+
+
+double radian2degrees(double radians) {
+    return radians * (180.0f / M_PI);
+}
+
+double degress2radian(double degress) {
+    return degress * (M_PI / 180.0f);
+}
+
+void quaternion_init(quaternion_t* q) {
+    q->w = 1.0f;
+    q->x = 0.0f;
+    q->y = 0.0f;
+    q->z = 0.0f;
+}
+
+void angle_init(angle_t* a) {
+    a->z = 0.0f;
+    a->y = 0.0f;
+    a->x = 0.0f;
+}
+
+quaternion_t quaternion_multi(const quaternion_t* q1, const quaternion_t* q2) {
+    quaternion_t r;
+    r.x = q1->w*q2->x + q1->x*q2->w + q1->y*q2->z - q1->z*q2->y;
+    r.y = q1->w*q2->y - q1->x*q2->z + q1->y*q2->w + q1->z*q2->x;
+    r.z = q1->w*q2->z + q1->x*q2->y - q1->y*q2->x + q1->z*q2->w;
+    r.w = q1->w*q2->w - q1->x*q2->x - q1->y*q2->y - q1->z*q2->z;
+    return r;
+}
+
+void quaternion_norm(quaternion_t* q) {
+    double len = sqrtf(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
+    q->w /= len;
+    q->x /= len;
+    q->y /= len;
+    q->z /= len;
+}
+
+quaternion_t zyx2quaternion(angle_t* a) {
+
+    quaternion_t q;
+
+    double hy = 0.5f * a->z;
+    double hp = 0.5f * a->y;
+    double hr = 0.5f * a->x;
+
+    double ys = sin(hy), yc = cos(hy);
+    double ps = sin(hp), pc = cos(hp);
+    double rs = sin(hr), rc = cos(hr);
+
+    q.w = (rc*pc*yc + rs*ps*ys);
+    q.x = (rs*pc*yc - rc*ps*ys);
+    q.y = (rc*ps*yc + rs*pc*ys);
+    q.z = (rc*pc*ys - rs*ps*yc);
+
+    return q;
+
+    // below here temp. hacks for error measures
+    //if (q.w >= 0.0f) return;
+    //quat_neg(q, q);
+}
+
+double sgn(double x) {
+    return copysignf(1.f,x);
+}
+
+angle_t quaternion2xyz(quaternion_t* q) {
+    angle_t a;
+
+    double x = q->x;
+    double y = q->y;
+    double z = q->z;
+    double w = q->w;
+
+    double t0 = (x + z)*(x - z);     // x^2-z^2
+    double t1 = (w + y)*(w - y);     // w^2-y^2
+
+    double xx = 0.5f * (t0 + t1);    // 1/2 x of x'
+    double xy = x*y + w*z;           // 1/2 y of x'
+    double xz = w*y - x*z;           // 1/2 z of x'
+
+    double t  = xx*xx + xy*xy;       // cos(theta)^2
+    double yz = 2.0f * (y*z + w*x);  // z of y'
+
+    a.z = atan2(xy, xx);             // yaw   (psi)
+    a.y = atan(xz /sqrt(t));         // pitch (theta)
+
+    if (t != 0) {
+        a.x = atan2(yz, t1 - t0);
+    } else {
+        a.x = (2.0 * atan2(x, w) - sgn(xz) * a.z);
+    }
+    return a;
+}
+
+int main(int argc, char **argv) {
+
+    quaternion_t q0;
+    quaternion_init(&q0);
+    printf("qw=%5.3f qx=%5.3f qy=%5.3f qz=%5.3f\n", q0.w, q0.x, q0.y, q0.z);
+
+    angle_t a0;
+    angle_init(&a0);
+    a0.x = 0.01f;
+    a0.y = 0.01f;
+    a0.z = 0.01f;
+
+    quaternion_t q1 = zyx2quaternion(&a0);
+    printf("qw=%5.3f qx=%5.3f qy=%5.3f qz=%5.3f\n", q1.w, q1.x, q1.y, q1.z);
+
+    quaternion_t q2 = q0;
+    for (int i = 0; i < 100; i++) {
+        q2 = quaternion_multi(&q1, &q2);
+        quaternion_norm(&q2);
+    }
+    printf("qw=%5.3f qx=%5.3f qy=%5.3f qz=%5.3f\n", q2.w, q2.x, q2.y, q2.z);
+
+
+    angle_t a1 = quaternion2xyz(&q2);
+
+    printf("a.x=%.3f a.y=%.3f a.z=%.3f \n", a1.x, a1.y, a1.z);
+
+    return 0;
+}