From 68ce564994df954d5e32c768233dc6b725112ad2 Mon Sep 17 00:00:00 2001 From: Jon Daniel Date: Sun, 17 Nov 2024 12:01:48 +0100 Subject: [PATCH] Add Clang Vector Extension and switch to xyzw quat --- .gitignore | 2 + src/i_gyro.c | 48 +++++++++++------------ src/m_vector.h | 102 +++++++++++++++++++++++++++---------------------- 3 files changed, 83 insertions(+), 69 deletions(-) diff --git a/.gitignore b/.gitignore index 818793e79..16bb0fcad 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,5 @@ CMakeSettings.json # clangd /.cache/ +CMakeFiles/ +src/CMakeFiles/ diff --git a/src/i_gyro.c b/src/i_gyro.c index fe8d10290..35108c256 100644 --- a/src/i_gyro.c +++ b/src/i_gyro.c @@ -192,13 +192,13 @@ static void SaveCalibration(void) static void ProcessAccelCalibration(void) { cal.accel_count++; - cal.accel_sum += vec_length(&motion.accel); + cal.accel_sum += vec_length(motion.accel); } static void ProcessGyroCalibration(void) { cal.gyro_count++; - cal.gyro_sum = vec_add(&cal.gyro_sum, &motion.gyro); + cal.gyro_sum = vec_add(cal.gyro_sum, motion.gyro); } static void PostProcessCalibration(void) @@ -211,8 +211,8 @@ static void PostProcessCalibration(void) motion.accel_magnitude = cal.accel_sum / cal.accel_count; motion.accel_magnitude = BETWEEN(0.0f, 2.0f, motion.accel_magnitude); - motion.gyro_offset = vec_scale(&cal.gyro_sum, 1.0f / cal.gyro_count); - motion.gyro_offset = vec_clamp(-1.0f, 1.0f, &motion.gyro_offset); + motion.gyro_offset = vec_scale(cal.gyro_sum, 1.0f / cal.gyro_count); + motion.gyro_offset = vec_clamp(-1.0f, 1.0f, motion.gyro_offset); SaveCalibration(); @@ -505,7 +505,7 @@ static void ApplyGyroSpace_Local(void) static void ApplyGyroSpace_Player(void) { - const vec grav_norm = vec_normalize(&motion.gravity); + const vec grav_norm = vec_normalize(motion.gravity); const float world_yaw = motion.gyro.y * grav_norm.y + motion.gyro.z * grav_norm.z; @@ -532,36 +532,36 @@ static void CalcGravityVector_Skip(void) static void CalcGravityVector_Full(void) { // Convert gyro input to reverse rotation. - const float angle_speed = vec_length(&motion.gyro); + const float angle_speed = vec_length(motion.gyro); const float angle = angle_speed * motion.delta_time; - const vec negative_gyro = vec_negative(&motion.gyro); - const quat reverse_rotation = angle_axis(angle, &negative_gyro); + const vec negative_gyro = vec_negative(motion.gyro); + const quat reverse_rotation = angle_axis(angle, negative_gyro); // Rotate gravity vector. - motion.gravity = vec_rotate(&motion.gravity, &reverse_rotation); + motion.gravity = vec_rotate(motion.gravity, reverse_rotation); // Check accelerometer magnitude now. - const float accel_magnitude = vec_length(&motion.accel); + const float accel_magnitude = vec_length(motion.accel); if (accel_magnitude <= 0.0f) { return; } - const vec accel_norm = vec_scale(&motion.accel, 1.0f / accel_magnitude); + const vec accel_norm = vec_scale(motion.accel, 1.0f / accel_magnitude); // Shakiness/smoothness. - motion.smooth_accel = vec_rotate(&motion.smooth_accel, &reverse_rotation); + motion.smooth_accel = vec_rotate(motion.smooth_accel, reverse_rotation); const float smooth_factor = exp2f(-motion.delta_time * SMOOTH_HALF_TIME); motion.shakiness *= smooth_factor; - const vec delta_accel = vec_subtract(&motion.accel, &motion.smooth_accel); - const float delta_accel_magnitude = vec_length(&delta_accel); + const vec delta_accel = vec_subtract(motion.accel, motion.smooth_accel); + const float delta_accel_magnitude = vec_length(delta_accel); motion.shakiness = MAX(motion.shakiness, delta_accel_magnitude); motion.smooth_accel = - vec_lerp(&motion.accel, &motion.smooth_accel, smooth_factor); + vec_lerp(motion.accel, motion.smooth_accel, smooth_factor); // Find the difference between gravity and raw acceleration. - const vec new_gravity = vec_scale(&accel_norm, -motion.accel_magnitude); - const vec gravity_delta = vec_subtract(&new_gravity, &motion.gravity); - const vec gravity_direction = vec_normalize(&gravity_delta); + const vec new_gravity = vec_scale(accel_norm, -motion.accel_magnitude); + const vec gravity_delta = vec_subtract(new_gravity, motion.gravity); + const vec gravity_direction = vec_normalize(gravity_delta); // Calculate correction rate. float still_or_shaky = (motion.shakiness - SHAKINESS_MIN_THRESH) @@ -575,7 +575,7 @@ static void CalcGravityVector_Full(void) const float correction_limit = MAX(angle_speed_adjusted, COR_MIN_SPEED); if (correction_rate > correction_limit) { - const float gravity_delta_magnitude = vec_length(&gravity_delta); + const float gravity_delta_magnitude = vec_length(gravity_delta); float close_factor = (gravity_delta_magnitude - COR_GYRO_MIN_THRESH) / (COR_GYRO_MAX_THRESH - COR_GYRO_MIN_THRESH); close_factor = BETWEEN(0.0f, 1.0f, close_factor); @@ -585,20 +585,20 @@ static void CalcGravityVector_Full(void) // Apply correction to gravity vector. const vec correction = - vec_scale(&gravity_direction, correction_rate * motion.delta_time); - if (vec_lengthsquared(&correction) < vec_lengthsquared(&gravity_delta)) + vec_scale(gravity_direction, correction_rate * motion.delta_time); + if (vec_lengthsquared(correction) < vec_lengthsquared(gravity_delta)) { - motion.gravity = vec_add(&motion.gravity, &correction); + motion.gravity = vec_add(motion.gravity, correction); } else { - motion.gravity = vec_scale(&accel_norm, -motion.accel_magnitude); + motion.gravity = vec_scale(accel_norm, -motion.accel_magnitude); } } static void ApplyCalibration(void) { - motion.gyro = vec_subtract(&motion.gyro, &motion.gyro_offset); + motion.gyro = vec_subtract(motion.gyro, motion.gyro_offset); } static float GetDeltaTime(void) diff --git a/src/m_vector.h b/src/m_vector.h index b5f5573c4..d8e19b5c9 100644 --- a/src/m_vector.h +++ b/src/m_vector.h @@ -22,93 +22,105 @@ #include "doomtype.h" +#if defined(__clang__) +#if __has_attribute(ext_vector_type) +typedef float vec __attribute__((ext_vector_type(3))); +#else typedef struct { float x; float y; float z; } vec; +#endif +#endif +#if defined(__clang__) +#if __has_attribute(ext_vector_type) +typedef float quat __attribute__((ext_vector_type(4))); +#else typedef struct { - float w; float x; float y; float z; + float w; } quat; +#endif +#endif // // Adds two vectors. // -inline static vec vec_add(const vec *a, const vec *b) +inline static vec vec_add(const vec a, const vec b) { - return (vec){a->x + b->x, a->y + b->y, a->z + b->z}; + return (vec){a.x + b.x, a.y + b.y, a.z + b.z}; } // // Subtracts two vectors. // -inline static vec vec_subtract(const vec *a, const vec *b) +inline static vec vec_subtract(const vec a, const vec b) { - return (vec){a->x - b->x, a->y - b->y, a->z - b->z}; + return (vec){a.x - b.x, a.y - b.y, a.z - b.z}; } // // Cross product of two vectors. // -inline static vec vec_crossproduct(const vec *a, const vec *b) +inline static vec vec_crossproduct(const vec a, const vec b) { - return (vec){a->y * b->z - a->z * b->y, - a->z * b->x - a->x * b->z, - a->x * b->y - a->y * b->x}; + return (vec){a.y * b.z - a.z * b.y, + a.z * b.x - a.x * b.z, + a.x * b.y - a.y * b.x}; } // // Dot product of two vectors. // -inline static float vec_dotproduct(const vec *a, const vec *b) +inline static float vec_dotproduct(const vec a, const vec b) { - return (a->x * b->x + a->y * b->y + a->z * b->z); + return (a.x * b.x + a.y * b.y + a.z * b.z); } // // Returns the negative of the input vector. // -inline static vec vec_negative(const vec *v) +inline static vec vec_negative(const vec v) { - return (vec){-v->x, -v->y, -v->z}; + return (vec){-v.x, -v.y, -v.z}; } // // Multiplies a vector by a scalar value. // -inline static vec vec_scale(const vec *v, float scalar) +inline static vec vec_scale(const vec v, float scalar) { - return (vec){v->x * scalar, v->y * scalar, v->z * scalar}; + return (vec){v.x * scalar, v.y * scalar, v.z * scalar}; } // // Clamps each vector component. // -inline static vec vec_clamp(float min, float max, const vec *v) +inline static vec vec_clamp(float min, float max, const vec v) { - return (vec){BETWEEN(min, max, v->x), - BETWEEN(min, max, v->y), - BETWEEN(min, max, v->z)}; + return (vec){BETWEEN(min, max, v.x), + BETWEEN(min, max, v.y), + BETWEEN(min, max, v.z)}; } // // Vector length squared. // -inline static float vec_lengthsquared(const vec *v) +inline static float vec_lengthsquared(const vec v) { - return (v->x * v->x + v->y * v->y + v->z * v->z); + return (v.x * v.x + v.y * v.y + v.z * v.z); } // // Vector magnitude. // -inline static float vec_length(const vec *v) +inline static float vec_length(const vec v) { return sqrtf(vec_lengthsquared(v)); } @@ -116,15 +128,15 @@ inline static float vec_length(const vec *v) // // Normalizes a vector using a given magnitude. // -inline static vec vec_normalize_mag(const vec *v, float mag) +inline static vec vec_normalize_mag(const vec v, float mag) { - return (mag > 0.0f ? vec_scale(v, 1.0f / mag) : *v); + return (mag > 0.0f ? vec_scale(v, 1.0f / mag) : v); } // // Normalizes a vector. // -inline static vec vec_normalize(const vec *v) +inline static vec vec_normalize(const vec v) { return vec_normalize_mag(v, vec_length(v)); } @@ -132,68 +144,68 @@ inline static vec vec_normalize(const vec *v) // // Is this a zero vector? // -inline static boolean is_zero_vec(const vec *v) +inline static boolean is_zero_vec(const vec v) { - return (v->x == 0.0f && v->y == 0.0f && v->z == 0.0f); + return (v.x == 0.0f && v.y == 0.0f && v.z == 0.0f); } // // Returns the conjugate of a unit quaternion (same as its inverse). // -inline static quat quat_inverse(const quat *q) +inline static quat quat_inverse(const quat q) { - return (quat){q->w, -q->x, -q->y, -q->z}; + return (quat){-q.x, -q.y, -q.z, q.w}; } // // Converts a vector to a quaternion. // -inline static quat vec_to_quat(const vec *v) +inline static quat vec_to_quat(const vec v) { - return (quat){0.0f, v->x, v->y, v->z}; + return (quat){v.x, v.y, v.z, 0.0f}; } // // Multiplies two quaternions. // -inline static quat quat_multiply(const quat *a, const quat *b) +inline static quat quat_multiply(const quat a, const quat b) { - return (quat){a->w * b->w - a->x * b->x - a->y * b->y - a->z * b->z, - a->w * b->x + a->x * b->w + a->y * b->z - a->z * b->y, - a->w * b->y - a->x * b->z + a->y * b->w + a->z * b->x, - a->w * b->z + a->x * b->y - a->y * b->x + a->z * b->w}; + return (quat){a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, + a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, + a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, + a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z}; } // // Returns a unit quaternion from an angle and vector. // -inline static quat angle_axis(float angle, const vec *v) +inline static quat angle_axis(float angle, const vec v) { vec temp = vec_normalize(v); - temp = vec_scale(&temp, sinf(angle * 0.5f)); - return (quat){cosf(angle * 0.5f), temp.x, temp.y, temp.z}; + temp = vec_scale(temp, sinf(angle * 0.5f)); + return (quat){temp.x, temp.y, temp.z,cosf(angle * 0.5f)}; } // // Rotates a vector by a unit quaternion. // -inline static vec vec_rotate(const vec *v, const quat *q) +inline static vec vec_rotate(const vec v, const quat q) { const quat q_inv = quat_inverse(q); const quat v_quat = vec_to_quat(v); - quat temp = quat_multiply(q, &v_quat); - temp = quat_multiply(&temp, &q_inv); + quat temp = quat_multiply(q,v_quat); + temp = quat_multiply(temp, q_inv); return (vec){temp.x, temp.y, temp.z}; } // // Linear interpolation between two vectors. // -inline static vec vec_lerp(const vec *a, const vec *b, float factor) +inline static vec vec_lerp(const vec a, const vec b, float factor) { vec temp = vec_subtract(b, a); - temp = vec_scale(&temp, factor); - return vec_add(a, &temp); + temp = vec_scale(temp, factor); + return vec_add(a, temp); } #endif