From 3993c5a52ae9553f7574b3b5938ce1da0d842695 Mon Sep 17 00:00:00 2001 From: Ravi Prakash Singh Date: Sat, 2 Apr 2022 18:05:36 +0530 Subject: [PATCH] Quaternion (#7) * WIP * WIP * Implemented Quaternions quat.h & quat.c --- include/hpml/constants.h | 13 + include/hpml/defines.h | 8 +- include/hpml/quat.h | 267 ++++++++++++++++- include/hpml/quat/header_config.h | 19 -- include/hpml/quat/quat.h | 10 - include/hpml/quat/source_config.h | 19 -- include/hpml/quat/template_definitions.h | 66 ----- include/hpml/quat/template_instantiations.h | 6 - source/main.c | 23 ++ source/quat.c | 301 ++++++++++++++++++++ 10 files changed, 609 insertions(+), 123 deletions(-) create mode 100644 include/hpml/constants.h delete mode 100644 include/hpml/quat/header_config.h delete mode 100644 include/hpml/quat/quat.h delete mode 100644 include/hpml/quat/source_config.h delete mode 100644 include/hpml/quat/template_definitions.h delete mode 100644 include/hpml/quat/template_instantiations.h create mode 100644 source/quat.c diff --git a/include/hpml/constants.h b/include/hpml/constants.h new file mode 100644 index 0000000..2bb3d44 --- /dev/null +++ b/include/hpml/constants.h @@ -0,0 +1,13 @@ + + +#pragma once + + +#define PI (3.14159f) +#define HALF_PI (1.57f) +#define SQRT_3 // \/3 +#define SQRT_2 // \/2 +#define SQRT_5 // \/5 +#define SQRT_7 // \/7 +#define GOLDEN_RATIO + diff --git a/include/hpml/defines.h b/include/hpml/defines.h index 49b6834..e780b09 100644 --- a/include/hpml/defines.h +++ b/include/hpml/defines.h @@ -3,6 +3,7 @@ #include #include +#include typedef uint16_t u16; typedef int16_t s16; @@ -16,7 +17,7 @@ typedef int64_t s64; typedef uint8_t u8; typedef int8_t s8; -#define IGNORE_CONST(type, value) *(type*)(&value) +#define IGNORE_CONST(type, value) (*(type*)(&value)) #ifdef GLOBAL_DEBUG @@ -42,3 +43,8 @@ do\ #else # define HPML_API #endif + + +#define HPML_INLINE inline +#define HPML_FORCE_INLINE __attribute__((always_inline)) inline + diff --git a/include/hpml/quat.h b/include/hpml/quat.h index 3a94160..7c98cd1 100644 --- a/include/hpml/quat.h +++ b/include/hpml/quat.h @@ -1,6 +1,269 @@ +/* + + Resources: + + https://en.wikipedia.org/wiki/Quaternion + https://math.stackexchange.com/questions/2552/the-logarithm-of-quaternion + https://en.wikipedia.org/wiki/Slerp + https://web.mit.edu/2.998/www/QuaternionReport1.pdf + https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation + https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions + https://en.wikipedia.org/wiki/Three-dimensional_rotation_operator + https://en.wikipedia.org/wiki/Euler%27s_rotation_theorem + https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation + https://en.wikipedia.org/wiki/Spherical_coordinate_system + + https://math.stackexchange.com/questions/939229/unit-quaternion-to-a-scalar-power + + */ + + #pragma once -#include -#include +#include + +typedef struct quat_t +{ + /* + vector part = { x, y, z }, + scalar part = { w } + */ + union + { + struct + { + float x, y, z, w; + }; + + struct + { + float v[3]; + float s; + }; + }; +} quat_t; + +#define QUAT (quat_t) + +/* quat : constructs a quat_t instance + x, y, z, w : components of the quaternion + returns: an initialized instance of quat_t struct +*/ +static HPML_API HPML_FORCE_INLINE quat_t quat(float x, float y, float z, float w) { return QUAT { x, y, z, w }; } + +/* + quat_identity : consturcts an identity quaternion instance + returns: identity quaternion + */ +static HPML_API HPML_FORCE_INLINE quat_t quat_identity() { return quat(0, 0, 0, 1); } + +/* + quat_zero : constructs a null quaternion instance + returns : null quaternion + */ +static HPML_API HPML_FORCE_INLINE quat_t quat_zero() { return quat(0, 0, 0, 0); } + +/* quat_add : adds variable number of quaternions into q (quat_t) + count : number of variable quaternions to add + q : first quaternion + ... : variable number of quaternions + returns : result of the addition +*/ +HPML_API quat_t quat_add(u32 count, quat_t q, ...); +HPML_API quat_t __quat_add(quat_t q1, quat_t q2); + +/* quat_sub : subtracts variable number of quaternions from q (quat_t) + count : number of variable quaternions to add + q : first quaternion + ... : variable number of quaternions + returns : result of the subtraction +*/ +HPML_API quat_t quat_sub(u32 count, quat_t q, ...); +HPML_API quat_t __quat_sub(quat_t q1, quat_t q2); + +/* quat_add : multiplies variable number of quaternions with q (quat_t) + count : number of variable quaternions to multiply + q : first quaternion + ... : variable number of quaternions + returns : result of the multiplication +*/ +HPML_API quat_t quat_mul(u32 count, quat_t q, ...); +HPML_API quat_t __quat_mul(quat_t q1, quat_t q2); + +/* quat_add : divides q (quat_t) by variable number of quaternions + count : number of variable quaternions to divide with + q : first quaternion + ... : variable number of quaternions + returns : result of the division +*/ +HPML_API quat_t quat_div(u32 count, quat_t q, ...); +HPML_API quat_t __quat_div(quat_t q1, quat_t q2); + +/* + quat_mul_scalar : multiplies each component of the quaternion with a scalar value + q : original quaternion + s : scalar floating point value + returns : q * s + */ +static HPML_API HPML_FORCE_INLINE quat_t quat_mul_scalar(quat_t q, float s) { return quat(q.x * s, q.y * s, q.z * s, q.w * s); } + +/* quat_difference : calculates the rotation difference between two quaternions q1 and q2 + q1 : first quaternion (final rotation) + q2 : second quaternion (intial rotation) + returns: quaternion equivalent to the difference of the two quaternions q1 and q2 + +NOTE: difference(q1, q0) = q1 * inverse(q0) + +*/ +HPML_API quat_t quat_difference(quat_t q1, quat_t q2); + +/* + quat_inverse : calculates the inverse of a quaternion + q : original quaternion + returns: inverse of the quaterion 'q' + */ +HPML_API quat_t quat_inverse(quat_t q); + +/* + quat_conjugate : calculates the conjugate of a quatenion + q : original quaternion + returns : conjugate of the quaterion 'q' + */ +static HPML_API HPML_FORCE_INLINE quat_t quat_conjugate(quat_t q) { return quat(-q.x, -q.y, -q.z, q.w); } +#define quat_conj(x) quat_conjugate(x) + +/* + quat_reciprocal : calculates the reciprocal of a quaternion + q : original quaternion + returns: reciprocal of the quaternion 'q' + +NOTE: 1 / q = conjugate(q) / (q * conjugate(q)) = conjugate(q) / sqrmagnitude(q); + + */ +HPML_API quat_t quat_reciprocal(quat_t q); + +/* + quat_sqrt : calcualtes the square root of a quaternion + q : original quaternion + returns: square root of the quaternion 'q' + */ +HPML_API quat_t quat_sqrt(quat_t q); + +/* + quat_log : calculates the logarithm of a quaternion + q : original quaternion + base : scalar base + returns: logarithm of the quaternion 'q' + */ +HPML_API quat_t quat_log(quat_t q, float base); + +/* + quat_pow : calculates power of a quaternion + q : base quaternion + t : exponent + returns: power of the quaternion 'q' + */ +HPML_API quat_t quat_pow(quat_t q, float t); + +/* + quat_magnitude : calculates the magnitude of a quaternion + q : quaternion + returns: magnitude of the quaternion 'q' + */ +HPML_API float quat_magnitude(quat_t q); + +/* + quat_sqrmagnitude : calculates the squared magnitude of a quaternion, efficient than magnitude version + q : quaternion + returns: squared magnitude of the quaternion 'q' + */ +HPML_API float quat_sqrmagnitude(quat_t q); + +/* + quat_normalize : calculates a unit quaternion from a quaternion + q : original quaternion + returns: unit quaterion of the quaterion 'q' + */ +HPML_API quat_t quat_normalize(quat_t q); + +/* + quat_angle_axis : calculates a quaterion rotor with angle along an axis + x : x coordinate of the axis + y : y coordinate of the axis + z : z coordinate of the axis + angle : angle (in radians), +ve is anticlockwise, -ve is clockwise + returns: quaterion rotor + */ +HPML_API quat_t quat_angle_axis(float x, float y, float z, float angle); + +/* + quat_versor : calculates a quaternion versor + x : x coordiante of the imaginary part (r) + y : y coordinate of the imaginary part (r) + z : z coordinate of the imaginary part (r) + angle : ange (in radians) + +NOTE: + versor = exp(r * angle) = cos(angle) + sin(angle) * r; + */ +HPML_API quat_t quat_versor(float x, float y, float z, float angle); + +/* + quat_angle : calculates an angle between two quaternions + q1 : first quaterion (from) + q2 : second quaterion (to) + returns: angle (in radians), +ve is anticlockwise, -ve is clockwise + */ +HPML_API float quat_angle(quat_t q1, quat_t q2); + +/* + quat_lerp : calculates a linearly interpolated quaternion + from : initial quaternion + to : final quaternion + t : interpolation parameter + returns: interpolated quaternion or { from * (1 - t) + to * t } + */ +HPML_API quat_t quat_lerp(quat_t from, quat_t to, float t); + +/* + quat_slerp : calculates a spherically interpolated quaternion + from : intial quaternion + to : final quaternion + t : interpolation paramter + returns : interpolated quaterion OR + +NOTE: + versor : q = exp(r * angle) = cos(angle * 0.5f) + r * sin(angle * 0.5f), + where r * r = -1, which is a quaternion with zero scalar part and unit vector part + + slerp(q0, q1, t) = pow(q1 * inverse(q0), t) * q0 + OR + slerp(q0, q1, t) = pow(quat_difference(q1, q0), t) * q0 + */ +HPML_API quat_t quat_slerp(quat_t from, quat_t to, float t); + +/* + quat_sandwitch : calculates the sandwitched multiplication q * p * inverse(q), where q is a quaterion versor + versor : quaternion q in the sandwitched multiplication + p : quaternion p in the sandwitched multiplication + returns: sandwitched product (q * p * inverse(q)) + */ +HPML_API quat_t quat_sandwitch(quat_t versor, quat_t p); + +/* + Comparison functions + */ + +/* + quat_equal : determines whether two quaternions are equal or not + q1 : first quaternion + q2 : quaternion to compare against + returns: true if both are equal to each other; false otherwise. + */ +HPML_API bool quat_equal(quat_t q1, quat_t q2); + +/* For debugging purpose */ +HPML_API void quat_print(quat_t q); diff --git a/include/hpml/quat/header_config.h b/include/hpml/quat/header_config.h deleted file mode 100644 index d5aa7ff..0000000 --- a/include/hpml/quat/header_config.h +++ /dev/null @@ -1,19 +0,0 @@ -/*Begin: Header Configuration System*/ - -/*set the configuration, for this file*/ -#define HEADER_CONFIGURATION_SYSTEM_HEADER - -#ifdef GLOBAL_DEBUG -# ifndef DEBUG -# define DEBUG -# endif -#endif - -#ifdef GLOBAL_RELEASE -# ifndef RELEASE -# define RELEASE -# endif -#endif - -#include -/*End: Header Configuration System*/ \ No newline at end of file diff --git a/include/hpml/quat/quat.h b/include/hpml/quat/quat.h deleted file mode 100644 index f2d41b2..0000000 --- a/include/hpml/quat/quat.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef __HPML_QUAT_H__ -#define __HPML_QUAT_H__ - -#include - - -#include - -#include -#endif/*__HPML_QUAT_H__*/ \ No newline at end of file diff --git a/include/hpml/quat/source_config.h b/include/hpml/quat/source_config.h deleted file mode 100644 index 86bd48b..0000000 --- a/include/hpml/quat/source_config.h +++ /dev/null @@ -1,19 +0,0 @@ -/*Begin: Header Configuration System*/ - -/*set the configuration, for this file*/ -#define HEADER_CONFIGURATION_SYSTEM_IMPLEMENTATION - -#ifdef GLOBAL_DEBUG -# ifndef DEBUG -# define DEBUG -# endif -#endif - -#ifdef GLOBAL_RELEASE -# ifndef RELEASE -# define RELEASE -# endif -#endif - -#include -/*End: Header Configuration System*/ \ No newline at end of file diff --git a/include/hpml/quat/template_definitions.h b/include/hpml/quat/template_definitions.h deleted file mode 100644 index 50049c4..0000000 --- a/include/hpml/quat/template_definitions.h +++ /dev/null @@ -1,66 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include - -/* Begin: template signatures*/ -#define quat_t(T) c_template(quat_t, T) -#define quat(T) c_template(quat, T) -#define quat_add(T) c_template(quat_add, T) -#define quat_sub(T) c_template(quat_sub, T) -#define quat_mul(T) c_template(quat_mul, T) -#define quat_div(T) c_template(quat_div, T) -#define quat_mul_component_wise(T) c_template(quat_mul_component_wise, T) -#define quat_identity(T) NOT_IMPLEMENTED -#define quat_inverse(T) c_template(quat_inverse, T) -/* End: template signatures*/ - -/* Begin: template declarations*/ -#define instantiate_declaration_quat(T) HPML_API quat_t(T) quat(T)(T x, T y, T z, T w) -#define instantiate_declaration_quat_add(T) HPML_API quat_t(T) quat_add(T)(quat_t(T) q1, quat_t(T) q2) -#define instantiate_declaration_quat_sub(T) HPML_API quat_t(T) quat_sub(T)(quat_t(T) q1, quat_t(T) q2) -#define instantiate_declaration_quat_mul(T) HPML_API quat_t(T) quat_mul(T)(quat_t(T) q1, quat_t(T) q2) -#define instantiate_declaration_quat_div(T) HPML_API quat_t(T) quat_div(T)(quat_t(T) q1, quat_t(T) q2) -#define instantiate_declaration_quat_mul_component_wise(T) HPML_API quat_t(T) quat_mul_component_wise(T)(quat_t(T) q1, quat_t(T) q2) -#define instantiate_declaration_quat_identity(T) NOT_IMPLEMENTED -#define instantiate_declaration_quat_inverse(T) HPML_API quat_t(T) quat_inverse(T)(quat_t(T) q) -/* End: template dclarations*/ - -/* Begin: template definitiosn*/ -#define instantiate_quat_struct(T)\ -typedef struct quat_t(T)\ -{ - T x;\ - T y;\ - T z;\ - T w;\ -} quat_t(T) - -#define instantiate_implementation_quat(T)\ -HPML_API quat_t(T) quat(T)(T x, T y, T z, T w)\ -{\ - quat_t(T) q = { x, y, z, w };\ - return q;\ -} - -#define instantiate_implementation_quat_add(T)\ -HPML_API quat_t(T) quat_add(T)(quat_t(T) q1, quat_t(T) q2)\ -{\ - quat_t(T) q = { q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w };\ - return q;\ -} - -#define instantiate_implementation_quat_sub(T)\ -HPML_API quat_t(T) quat_sub(T)(quat_t(T) q1, quat_t(T) q2)\ -{\ - quat_t(T) q = { q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w };\ - return q;\ -} - -/* End: template definitions*/ - -#endif \ No newline at end of file diff --git a/include/hpml/quat/template_instantiations.h b/include/hpml/quat/template_instantiations.h deleted file mode 100644 index e56967d..0000000 --- a/include/hpml/quat/template_instantiations.h +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef __HPML_QUAT_TEMPLATE_INSTANTIATIONS_H__ -#define __HPML_QUAT_TEMPLATE_INSTANTIATIONS_H__ - -#include - -#endif /*__HPML_QUAT_TEMPLATE_INSTANTIATIONS_H__*/ \ No newline at end of file diff --git a/source/main.c b/source/main.c index 6a503d5..df759f0 100644 --- a/source/main.c +++ b/source/main.c @@ -5,10 +5,33 @@ #include #include +#include // quaternions +#include + #include +void quaternion_test() +{ + puts("-------------- Quaternion Tests ----------------"); + + quat_t q = quat(4, 5, 6, 7); + quat_t rotor = quat_angle_axis(0, 1, 0, PI * 0.3f); + quat_print(rotor); + + quat_t q1 = quat_angle_axis(0, 1, 0, PI * 0.3f); + quat_t q2 = quat_angle_axis(0, 1, 0, PI * 0.6f); + + quat_print(quat_slerp(q1, q2, 0.5f)); + + puts("--------------- Success ------------------------"); +} + int main() { + // Perform quaternion tests + quaternion_test(); + + vec4_t(float) v = vec4_zero(float)(); vec4_print(float)(v); diff --git a/source/quat.c b/source/quat.c new file mode 100644 index 0000000..d99fcb7 --- /dev/null +++ b/source/quat.c @@ -0,0 +1,301 @@ + +#include +#include + +#include + +static HPML_FORCE_INLINE void cross(const float* const from, const float* const to, float* const out) +{ + /* + | i j k | + | a1 a2 a3 | + | b1 b2 b3 | + + i(a2 * b3 - a3 * b2) - j(a1 * b3 - a3 * b1) + k(a1 * b2 - a2 * b1) + */ + + out[0] = from[1] * to[2] - from[2] * to[1]; + out[1] = from[0] * to[2] - from[2] * to[0]; + out[2] = from[0] * to[1] - from[1] * to[0]; +} + +static HPML_FORCE_INLINE float dot(const float* const from, const float* const to) +{ + return from[0] * to[0] + from[1] * to[1] + from[2] * to[2]; +} + +HPML_API quat_t quat_add(u32 count, quat_t q, ...) +{ + va_list args; + va_start(args, q); + + while(count) + { + q = __quat_add(q, va_arg(args, quat_t)); + --count; + } + + va_end(args); + + return q; +} + +HPML_API quat_t __quat_add(quat_t q1, quat_t q2) +{ + return quat(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w); +} + +HPML_API quat_t quat_sub(u32 count, quat_t q, ...) +{ + va_list args; + va_start(args, q); + + while(count) + { + q = __quat_sub(q, va_arg(args, quat_t)); + --count; + } + + va_end(args); + + return q; +} + +HPML_API quat_t __quat_sub(quat_t q1, quat_t q2) +{ + return quat(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w); +} + +HPML_API quat_t quat_mul(u32 count, quat_t q, ...) +{ + va_list args; + va_start(args, q); + + while(count) + { + q = __quat_mul(q, va_arg(args, quat_t)); + --count; + } + + va_end(args); + + return q; +} + +HPML_API quat_t __quat_mul(quat_t q1, quat_t q2) +{ + float v[3]; + cross(q1.v, q2.v, v); + float x = q1.s * q2.x + q2.s * q1.x + v[0]; + float y = q1.s * q2.y + q2.s * q1.y + v[1]; + float z = q1.s * q2.z + q2.s * q1.z + v[2]; + float s = q1.s * q2.s - (q1.x * q2.x + q1.y * q2.y + q1.z * q2.z); + return quat(x, y, z, s); +} + +HPML_API quat_t quat_div(u32 count, quat_t q, ...) +{ + va_list args; + va_start(args, q); + + while(count) + { + q = __quat_div(q, va_arg(args, quat_t)); + --count; + } + + va_end(args); + + return q; +} + +HPML_API quat_t __quat_div(quat_t q1, quat_t q2) +{ + quat_t q = __quat_mul(q1, quat_conj(q2)); + float sqrm = 1 / quat_sqrmagnitude(q2); + return quat(q.x * sqrm, q.y * sqrm, q.z * sqrm, q.w * sqrm); +} + +HPML_API quat_t quat_difference(quat_t q1, quat_t q2) +{ + // q1 * inverse(q2) + return __quat_mul(q1, quat_inverse(q2)); +} + +HPML_API quat_t quat_inverse(quat_t q) +{ + // q * conj(q) = sqr(magnitude(q)) + // implies => conj(q) / sqr(magnitude(q)) = inverse(q) + float sqrm = 1 / quat_sqrmagnitude(q); + return quat(-q.x * sqrm, -q.y * sqrm, -q.z * sqrm, q.w * sqrm); +} + +HPML_API quat_t quat_reciprocal(quat_t q) +{ + /* + 1 / q = conj(q) / { conj(q) * q }; + 1 / q = conj(q) / sqr_mag(q); + */ + return quat_mul_scalar(quat_conjugate(q), 1 / quat_sqrmagnitude(q)); +} + +HPML_API quat_t quat_sqrt(quat_t q) +{ + /* ___ + \/ q = sqrt(q) + + let \/(x, y, z, w) = (a, b, c, d) + then (x, y, z, w) = mul((a, b, c, d), (a, b, c, d)) + (x, y, z, w) = (a ^ 2 - b ^ 2 - c ^ 2 - d ^ 2, 2ab, 2ac, 2ad) + x = a ^ 2 - b ^ 2 - c ^ 2 - d ^ 2 equation 1 + y = 2ab equation 2 + z = 2ac equation 3 + w = 2ad equation 4 + solving 1, 2, 3, 4 we will get the value of a ^ 2 = something + so for each value of a we have two values of b and so on ... + therefore, total solution = 2 ^ 4 = 32 solutions + + */ + return quat_identity(); // TO BE IMPLEMENTED +} + +HPML_API quat_t quat_log(quat_t q, float base) +{ + /* + logarithm of a complex number: + ------------------------------ + + let log(x + iy) = a + ib + or + log(x, y) = (a, b) + + since complex number are also numbers (much like real numbers) + (x, y) = exp(a, b) + => (x, y) = exp(a) * exp(ib) + + by applying euler's identity + (x, y) = exp(a) * (cos(b) + isin(b)) + (x, y) = exp(a) * (cos(b), sin(b)) + (x, y) = (exp(a)cos(b), exp(a)sin(b)) + => x = exp(a)cos(b), - (1) + and y = exp(a)sin(b) - (2) + + solving (1) and (2) + + we get _________ + log(x + iy) = (0.5 * log(x^2 + y^2)) + iacos(x / \/ x^2 + y^2) + + logarithm of a general quaternion: + --------------------------- + + let log(x + iy + jz + kw) = a + ib + jc + kd + or + log(x, y, z, w) = (a, b, c, d) + + lets treat quaternions just like complex numbers + (x, y, z, w) = exp(a, b, c, d) + = exp(a) * exp(b, c, d) + applying euler's identity for quaternions + (x, y, z, w) = exp(a) * (cos(mag(b, c, d)) + norm(b, c, d)sin(mag(b, c, d))) + - Not solved + + + logarithm of a unit quaternion: + ------------------------------- + + let q = cos($) + n * sin($), where n is a unit vector and mag(q) = 1 + + log(q) = log(exp(n * $)) = n * $ = (0, n * $) + + likewise, logarithm of a general quaternion should be: + + log q = s + v + + log(q) = log( mag(v) * exp(unit(v) * acos(s / mag(v))) ) + = log( mag(v) ) + log( exp(unit(v) * acos(a / mag(v))) ) + = log( mag(v) ) + (0, n * $) + = (log( mag (v) ), n * $) + + */ + return quat_identity(); // TO BE IMPLEMENTED +} + +//https://math.stackexchange.com/questions/939229/unit-quaternion-to-a-scalar-power +HPML_API quat_t quat_pow(quat_t q, float t) +{ + + /* + raising to a power a quaternion: + + Let q be a unit quaterion exp(n * $), where mag(n) = 1 + + exp(n * $) ^ t = exp(n * $ * t) = cos($*t) + sin($*t)n + + */ + return quat_identity(); // TO BE IMPLEMENTED +} + +HPML_API float quat_magnitude(quat_t q) +{ + return sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); +} + +HPML_API float quat_sqrmagnitude(quat_t q) +{ + return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; +} + +HPML_API quat_t quat_normalize(quat_t q) +{ + return quat_mul_scalar(q, 1 / quat_magnitude(q)); +} + +HPML_API quat_t quat_angle_axis(float x, float y, float z, float angle) +{ + float ha = angle * 0.5f; + float s = sin(ha); + return quat(x * s, y * s, z * s, cos(ha)); +} + + +HPML_API quat_t quat_versor(float x, float y, float z, float angle) +{ + float s = sin(angle); + return quat(s * x, s * y, s * z, cos(angle)); +} + +HPML_API float quat_angle(quat_t q1, quat_t q2) +{ + /* + q1 * q = q2 + implies => q = inverse(q) * q2; + */ + return acos(quat_difference(q1, q2).s) * 2.0f; +} + +HPML_API quat_t quat_lerp(quat_t from, quat_t to, float t) +{ + return __quat_add(quat_mul_scalar(from, 1 - t), quat_mul_scalar(to, t)); +} + +HPML_API quat_t quat_slerp(quat_t from, quat_t to, float t) +{ + return __quat_mul(quat_pow(quat_difference(from, to), t), from); +} + +HPML_API quat_t quat_sandwitch(quat_t versor, quat_t p) +{ + return quat_mul(2, versor, p, quat_inverse(versor)); +} + +HPML_API bool quat_equal(quat_t q1, quat_t q2) +{ + return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w); +} + + +HPML_API void quat_print(quat_t q) +{ + printf("Quat: (%f, %f, %f, %f)\n", q.x, q.y, q.z, q.w); +} +