Skip to content

Commit

Permalink
Add Clang Vector Extension and switch to xyzw quat
Browse files Browse the repository at this point in the history
  • Loading branch information
Jon Daniel committed Nov 17, 2024
1 parent 8a73633 commit 68ce564
Show file tree
Hide file tree
Showing 3 changed files with 83 additions and 69 deletions.
2 changes: 2 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,3 +14,5 @@ CMakeSettings.json

# clangd
/.cache/
CMakeFiles/
src/CMakeFiles/
48 changes: 24 additions & 24 deletions src/i_gyro.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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();

Expand Down Expand Up @@ -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;

Expand All @@ -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)
Expand All @@ -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);
Expand All @@ -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)
Expand Down
102 changes: 57 additions & 45 deletions src/m_vector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,178 +22,190 @@

#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));
}

//
// 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));
}

//
// 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

0 comments on commit 68ce564

Please sign in to comment.