mirror of
https://github.com/fabiangreffrath/woof.git
synced 2025-08-03 20:57:55 -04:00
Add Clang Vector Extension and swich to xyzw quat (#2025)
* Add Clang Vector Extension and switch to xyzw quat * Formatting --------- Co-authored-by: Jon Daniel <jopadan@mailfence.com> Co-authored-by: ceski <56656010+ceski-1@users.noreply.github.com>
This commit is contained in:
Jon Daniel
GitHub
parent
b674af945d
commit
89aae9c789
2
.gitignore
vendored
2
.gitignore
vendored
@ -14,3 +14,5 @@ CMakeSettings.json
|
||||
|
||||
# clangd
|
||||
/.cache/
|
||||
CMakeFiles/
|
||||
src/CMakeFiles/
|
||||
|
||||
@ -74,6 +74,16 @@ check_c_source_compiles("
|
||||
"
|
||||
HAVE__DIV64
|
||||
)
|
||||
check_c_source_compiles("
|
||||
typedef float vec __attribute__((ext_vector_type(4)));
|
||||
int main()
|
||||
{
|
||||
vec a = (vec){0, 1, 2, 3};
|
||||
return 0;
|
||||
}
|
||||
"
|
||||
HAVE_EXT_VECTOR_TYPE
|
||||
)
|
||||
|
||||
option(CMAKE_FIND_PACKAGE_PREFER_CONFIG
|
||||
"Lookup package config files before using find modules" ON)
|
||||
|
||||
48
src/i_gyro.c
48
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)
|
||||
|
||||
@ -20,95 +20,104 @@
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "config.h"
|
||||
#include "doomtype.h"
|
||||
|
||||
#if defined(HAVE_EXT_VECTOR_TYPE)
|
||||
typedef float vec __attribute__((ext_vector_type(3)));
|
||||
#else
|
||||
typedef struct
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} vec;
|
||||
#endif
|
||||
|
||||
#if defined(HAVE_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
|
||||
|
||||
//
|
||||
// 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 +125,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 +141,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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user