From 704ee30b8b72e10fa24eb427e5863261b816fc64 Mon Sep 17 00:00:00 2001
From: James Alan Preiss <jamesalanpreiss@gmail.com>
Date: Mon, 18 Dec 2017 16:06:24 -0800
Subject: [PATCH] fix double promotions, add (void) decls
also add a few new quaternion decomposition functions
---
math3d.h | 51 +++++++++++++++++++++++++++++++++++----------------
1 file changed, 35 insertions(+), 16 deletions(-)
diff --git a/math3d.h b/math3d.h
index e61332d..e58b8cd 100644
--- a/math3d.h
+++ b/math3d.h
@@ -26,14 +26,18 @@ SOFTWARE.
*/
#include <math.h>
-#include <stdbool.h>
+#include <stdbool.h>
+
+#ifndef M_PI_F
+#define M_PI_F (3.14159265358979323846f)
+#endif
// ----------------------------- scalars --------------------------------
static inline float fsqr(float x) { return x * x; }
-static inline float radians(float degrees) { return (M_PI / 180.0) * degrees; }
-static inline float degrees(float radians) { return (180.0 / M_PI) * radians; }
+static inline float radians(float degrees) { return (M_PI_F / 180.0f) * degrees; }
+static inline float degrees(float radians) { return (180.0f / M_PI_F) * radians; }
// ---------------------------- 3d vectors ------------------------------
@@ -57,7 +61,7 @@ static inline struct vec vrepeat(float x) {
return mkvec(x, x, x);
}
// construct a zero-vector.
-static inline struct vec vzero() {
+static inline struct vec vzero(void) {
return vrepeat(0.0f);
}
@@ -134,6 +138,10 @@ static inline struct vec vmin(struct vec a, struct vec b) {
static inline struct vec vmax(struct vec a, struct vec b) {
return mkvec(fmaxf(a.x, b.x), fmaxf(a.y, b.y), fmaxf(a.z, b.z));
}
+// L1 norm (aka Minkowski, Taxicab, Manhattan norm) of a vector.
+static inline float vnorm1(struct vec v) {
+ return fabs(v.x) + fabs(v.y) + fabs(v.z);
+}
//
// comparisons
@@ -170,7 +178,7 @@ static inline bool visnan(struct vec v) {
//
// special functions to ease the pain of writing vector math in C.
-//
+//
// add 3 vectors.
static inline struct vec vadd3(struct vec a, struct vec b, struct vec c) {
@@ -210,12 +218,12 @@ struct mat33 {
float m[3][3];
};
-//
+//
// constructors
//
// construct a zero matrix.
-static inline struct mat33 mzero() {
+static inline struct mat33 mzero(void) {
struct mat33 m;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
@@ -237,7 +245,7 @@ static inline struct mat33 eyescl(float a) {
return diag(a, a, a);
}
// construct an identity matrix.
-static inline struct mat33 eye() {
+static inline struct mat33 eye(void) {
return eyescl(1.0f);
}
// construct a matrix from three column vectors.
@@ -383,10 +391,10 @@ static inline bool misnan(struct mat33 m) {
// set a 3x3 block within a big row-major matrix.
// block: pointer to the upper-left element of the block in the big matrix.
// stride: the number of columns in the big matrix.
-static inline void set_block33_rowmaj(float *block, int stride, struct mat33 m) {
+static inline void set_block33_rowmaj(float *block, int stride, struct mat33 const *m) {
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
- block[j] = m.m[i][j];
+ block[j] = m->m[i][j];
}
block += stride;
}
@@ -423,12 +431,12 @@ static inline struct quat quatvw(struct vec v, float w) {
return q;
}
// construct an identity quaternion.
-static inline struct quat qeye() {
+static inline struct quat qeye(void) {
return mkquat(0, 0, 0, 1);
}
// construct a quaternion from an axis and angle of rotation.
static inline struct quat qaxisangle(struct vec axis, float angle) {
- float scale = sin(angle / 2) / vmag(axis);
+ float scale = sinf(angle / 2) / vmag(axis);
struct quat q;
q.x = scale * axis.x;
q.y = scale * axis.y;
@@ -436,6 +444,14 @@ static inline struct quat qaxisangle(struct vec axis, float angle) {
q.w = cos(angle/2);
return q;
}
+static inline struct vec quataxis(struct quat q) {
+ // TODO this is not numerically stable for tiny rotations
+ float s = 1.0f / sqrtf(1 - q.w * q.w);
+ return vscl(s, mkvec(q.x, q.y, q.z));
+}
+static inline float quatangle(struct quat q) {
+ return 2 * acosf(q.w);
+}
// APPROXIMATE conversion of small (roll, pitch, yaw) Euler angles
// into a quaternion without computing any trig functions.
// only produces useful result for small angles.
@@ -451,6 +467,9 @@ static inline struct quat rpy2quat_small(struct vec rpy) {
return quatvw(vscl(w/2, rpy), w);
}
}
+static inline struct vec quatimagpart(struct quat q) {
+ return mkvec(q.x, q.y, q.z);
+}
//
// conversions to other parameterizations of 3D rotations
@@ -537,14 +556,14 @@ static inline struct quat qnormalize(struct quat q) {
return mkquat(s*q.x, s*q.y, s*q.z, s*q.w);
}
// update an attitude estimate quaternion with a reading from a gyroscope
-// over the timespan dt. Gyroscope is assumed (roll, pitch, yaw)
+// over the timespan dt. Gyroscope is assumed (roll, pitch, yaw)
// angular velocities in radians per second.
static inline struct quat quat_gyro_update(struct quat quat, struct vec gyro, float const dt) {
// from "Indirect Kalman Filter for 3D Attitude Estimation", N. Trawny, 2005
struct quat q1;
- double const r = (dt / 2) * gyro.x;
- double const p = (dt / 2) * gyro.y;
- double const y = (dt / 2) * gyro.z;
+ float const r = (dt / 2) * gyro.x;
+ float const p = (dt / 2) * gyro.y;
+ float const y = (dt / 2) * gyro.z;
q1.x = quat.x + y*quat.y - p*quat.z + r*quat.w;
q1.y = -y*quat.x + quat.y + r*quat.z + p*quat.w;
--
2.20.1