From e154d5730db6d03c47fe930936d2559f1f7a1b1d Mon Sep 17 00:00:00 2001 From: James Preiss Date: Sun, 24 Jul 2016 12:52:05 -0700 Subject: [PATCH] initial commit --- README.md | 20 +++ math3d.h | 516 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 536 insertions(+) create mode 100644 math3d.h diff --git a/README.md b/README.md index 24e50ac..f2a35ae 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,22 @@ # cmath3d 3d math library for C. Vectors, 3x3 matrices, quaternions. + +Unlike many other C libraries of this type, `cmath3d` passes arguments +and returns results by value instead of by pointer and pointer-to-output. +This choice has several motivations: + +- avoids the need to name non-meaningful intermediate results for the sake of taking their address +- enables nested expressions +- reduces bugs by allowing more variables to be delcared `const` +- gives the optimizing compiler complete knowledge about function semantics, + theoretically enabling better optimizations + (see [Chandler Carruth's talk](https://www.youtube.com/watch?v=eR34r7HOU14) + +Although Carruth's talk implies that using these functions with inline, +header-only definitions -- thus hiding no code from the compiler +in external object files -- will enable as good or better optimizations +than a pass-by-pointer version, in practice this library seems to consume +more stack memory than it should. +I am currently researching ways to fix this problem, since I believe +a SSA compiler with the ability to destructure structs should produce +optimal code from this library. diff --git a/math3d.h b/math3d.h new file mode 100644 index 0000000..dab580c --- /dev/null +++ b/math3d.h @@ -0,0 +1,516 @@ +#pragma once + +/* +The MIT License (MIT) + +cmath3d +Copyright (c) 2016 James Alan Preiss + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +#include +#include + + +// ----------------------------- 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; } + + +// ---------------------------- 3d vectors ------------------------------ + +struct vec { + float x; float y; float z; +}; + +// +// constructors +// + +// construct a vector from 3 floats. +static inline struct vec mkvec(float x, float y, float z) { + struct vec v; + v.x = x; v.y = y; v.z = z; + return v; +} +// construct a vector with the same value repeated for x, y, and z. +static inline struct vec vrepeat(float x) { + return mkvec(x, x, x); +} +// construct a zero-vector. +static inline struct vec vzero() { + return vrepeat(0.0f); +} + +// +// operators +// + +// multiply a vector by a scalar. +static inline struct vec vscl(float s, struct vec v) { + return mkvec(s * v.x , s * v.y, s * v.z); +} +// negate a vector. +static inline struct vec vneg(struct vec v) { + return mkvec(-v.x, -v.y, -v.z); +} +// divide a vector by a scalar. +// does not perform divide-by-zero check. +static inline struct vec vdiv(struct vec v, float s) { + return vscl(1.0f/s, v); +} +// add two vectors. +static inline struct vec vadd(struct vec a, struct vec b) { + return mkvec(a.x + b.x, a.y + b.y, a.z + b.z); +} +// subtract a vector from another vector. +static inline struct vec vsub(struct vec a, struct vec b) { + return vadd(a, vneg(b)); +} +// vector dot product. +static inline float vdot(struct vec a, struct vec b) { + return a.x * b.x + a.y * b.y + a.z * b.z; +} +// vector magnitude squared. +static inline float vmag2(struct vec v) { + return vdot(v, v); +} +// vector magnitude. +static inline float vmag(struct vec v) { + return sqrt(vmag2(v)); +} +// vector Euclidean distance squared. +static inline float vdist2(struct vec a, struct vec b) { + return vmag2(vsub(a, b)); +} +// vector Euclidean distance. +static inline float vdist(struct vec a, struct vec b) { + return sqrt(vdist2(a, b)); +} +// normalize a vector (make a unit vector). +static inline struct vec vnormalize(struct vec v) { + return vdiv(v, vmag(v)); +} +// vector cross product. +static inline struct vec vcross(struct vec a, struct vec b) { + return mkvec(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x); +} +// projection of a onto b, where b is a unit vector +static inline struct vec vprojectunit(struct vec a, struct vec b_unit) { + return vscl(vdot(a, b_unit), b_unit); +} +// component of a orthogonal to b, where b is a unit vector +static inline struct vec vorthunit(struct vec a, struct vec b_unit) { + return vsub(a, vprojectunit(a, b_unit)); +} + +// +// comparisons +// + +// compare two vectors for exact equality. +static inline bool veq(struct vec a, struct vec b) { + return (a.x == b.x) && (a.y == b.y) && (a.z == b.z); +} +// compare two vectors for exact inequality. +static inline bool vneq(struct vec a, struct vec b) { + return !veq(a, b); +} + +// +// 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) { + return vadd(vadd(a, b), c); +} +// subtract b and c from a. +static inline struct vec vsub2(struct vec a, struct vec b, struct vec c) { + return vadd3(a, vneg(b), vneg(c)); +} + +// +// conversion to/from raw float and double arrays. +// + +// load a vector from a double array. +static inline struct vec vload(double const *d) { + return mkvec(d[0], d[1], d[2]); +} +// store a vector into a double array. +static inline void vstore(struct vec v, double *d) { + d[0] = v.x; d[1] = v.y; d[2] = v.z; +} +// load a vector from a float array. +static inline struct vec vloadf(float const *f) { + return mkvec(f[0], f[1], f[2]); +} +// store a vector into a float array. +static inline void vstoref(struct vec v, float *f) { + f[0] = v.x; f[1] = v.y; f[2] = v.z; +} +// Vector TODO: abs, min, max, fuzzy comparison + + +// ---------------------------- 3x3 matrices ------------------------------ + +struct mat33 { + float m[3][3]; +}; + +// +// constructors +// + +// construct a zero matrix. +static inline struct mat33 mzero() { + struct mat33 m; + ZEROARR(m.m); + return m; +} +// construct a matrix with the given diagonal. +static inline struct mat33 diag(float a, float b, float c) { + struct mat33 m = mzero(); + m.m[0][0] = a; + m.m[1][1] = b; + m.m[2][2] = c; + return m; +} +// construct the matrix a * I for scalar a. +static inline struct mat33 eyescl(float a) { + return diag(a, a, a); +} +// construct an identity matrix. +static inline struct mat33 eye() { + return eyescl(1.0f); +} +// construct a matrix from three column vectors. +static inline struct mat33 mcolumns(struct vec a, struct vec b, struct vec c) { + struct mat33 m; + m.m[0][0] = a.x; + m.m[1][0] = a.y; + m.m[2][0] = a.z; + + m.m[0][1] = b.x; + m.m[1][1] = b.y; + m.m[2][1] = b.z; + + m.m[0][2] = c.x; + m.m[1][2] = c.y; + m.m[2][2] = c.z; + + return m; +} +// construct a matrix from three row vectors. +static inline struct mat33 mrows(struct vec a, struct vec b, struct vec c) { + struct mat33 m; + vstoref(a, m[0]); + vstoref(b, m[1]); + vstoref(c, m[2]); +} +// construct the matrix A from vector v such that Ax = cross(v, x) +static inline struct mat33 mcrossmat(struct vec v) { + struct mat33 m; + m.m[0][0] = 0; + m.m[0][1] = -v.z; + m.m[0][2] = v.y; + m.m[1][0] = v.z; + m.m[1][1] = 0; + m.m[1][2] = -v.x; + m.m[2][0] = -v.y; + m.m[2][1] = v.x; + m.m[2][2] = 0; + return m; +} + +// +// accessors +// + +// return one column of a matrix as a vector. +static inline struct vec mcolumn(struct mat33 m, int col) { + return mkvec(m.m[0][col], m.m[1][col], m.m[2][col]); +} +// return one row of a matrix as a vector. +static inline struct vec mrow(struct mat33 m, int row) { + return vloadf(m.m[row]); +} + +// +// operators +// + +// matrix transpose. +static inline struct mat33 mtranspose(struct mat33 m) { + struct mat33 mt; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + mt.m[i][j] = m.m[j][i]; + } + } + return mt; +} +// multiply a matrix by a scalar. +static inline struct mat33 mscale(float s, struct mat33 a) { + struct mat33 sa; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + sa.m[i][j] = s * a.m[i][j]; + } + } + return sa; +} +// negate a matrix. +static inline struct mat33 mneg(struct mat33 a) { + return mscale(-1.0, a); +} +// add two matrices. +static inline struct mat33 madd(struct mat33 a, struct mat33 b) { + struct mat33 c; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + c.m[i][j] = a.m[i][j] + b.m[i][j]; + } + } + return c; +} +// subtract two matrices. +static inline struct mat33 msub(struct mat33 a, struct mat33 b) { + struct mat33 c; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + c.m[i][j] = a.m[i][j] - b.m[i][j]; + } + } + return c; +} +// multiply two matrices. +static inline struct mat33 mmult(struct mat33 a, struct mat33 b) { + struct mat33 ab; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + float accum = 0; + for (int k = 0; k < 3; ++k) { + accum += a.m[i][k] * b.m[k][j]; + } + ab.m[i][j] = accum; + } + } + return ab; +} +// add a scalar along the diagonal, i.e. a + dI. +static inline struct mat33 maddridge(struct mat33 a, float d) { + a.m[0][0] += d; + a.m[1][1] += d; + a.m[2][2] += d; + return a; +} +// 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) { + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + block[j] = m.m[i][j]; + } + block += stride; + } +} + +// Matrix TODO: mmultv, inv, solve, eig, raw floats + + +// ---------------------------- quaternions ------------------------------ + +struct quat { + union { + struct vec v; + struct { + float x; + float y; + float z; + }; + }; + float w; +}; + +// +// constructors +// + +// construct a quaternion from its x, y, z, w elements. +static inline struct quat mkquat(float x, float y, float z, float w) { + struct quat q; + q.v = mkvec(x, y, z); + q.w = w; + return q; +} +// construct a quaternion from a vector containing (x, y, z) and a scalar w. +// note that this is NOT an axis-angle constructor. +static inline struct quat quatvw(struct vec v, float w) { + struct quat q; + q.v = v; + q.w = w; + return q; +} +// construct an identity quaternion. +static inline struct quat qeye() { + 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); + struct quat q; + q.v = vscl(scale, axis); + q.w = cos(angle/2); + return q; +} +// APPROXIMATE conversion of small (roll, pitch, yaw) Euler angles +// into a quaternion without computing any trig functions. +// only produces useful result for small angles. +// Example application is integrating a gyroscope when the angular velocity +// of the object is small compared to the sampling frequency. +static inline struct quat rpy2quat_small(struct vec rpy) { + float q2 = vmag2(rpy) / 4.0f; + if (q2 < 1) { + return quatvw(vdiv(rpy, 2), sqrtf(1.0f - q2)); + } + else { + float w = 1.0f / sqrtf(1.0f + q2); + return quatvw(vscl(w/2, rpy), w); + } +} + +// +// conversions to other parameterizations of 3D rotations +// + +// convert quaternion to (roll, pitch, yaw) Euler angles +static inline struct vec quat2rpy(struct quat q) { + struct vec v; + v.x = atan2(2 * (q.w * q.x + q.y * q.z), 1 - 2 * (fsqr(q.x) + fsqr(q.y))); // roll + v.y = asin(2 * (q.w * q.y - q.x * q.z)); // pitch + v.z = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (fsqr(q.y) + fsqr(q.z))); // yaw + return v; +} +// compute the axis of a quaternion's axis-angle decomposition. +static inline struct vec quat2axis(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, q.v); +} +// compute the angle of a quaternion's axis-angle decomposition. +static inline float quat2angle(struct quat q) { + return 2 * acos(q.w); +} +// convert a quaternion into a 3x3 rotation matrix. +static inline struct mat33 quat2rotmat(struct quat q) { + float x = q.x; + float y = q.y; + float z = q.z; + float w = q.w; + + struct mat33 m; + m.m[0][0] = 1 - 2*y*y - 2*z*z; + m.m[0][1] = 2*x*y - 2*z*w; + m.m[0][2] = 2*x*z + 2*y*w, + m.m[1][0] = 2*x*y + 2*z*w; + m.m[1][1] = 1 - 2*x*x - 2*z*z; + m.m[1][2] = 2*y*z - 2*x*w, + m.m[2][0] = 2*x*z - 2*y*w; + m.m[2][1] = 2*y*z + 2*x*w; + m.m[2][2] = 1 - 2*x*x - 2*y*y; + return m; +} + +// +// operators +// + +// rotate a vector by a quaternion. +static inline struct vec qvrot(struct quat q, struct vec v) { + // from http://gamedev.stackexchange.com/a/50545 - TODO find real citation + return vadd3( + vscl(2.0f * vdot(q.v, v), q.v), + vscl(q.w * q.w - vdot(q.v, q.v), v), + vscl(2.0f * q.w, vcross(q.v, v)) + ); +} +// multiply (compose) two quaternions +// such that qvrot(qqmul(q, p), v) == qvrot(q, qvrot(p, v)) +static inline struct quat qqmul(struct quat q, struct quat p) { + float x = q.w*p.x + q.z*p.y - q.y*p.z + q.x*p.w; + float y = -q.z*p.x + q.w*p.y + q.x*p.z + q.y*p.w; + float z = q.y*p.x - q.x*p.y + q.w*p.z + q.z*p.w; + float w = -q.x*p.x - q.y*p.y - q.z*p.z + q.w*p.w; + return mkquat(x, y, z, w); +} +// invert a quaternion +static inline struct quat qinv(struct quat q) { + return quatvw(vneg(q.v), q.w); +} +// normalize a quaternion. +// typically used to mitigate precision errors. +static inline struct quat qnormalize(struct quat q) { + float maginv = 1.0f / sqrtf(vmag2(q.v) + fsqr(q.w)); + return quatvw(vscl(maginv, q.v), maginv * q.w); +} +// update an attitude estimate quaternion with a reading from a gyroscope +// 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; + + q1.v.x = quat.x + y*quat.y - p*quat.z + r*quat.w; + q1.v.y = -y*quat.x + quat.y + r*quat.z + p*quat.w; + q1.v.z = p*quat.x - r*quat.y + quat.z + y*quat.w; + q1.w = -r*quat.x - p*quat.y - y*quat.z + quat.w; + return q1; +} + +// +// conversion to/from raw float and double arrays. +// + +// load a quaternion from a raw double array. +static inline struct quat qload(double const *d) { + return mkquat(d[0], d[1], d[2], d[3]); +} +// store a quaternion into a raw double array. +static inline void qstore(struct quat q, double *d) { + d[0] = q.x; d[1] = q.y; d[2] = q.z; d[3] = q.w; +} +// load a quaternion from a raw float array. +static inline struct quat qloadf(float const *f) { + return mkquat(f[0], f[1], f[2], f[3]); +} +// store a quaternion into a raw float array. +static inline void qstoref(struct quat q, float *f) { + f[0] = q.x; f[1] = q.y; f[2] = q.z; f[3] = q.w; +} + +// Quaternion TODO: rpy2quat, slerp -- 2.20.1