157 lines
3.3 KiB
C
157 lines
3.3 KiB
C
/*
|
|
*
|
|
* 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;
|
|
}
|