Skip to content

Instantly share code, notes, and snippets.

@TrevorSundberg

TrevorSundberg/kongroth_flight_physics.c

Created July 23, 2025 05:07
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save TrevorSundberg/e4066c6d69c7ccb058a39f85da689bbf to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save TrevorSundberg/e4066c6d69c7ccb058a39f85da689bbf to your computer and use it in GitHub Desktop.
Download ZIP
Raw
kongroth_flight_physics.c
#include <assert.h>
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#define INLINE __attribute__((always_inline)) inline
#if defined(_WIN32)
#define EXPORT __declspec(dllexport)
#else
#define EXPORT __attribute__((visibility("default")))
#endif
#define X 0
#define Y 1
#define Z 2
#define W 3
typedef int32_t bool;
// #define USE_FLOAT_NUMBER
#if !defined(USE_FLOAT_NUMBER) && !defined(USE_FLOAT4_NUMBER)
#define USE_FLOAT4_NUMBER
#endif
#ifdef USE_FLOAT4_NUMBER
typedef float component;
typedef __attribute__((ext_vector_type(4))) float number;
typedef __attribute__((ext_vector_type(4))) bool boolean;
typedef __attribute__((ext_vector_type(4))) int32_t integer;
#define NUMBER_COMPONENTS 4
#define NUMBER_ACCESS(value, component_index) ((value)[component_index])
#define NUMBER_INDEX(state_index, index_subscripts) [state_index / NUMBER_COMPONENTS] index_subscripts[state_index % NUMBER_COMPONENTS]
#define VECTOR_REPEAT_STRING(string) string ", " string ", " string ", " string
#define VECTOR_REPEAT_COMPONENT(before, value, after) before value.x after, before value.y after, before value.z after, before value.w after
INLINE boolean boolean_standardize(const boolean nonstandard_boolean) {
// OpenCL boolean vectors use -1 instead of +1, so we standardize it to always be +1
return __builtin_elementwise_abs(nonstandard_boolean);
}
INLINE bool number_nearly_equal_epsilon(number a, number b, number epsilon) {
return __builtin_reduce_add(__builtin_elementwise_abs(a - b) < epsilon) == -4;
}
#endif
#ifdef USE_FLOAT_NUMBER
typedef float component;
typedef float number;
typedef bool boolean;
typedef int32_t integer;
#define NUMBER_COMPONENTS 1
#define NUMBER_ACCESS(value, component_index) (value)
#define NUMBER_INDEX(state_index, index_subscripts) [state_index] index_subscripts
#define VECTOR_REPEAT_STRING(string) string
#define VECTOR_REPEAT_COMPONENT(before, value, after) before value after
INLINE boolean boolean_standardize(const boolean value) {
return value;
}
INLINE bool number_nearly_equal_epsilon(number a, number b, number epsilon) {
return __builtin_elementwise_abs(a - b) < epsilon;
}
#endif
#define VECTOR_EXPAND(type, value) ((type){VECTOR_REPEAT_COMPONENT(, value, )})
#define CONSTANT(value) ((number)value)
#define PI CONSTANT(3.14159265358979323846)
#define E CONSTANT(2.71828182845904523536)
#define RADIAN_TO_DEGREE (CONSTANT(180) / PI)
#define INDEXED_GETTER_SETTER_BASE(states_type, member, index_subscripts, ...) \
EXPORT component states_type##_get_##member(const states_type *const states, const uintptr_t state_index, ##__VA_ARGS__) { \
return states->member NUMBER_INDEX(state_index, index_subscripts); \
} \
EXPORT void states_type##_set_##member(states_type *const states, const uintptr_t state_index, ##__VA_ARGS__, const component value) { \
states->member NUMBER_INDEX(state_index, index_subscripts) = value; \
}
#define INDEXED_GETTER_SETTER(states_type, member) \
INDEXED_GETTER_SETTER_BASE(states_type, member, )
#define INDEXED_GETTER_SETTER_VECTOR(states_type, member) \
INDEXED_GETTER_SETTER_BASE(states_type, member, .array[component_index], const uintptr_t component_index)
#define INDEXED_GETTER_SETTER_ARRAY(states_type, member) \
INDEXED_GETTER_SETTER_BASE(states_type, member, [array_index], const uintptr_t array_index)
#define INDEXED_GETTER_SETTER_ARRAY_VECTOR(states_type, member) \
INDEXED_GETTER_SETTER_BASE(states_type, member, [array_index].array[component_index], const uintptr_t array_index, const uintptr_t component_index)
#define STATES_FUNCTIONS(name) \
EXPORT void name##_init(name *const states); \
EXPORT uintptr_t name##_size(void) { \
return sizeof(name); \
} \
EXPORT name *name##_allocate(void) { \
name *states = malloc(sizeof(name)); \
name##_init(states); \
return states; \
} \
EXPORT void name##_free(name *const states) { \
free(states); \
}
#define BODY_COUNT 100
#define BODY_GROUP_COUNT (BODY_COUNT / NUMBER_COMPONENTS)
static_assert(BODY_COUNT % NUMBER_COMPONENTS == 0, "BODY_COUNT must be divisible by NUMBER_COMPONENTS");
#define MOTOR_COUNT 4
// Motor configuration for back/front or right/left
#define MOTOR_BR 0
#define MOTOR_FR 1
#define MOTOR_BL 2
#define MOTOR_FL 3
#define PRIMARY_AXIS_COUNT 3
#define ALL_AXIS_COUNT 4
#define AXIS_PITCH 0 // X axis
#define AXIS_YAW 1 // Y axis
#define AXIS_ROLL 2 // Z axis
#define AXIS_THROTTLE 3 // Not included in rates but used in mixing
#define BOX_FACE_COUNT 6
#define BOX_FACE_PX 0
#define BOX_FACE_NX 1
#define BOX_FACE_PY 2
#define BOX_FACE_NY 3
#define BOX_FACE_PZ 4
#define BOX_FACE_NZ 5
#define EPSILON CONSTANT(1e-4)
#define UNIVERSAL_GAS_CONSTANT CONSTANT(8.3144598) /* J/(mol*K) */
#define MOLAR_MASS_AIR CONSTANT(0.0289644) /* kg/mol */
typedef union {
struct
{
number x;
number y;
number z;
};
number array[3];
} vector3;
typedef union {
struct
{
number x;
number y;
number z;
number w;
};
number array[4];
} quaternion;
typedef union {
struct
{
number m00;
number m01;
number m02;
number m11;
number m12;
number m22;
};
number array[6];
} upper_diagonal_matrix3;
typedef struct {
number output;
number alpha;
} low_pass_filter;
INLINE number number_from_bool(const boolean nonstandard_boolean) {
const boolean standardized_boolean = boolean_standardize(nonstandard_boolean);
return VECTOR_EXPAND(number, standardized_boolean);
}
INLINE number choose_number(const number if_true, const number if_false, const boolean nonstandard_boolean) {
number true_multiplier = number_from_bool(nonstandard_boolean);
number false_multiplier = CONSTANT(1) - true_multiplier;
return if_true * true_multiplier + if_false * false_multiplier;
}
// https://dsp.stackexchange.com/a/54088
INLINE number low_pass_filter_compute_alpha(const number delta_time, const number frequency_hz) {
// Compute wc (omega-c)
number angular_cuttoff_frequency = CONSTANT(2) * PI * frequency_hz;
number y = CONSTANT(1) - __builtin_elementwise_cos(angular_cuttoff_frequency * delta_time);
return -y + __builtin_elementwise_sqrt(y * y + CONSTANT(2) * y);
}
INLINE void low_pass_filter_init(low_pass_filter *filter, const number delta_time, const number frequency_hz) {
filter->output = 0;
filter->alpha = low_pass_filter_compute_alpha(delta_time, frequency_hz);
}
INLINE number quaternion_length(const quaternion value) {
return __builtin_elementwise_sqrt(
value.x * value.x +
value.y * value.y +
value.z * value.z +
value.w * value.w);
}
INLINE quaternion quaternion_normalize(const quaternion value) {
const number magnitude = quaternion_length(value);
return (quaternion){
value.x / magnitude,
value.y / magnitude,
value.z / magnitude,
value.w / magnitude};
}
INLINE quaternion quaternion_normalized_inverse(const quaternion q) {
// For normalized quaternion, inverse is just conjugate
return (quaternion){-q.x, -q.y, -q.z, q.w};
}
INLINE quaternion quaternion_add(const quaternion a, const quaternion b) {
return (quaternion){
a.x + b.x,
a.y + b.y,
a.z + b.z,
a.w + b.w};
}
INLINE quaternion quaternion_multiply(const quaternion a, const quaternion b) {
return (quaternion){
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};
}
INLINE number vector3_dot(const vector3 a, const vector3 b) {
return a.x * b.x + a.y * b.y + a.z * b.z;
}
INLINE number vector3_length_squared(const vector3 value) {
return vector3_dot(value, value);
}
INLINE number vector3_length(const vector3 value) {
return __builtin_elementwise_sqrt(vector3_length_squared(value));
}
INLINE vector3 vector3_scalar_multiply(const number scalar, const vector3 v) {
return (vector3){
scalar * v.x,
scalar * v.y,
scalar * v.z};
}
// If a divisior is large enough, it stays the same, otherwise it becomes 1 to avoid a division by zero
INLINE number number_fix_positive_divisor(const number positive_divisor, const number minimum_positive_divisor) {
number divisor_big_enough = number_from_bool(positive_divisor > minimum_positive_divisor);
number divisor_too_small = CONSTANT(1) - divisor_big_enough;
return positive_divisor * divisor_big_enough + divisor_too_small;
}
INLINE vector3 vector3_normalize(const vector3 value, const number minimum_length) {
const number magnitude = number_fix_positive_divisor(vector3_length(value), minimum_length);
return vector3_scalar_multiply(CONSTANT(1) / magnitude, value);
}
INLINE vector3 vector3_cross(const vector3 a, const vector3 b) {
return (vector3){
a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x};
}
INLINE vector3 vector3_add(const vector3 a, const vector3 b) {
return (vector3){
a.x + b.x,
a.y + b.y,
a.z + b.z};
}
INLINE vector3 vector3_subtract(const vector3 a, const vector3 b) {
return (vector3){
a.x - b.x,
a.y - b.y,
a.z - b.z};
}
INLINE vector3 vector3_abs(const vector3 value) {
return (vector3){
__builtin_elementwise_abs(value.x),
__builtin_elementwise_abs(value.y),
__builtin_elementwise_abs(value.z)};
}
// https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
INLINE vector3 rotate_vector_by_quaternion(const vector3 v, const quaternion q) {
vector3 u = {q.x, q.y, q.z};
number s = q.w;
vector3 projection = vector3_scalar_multiply(2.0f * vector3_dot(u, v), u);
vector3 scaled = vector3_scalar_multiply(s * s - vector3_dot(u, u), v);
vector3 perpendicular = vector3_scalar_multiply(2.0f * s, vector3_cross(u, v));
return vector3_add(vector3_add(projection, scaled), perpendicular);
}
INLINE vector3 point_velocity_from_offset_world(
const vector3 linear_velocity_world,
const vector3 angular_velocity_world,
const vector3 offset_world) {
return vector3_add(linear_velocity_world, vector3_cross(angular_velocity_world, offset_world));
}
INLINE vector3 apply_force_get_torque_world(
const vector3 force_world,
const vector3 offset_world) {
return vector3_cross(offset_world, force_world);
}
INLINE uintptr_t ceil_division(uintptr_t x, uintptr_t y) {
return (x + y - 1) / y;
}
INLINE number clamp(number value, number min, number max) {
return __builtin_elementwise_min(max, __builtin_elementwise_max(min, value));
}
INLINE number smooth_noise(number input, uintptr_t octaves, number radian_frequency_per_octave[], number amplitude_per_octave[]) {
number result = 0;
number total_amplitude = 0;
for (uintptr_t i = 0; i < octaves; ++i) {
result += __builtin_elementwise_sin(input * radian_frequency_per_octave[i]) * amplitude_per_octave[i];
total_amplitude += amplitude_per_octave[i];
}
return result / total_amplitude;
}
INLINE number smooth_noise_simple(number input) {
number radian_frequency_per_octave[] = {
CONSTANT(1.00000),
CONSTANT(1.05245),
CONSTANT(2.64983),
CONSTANT(4.33752),
};
number amplitude_per_octave[] = {
CONSTANT(1.00000),
CONSTANT(1.00000),
CONSTANT(0.59456),
CONSTANT(0.34035),
};
return smooth_noise(input, 4, radian_frequency_per_octave, amplitude_per_octave);
}
INLINE vector3 smooth_noise_simple_vector3(vector3 input) {
return (vector3){
smooth_noise_simple(input.x),
smooth_noise_simple(input.y),
smooth_noise_simple(input.z),
};
}
typedef struct {
uintptr_t count;
bool enable_ground_plane;
number inverse_mass[BODY_GROUP_COUNT];
vector3 forces_world[BODY_GROUP_COUNT];
vector3 linear_acceleration_world[BODY_GROUP_COUNT];
vector3 linear_velocity_world[BODY_GROUP_COUNT];
vector3 position_world[BODY_GROUP_COUNT];
upper_diagonal_matrix3 inverse_inertia_tensor_local[BODY_GROUP_COUNT];
vector3 torques_world[BODY_GROUP_COUNT];
vector3 angular_velocity_world[BODY_GROUP_COUNT];
vector3 angular_velocity_local[BODY_GROUP_COUNT];
quaternion orientation_world[BODY_GROUP_COUNT];
number air_density[BODY_GROUP_COUNT];
vector3 drag_box_size[BODY_GROUP_COUNT];
number drag_coefficient[BODY_GROUP_COUNT][BOX_FACE_COUNT];
number drag_pillow_effect[BODY_GROUP_COUNT][BOX_FACE_COUNT];
vector3 wind_velocity_world[BODY_GROUP_COUNT];
vector3 wind_air_pocket_sample_position_world[BODY_GROUP_COUNT];
number wind_air_pocket_width[BODY_GROUP_COUNT];
number wind_air_pocket_amplitude[BODY_GROUP_COUNT];
number wind_air_pocket_velocity_scale[BODY_GROUP_COUNT];
vector3 gravity_world[BODY_GROUP_COUNT];
vector3 debug_output[BODY_GROUP_COUNT];
} physics_states;
STATES_FUNCTIONS(physics_states)
INDEXED_GETTER_SETTER(physics_states, inverse_mass)
INDEXED_GETTER_SETTER_VECTOR(physics_states, forces_world)
INDEXED_GETTER_SETTER_VECTOR(physics_states, linear_acceleration_world)
INDEXED_GETTER_SETTER_VECTOR(physics_states, linear_velocity_world)
INDEXED_GETTER_SETTER_VECTOR(physics_states, position_world)
INDEXED_GETTER_SETTER_VECTOR(physics_states, inverse_inertia_tensor_local)
INDEXED_GETTER_SETTER_VECTOR(physics_states, torques_world)
INDEXED_GETTER_SETTER_VECTOR(physics_states, angular_velocity_world)
INDEXED_GETTER_SETTER_VECTOR(physics_states, angular_velocity_local)
INDEXED_GETTER_SETTER_VECTOR(physics_states, orientation_world)
INDEXED_GETTER_SETTER(physics_states, air_density)
INDEXED_GETTER_SETTER_VECTOR(physics_states, drag_box_size)
INDEXED_GETTER_SETTER_ARRAY(physics_states, drag_coefficient)
INDEXED_GETTER_SETTER_ARRAY(physics_states, drag_pillow_effect)
INDEXED_GETTER_SETTER_VECTOR(physics_states, wind_velocity_world)
INDEXED_GETTER_SETTER_VECTOR(physics_states, gravity_world)
INDEXED_GETTER_SETTER_VECTOR(physics_states, debug_output)
void physics_states_init_index(physics_states *const physics_states, const uintptr_t group_index) {
physics_states->inverse_mass[group_index] = CONSTANT(1.0 / 0.5); // 500g
physics_states->forces_world[group_index] = (vector3){0, 0, 0};
physics_states->linear_velocity_world[group_index] = (vector3){0, 0, 0};
physics_states->linear_acceleration_world[group_index] = (vector3){0, 0, 0};
physics_states->position_world[group_index] = (vector3){0, 0, 0};
// physics_states->inverse_inertia_tensor_local[group_index] = (upper_diagonal_matrix3){1, 0, 0, 1, 0, 1}; // identity
physics_states->inverse_inertia_tensor_local[group_index] = (upper_diagonal_matrix3){
CONSTANT(23.76237), 0, 0, 12, 0, CONSTANT(23.76237)};
physics_states->torques_world[group_index] = (vector3){0, 0, 0};
physics_states->angular_velocity_world[group_index] = (vector3){0, 0, 0};
physics_states->angular_velocity_local[group_index] = (vector3){0, 0, 0};
physics_states->orientation_world[group_index] = (quaternion){0, 0, 0, 1};
physics_states->gravity_world[group_index] = (vector3){0, CONSTANT(-9.80665), 0};
physics_states->air_density[group_index] = 1.225f; // kg/m³ (at sea level)
physics_states->drag_box_size[group_index] = (vector3){CONSTANT(0.4), CONSTANT(0.1), CONSTANT(0.35)};
for (uintptr_t face = 0; face < BOX_FACE_COUNT; ++face) {
physics_states->drag_coefficient[group_index][face] = CONSTANT(0.5);
physics_states->drag_pillow_effect[group_index][face] = CONSTANT(0.5);
}
physics_states->drag_coefficient[group_index][BOX_FACE_NY] = CONSTANT(1);
physics_states->drag_coefficient[group_index][BOX_FACE_PY] = CONSTANT(1);
physics_states->wind_velocity_world[group_index] = (vector3){0, 0, 0};
physics_states->wind_air_pocket_sample_position_world[group_index] = (vector3){0, 0, 0};
physics_states->wind_air_pocket_width[group_index] = 0.5f;
physics_states->wind_air_pocket_amplitude[group_index] = 0.0f;
physics_states->wind_air_pocket_velocity_scale[group_index] = 1.0f;
physics_states->debug_output[group_index] = (vector3){0, 0, 0};
}
EXPORT void physics_states_init(physics_states *const physics_states) {
physics_states->count = 0;
physics_states->enable_ground_plane = 1;
for (uintptr_t i = 0; i < BODY_GROUP_COUNT; ++i) {
physics_states_init_index(physics_states, i);
}
}
EXPORT void physics_states_set_count(physics_states *const physics_states, const uintptr_t count) {
physics_states->count = count;
}
EXPORT uintptr_t physics_states_get_count(physics_states *const physics_states) {
return physics_states->count;
}
EXPORT void physics_states_set_enable_ground_plane(physics_states *const physics_states, const bool enable) {
physics_states->enable_ground_plane = enable;
}
EXPORT bool physics_states_get_enable_ground_plane(physics_states *const physics_states) {
return physics_states->enable_ground_plane;
}
EXPORT void physics_states_update(physics_states *const physics_states, const component delta_time) {
const number dt = delta_time;
const number dt_half = dt * CONSTANT(0.5);
const number inverse_dt = CONSTANT(1) / dt;
const uintptr_t group_count = ceil_division(physics_states->count, NUMBER_COMPONENTS);
for (uintptr_t i = 0; i < group_count; ++i) {
number inverse_mass = physics_states->inverse_mass[i];
vector3 *forces_world = &physics_states->forces_world[i];
vector3 *linear_velocity_world = &physics_states->linear_velocity_world[i];
vector3 *linear_acceleration_world = &physics_states->linear_acceleration_world[i];
vector3 *position_world = &physics_states->position_world[i];
vector3 *torques_world = &physics_states->torques_world[i];
vector3 *angular_velocity_world = &physics_states->angular_velocity_world[i];
vector3 *angular_velocity_local = &physics_states->angular_velocity_local[i];
quaternion *orientation_world = &physics_states->orientation_world[i];
vector3 *gravity_world = &physics_states->gravity_world[i];
vector3 drag_box_size = physics_states->drag_box_size[i];
vector3 drag_box_half_size = vector3_scalar_multiply(CONSTANT(0.5), drag_box_size);
vector3 wind_velocity_world = physics_states->wind_velocity_world[i];
number air_density = physics_states->air_density[i];
vector3 *wind_air_pocket_sample_position_world = &physics_states->wind_air_pocket_sample_position_world[i];
// Integrate wind sample position so that it moves around with the wind
*wind_air_pocket_sample_position_world = vector3_add(*wind_air_pocket_sample_position_world,
vector3_scalar_multiply(dt, wind_velocity_world));
quaternion orientation_world_to_local = quaternion_normalized_inverse(*orientation_world);
vector3 linear_velocity_local = rotate_vector_by_quaternion(
*linear_velocity_world,
orientation_world_to_local);
// https://projectborealis.com/docs/WADS_rev2.pdf
// https://projectborealis.com/wind-and-air-drag-system/
// Approximate drag per box face [+X, -X, +Y, -Y, +Z, -Z]
for (uintptr_t face = 0; face < BOX_FACE_COUNT; ++face) {
uintptr_t axis = face / 2;
uintptr_t positive = face % 2 == 0;
// Determine face normal and center offset in local space
vector3 normal_local = (vector3){0, 0, 0};
normal_local.array[axis] = positive ? +1 : -1;
// World-space normal & center of face
vector3 normal_world = rotate_vector_by_quaternion(normal_local, *orientation_world);
vector3 face_center_offset_world = vector3_scalar_multiply(drag_box_half_size.array[axis], normal_world);
vector3 face_center_world = vector3_add(*position_world, face_center_offset_world);
// Get relative airflow at this point, wind minus body point-velocity
vector3 face_center_velocity_world = point_velocity_from_offset_world(*linear_velocity_world, *angular_velocity_world, face_center_offset_world);
number turbulence_amplitude = physics_states->wind_air_pocket_amplitude[i] + physics_states->wind_air_pocket_velocity_scale[i] * vector3_length(wind_velocity_world);
vector3 turbulence_sample_position_world = vector3_scalar_multiply(
CONSTANT(1) / physics_states->wind_air_pocket_width[i],
vector3_add(*wind_air_pocket_sample_position_world, face_center_world));
vector3 turbulence_velocity_world = vector3_scalar_multiply(
turbulence_amplitude, smooth_noise_simple_vector3(turbulence_sample_position_world));
vector3 relative_air_velocity_world = vector3_add(turbulence_velocity_world, vector3_subtract(wind_velocity_world, face_center_velocity_world));
vector3 realtive_air_direction_world = vector3_normalize(relative_air_velocity_world, EPSILON);
// Keep only faces facing the wind (this will 0 out all calculations for opposing faces)
number air_direction_projected_on_face = __builtin_elementwise_max(
-vector3_dot(normal_world, realtive_air_direction_world), CONSTANT(0));
// Face area (world-space) depends on which axis is normal
// Note that because we don't implement scale or hierarchies, local box size = world box size
number face_area_world;
switch (axis) {
case 0: // X face
face_area_world = drag_box_size.y * drag_box_size.z;
break;
case 1: // Y face
face_area_world = drag_box_size.x * drag_box_size.z;
break;
case 2: // Z face
face_area_world = drag_box_size.x * drag_box_size.y;
break;
default:
abort();
}
// Projected area = area * cos(angle between face normal and air direction)
number face_area_projected_world = face_area_world * air_direction_projected_on_face;
// Get this face's pillow alpha
number drag_pillow_effect = physics_states->drag_pillow_effect[i][face];
// The pressure falls off radially, we compute the average falloff from integration
number drag_pillow_effect_force_multiplier = (PI - CONSTANT(4)) * drag_pillow_effect / CONSTANT(4) + CONSTANT(1);
number drag_coefficient = physics_states->drag_coefficient[i][face];
// Translational drag magnitude (1/2 * rho * v^2 * Aproj) * coefficient * pillow
number linear_drag_magnitude =
0.5f *
air_density *
vector3_length_squared(relative_air_velocity_world) *
face_area_projected_world *
drag_coefficient *
drag_pillow_effect_force_multiplier;
// Compute center-of-pressure shift along face (in face-plane)
// Find the tangent of flow on the face
vector3 relative_air_tangent_world =
vector3_normalize(vector3_subtract(realtive_air_direction_world, vector3_scalar_multiply(air_direction_projected_on_face, normal_world)), EPSILON);
vector3 relative_air_tangent_local = rotate_vector_by_quaternion(relative_air_tangent_world, orientation_world_to_local);
// This is an approximation of the half size along the tangent
number half_size_tangent = vector3_dot(vector3_abs(relative_air_tangent_local), drag_box_half_size);
// Shift fraction is affected by pillow effect (0=center, 1=trailing edge)
// We don't have scale or hierarchies, local distance = world distance
number shift_distance = half_size_tangent * drag_pillow_effect;
vector3 shift_world = vector3_scalar_multiply(shift_distance, relative_air_tangent_world);
vector3 center_of_pressure_offset_world = vector3_add(face_center_offset_world, shift_world);
// Apply translational drag
vector3 drag_force_world = vector3_scalar_multiply(-linear_drag_magnitude, normal_world);
vector3 drag_torque_from_force_world = apply_force_get_torque_world(drag_force_world, center_of_pressure_offset_world);
*forces_world = vector3_add(*forces_world, drag_force_world);
*torques_world = vector3_add(*torques_world, drag_torque_from_force_world);
// Rotational drag reduces with the pillow effect
number drag_pillow_effect_rotation_drag = (CONSTANT(15) - CONSTANT(7) * drag_pillow_effect) / CONSTANT(60);
// Rotational speed at that lever
number rotational_speed_squared = vector3_length_squared(*angular_velocity_world) * vector3_length_squared(center_of_pressure_offset_world);
number rotational_drag_magnitude =
0.5f *
air_density *
rotational_speed_squared *
face_area_projected_world *
drag_coefficient *
drag_pillow_effect_rotation_drag;
vector3 drag_torque_world = vector3_scalar_multiply(rotational_drag_magnitude, vector3_cross(normal_world, center_of_pressure_offset_world));
*torques_world = vector3_add(*torques_world, drag_torque_world);
}
vector3 prev_linear_velocity_world = *linear_velocity_world;
*linear_velocity_world = vector3_add(*linear_velocity_world,
vector3_scalar_multiply(dt * inverse_mass, *forces_world));
// Gravity is applied as an acceleration, not as a force
*linear_velocity_world = vector3_add(*linear_velocity_world,
vector3_scalar_multiply(dt, *gravity_world));
*position_world = vector3_add(*position_world,
vector3_scalar_multiply(dt, *linear_velocity_world));
vector3 torque_local = rotate_vector_by_quaternion(
*torques_world,
orientation_world_to_local);
// Clear the existing forces and torques so that next iteration we won't use the same ones
*forces_world = (vector3){0, 0, 0};
*torques_world = (vector3){0, 0, 0};
// Apply inverse inertia tensor (local space)
vector3 angular_acceleration_local = {
torque_local.x * physics_states->inverse_inertia_tensor_local[i].m00 +
torque_local.y * physics_states->inverse_inertia_tensor_local[i].m01 +
torque_local.z * physics_states->inverse_inertia_tensor_local[i].m02,
torque_local.x * physics_states->inverse_inertia_tensor_local[i].m01 +
torque_local.y * physics_states->inverse_inertia_tensor_local[i].m11 +
torque_local.z * physics_states->inverse_inertia_tensor_local[i].m12,
torque_local.x * physics_states->inverse_inertia_tensor_local[i].m02 +
torque_local.y * physics_states->inverse_inertia_tensor_local[i].m12 +
torque_local.z * physics_states->inverse_inertia_tensor_local[i].m22};
// Convert angular acceleration back to world-space
vector3 angular_acceleration_world = rotate_vector_by_quaternion(
angular_acceleration_local,
*orientation_world);
// Integrate angular velocity in world-space
*angular_velocity_world = vector3_add(
*angular_velocity_world,
vector3_scalar_multiply(dt, angular_acceleration_world));
vector3 omega_angular_velocity_local = rotate_vector_by_quaternion(
*angular_velocity_world,
orientation_world_to_local);
// https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/
quaternion omega_local = (quaternion){
omega_angular_velocity_local.x * dt_half,
omega_angular_velocity_local.y * dt_half,
omega_angular_velocity_local.z * dt_half,
0.0f};
*orientation_world = quaternion_add(*orientation_world,
quaternion_multiply(*orientation_world, omega_local));
*orientation_world = quaternion_normalize(*orientation_world);
orientation_world_to_local = quaternion_normalized_inverse(*orientation_world);
*angular_velocity_local = rotate_vector_by_quaternion(
*angular_velocity_world,
orientation_world_to_local);
// TODO(trevor): Make this a property or something, apply damping
//*linear_velocity_world = vector3_scalar_multiply(0.9999f, *linear_velocity_world);
//*angular_velocity_world = vector3_scalar_multiply(0.9999f, *angular_velocity_world);
// This is a VERY simple and approximate collision with an infinite ground plane
// This is not intended to be a correct approach to collision whatsoever, it's only made
// so that if you run this physics sim by itself, the drone will have a plane to rest on
// The intention of this library is to be coupled with a physics engine that performs collision
if (physics_states->enable_ground_plane) {
// We abs the comparison because OpenCL vectors return -1
number positive_y = number_from_bool(position_world->y >= (number)0);
position_world->y *= positive_y;
linear_velocity_world->y *= positive_y;
number smooth_interpolate = __builtin_elementwise_max(positive_y, (number)0.999);
// Slow the drone down (like drag)
linear_velocity_world->x *= smooth_interpolate;
linear_velocity_world->z *= smooth_interpolate;
angular_velocity_world->x *= smooth_interpolate;
angular_velocity_world->y *= smooth_interpolate;
angular_velocity_world->z *= smooth_interpolate;
// Reset the orientation of the drone to the identity if we hit the ground plane
orientation_world->x *= smooth_interpolate;
orientation_world->z *= smooth_interpolate;
*orientation_world = quaternion_normalize(*orientation_world);
}
*linear_acceleration_world = vector3_subtract(
vector3_scalar_multiply(inverse_dt, vector3_subtract(*linear_velocity_world, prev_linear_velocity_world)),
*gravity_world);
}
}
typedef struct {
number motor_outputs[BODY_GROUP_COUNT][MOTOR_COUNT];
vector3 motor_position_local[BODY_GROUP_COUNT][MOTOR_COUNT];
number propeller_force[BODY_GROUP_COUNT][MOTOR_COUNT];
number propeller_torque_rotation[BODY_GROUP_COUNT][MOTOR_COUNT];
number propeller_torque_arm[BODY_GROUP_COUNT][MOTOR_COUNT];
vector3 ground_plane_normal_world[BODY_GROUP_COUNT][MOTOR_COUNT];
number ground_plane_distance_world[BODY_GROUP_COUNT][MOTOR_COUNT];
number max_motor_with_prop_rpm[BODY_GROUP_COUNT];
number prop_pitch_meters[BODY_GROUP_COUNT];
number prop_diameter_meters[BODY_GROUP_COUNT];
number thrust_to_torque_ratio[BODY_GROUP_COUNT];
number barometer_reference_height[BODY_GROUP_COUNT];
number barometer_reference_pressure_pa[BODY_GROUP_COUNT];
number barometer_reference_temperature_kelvin[BODY_GROUP_COUNT];
number barometer_pressure_pa[BODY_GROUP_COUNT];
} drone_states;
STATES_FUNCTIONS(drone_states)
INDEXED_GETTER_SETTER_ARRAY(drone_states, motor_outputs)
INDEXED_GETTER_SETTER_ARRAY_VECTOR(drone_states, motor_position_local)
INDEXED_GETTER_SETTER_ARRAY(drone_states, propeller_force)
INDEXED_GETTER_SETTER_ARRAY(drone_states, propeller_torque_rotation)
INDEXED_GETTER_SETTER_ARRAY(drone_states, propeller_torque_arm)
INDEXED_GETTER_SETTER_ARRAY_VECTOR(drone_states, ground_plane_normal_world)
INDEXED_GETTER_SETTER_ARRAY(drone_states, ground_plane_distance_world)
INDEXED_GETTER_SETTER(drone_states, max_motor_with_prop_rpm)
INDEXED_GETTER_SETTER(drone_states, prop_pitch_meters)
INDEXED_GETTER_SETTER(drone_states, prop_diameter_meters)
INDEXED_GETTER_SETTER(drone_states, thrust_to_torque_ratio)
INDEXED_GETTER_SETTER(drone_states, barometer_reference_height)
INDEXED_GETTER_SETTER(drone_states, barometer_reference_pressure_pa)
INDEXED_GETTER_SETTER(drone_states, barometer_reference_temperature_kelvin)
INDEXED_GETTER_SETTER(drone_states, barometer_pressure_pa)
void drone_states_init_index(drone_states *const drone_states, const uintptr_t group_index) {
for (uintptr_t m = 0; m < MOTOR_COUNT; ++m) {
drone_states->motor_outputs[group_index][m] = 0;
drone_states->propeller_force[group_index][m] = 0;
drone_states->propeller_torque_rotation[group_index][m] = 0;
drone_states->propeller_torque_arm[group_index][m] = 0;
drone_states->ground_plane_normal_world[group_index][m] = (vector3){0, 1, 0};
drone_states->ground_plane_distance_world[group_index][m] = 0;
}
drone_states->motor_position_local[group_index][0] = (vector3){+0.33, 0, -0.29};
drone_states->motor_position_local[group_index][1] = (vector3){+0.33, 0, +0.29};
drone_states->motor_position_local[group_index][2] = (vector3){-0.33, 0, -0.29};
drone_states->motor_position_local[group_index][3] = (vector3){-0.33, 0, +0.29};
drone_states->max_motor_with_prop_rpm[group_index] = 10000;
drone_states->prop_pitch_meters[group_index] = 0.1524f; // 6in
drone_states->prop_diameter_meters[group_index] = 0.254f; // 10in
drone_states->thrust_to_torque_ratio[group_index] = 0.4f;
drone_states->barometer_reference_height[group_index] = 0; // Meters above sea level
drone_states->barometer_reference_pressure_pa[group_index] = 101325; // Standard Atmosphere (average at sea level)
drone_states->barometer_reference_temperature_kelvin[group_index] = CONSTANT(288.15); // Standard Temperature in Kelvin at sea level
drone_states->barometer_pressure_pa[group_index] = 101325; // Standard Atmosphere (average at sea level)
}
EXPORT void drone_states_init(drone_states *const drone_states) {
for (uintptr_t i = 0; i < BODY_GROUP_COUNT; ++i) {
drone_states_init_index(drone_states, i);
}
}
component get_motor_direction(bool motors_reversed, uintptr_t motor_index) {
component direction = 1;
if (motor_index == MOTOR_BR || motor_index == MOTOR_FL) {
direction = -1;
}
return motors_reversed ? -direction : direction;
}
EXPORT void drone_states_update(drone_states *const drone_states, physics_states *const physics_states) {
// TODO(trevor): Save values that we can precompute once, such as prop radius, radius^2, etc.
// Anything using values that don't often change too (such as the pitch and diameter).
const uintptr_t group_count = ceil_division(physics_states->count, NUMBER_COMPONENTS);
for (uintptr_t i = 0; i < group_count; ++i) {
vector3 *position_world = &physics_states->position_world[i];
quaternion *orientation_world = &physics_states->orientation_world[i];
vector3 *gravity_world = &physics_states->gravity_world[i];
vector3 *forces_world = &physics_states->forces_world[i];
vector3 *torques_world = &physics_states->torques_world[i];
vector3 up_world = rotate_vector_by_quaternion((vector3){0, 1, 0}, *orientation_world);
// Clear the existing forces and we will accumulate new ones
*forces_world = (vector3){0, 0, 0};
*torques_world = (vector3){0, 0, 0};
for (uintptr_t m = 0; m < MOTOR_COUNT; ++m) {
vector3 *motor_position_local = &drone_states->motor_position_local[i][m];
const vector3 motor_offset_world = rotate_vector_by_quaternion(*motor_position_local, *orientation_world);
vector3 propeller_velocity_world = point_velocity_from_offset_world(
physics_states->linear_velocity_world[i],
physics_states->angular_velocity_world[i],
motor_offset_world);
number air_speed_at_prop = vector3_dot(up_world, propeller_velocity_world);
// https://www.electricrcaircraftguy.com/2014/04/propeller-static-dynamic-thrust-equation-background.html
// https://www.tytorobotics.com/blogs/articles/how-to-calculate-propeller-thrust
// https://web.archive.org/web/20190402064809/https://quadstardrones.com/the-equations-for-speed/
number motor_output = drone_states->motor_outputs[i][m];
number rotations_per_second = drone_states->max_motor_with_prop_rpm[i] * motor_output / 60.0f;
number rps_pitch_term = rotations_per_second * drone_states->prop_pitch_meters[i];
number propeller_force = physics_states->air_density[i] * (PI * __builtin_elementwise_pow(drone_states->prop_diameter_meters[i], (number)2) / 4.0f) *
(__builtin_elementwise_pow(rps_pitch_term, (number)2) - rps_pitch_term * air_speed_at_prop) *
__builtin_elementwise_pow(drone_states->prop_diameter_meters[i] / (3.29546f * drone_states->prop_pitch_meters[i]), (number)1.5f);
propeller_force = __builtin_elementwise_max(propeller_force, (number)0);
// Compute ground effect
// Leishman, J. G., Principles of Helicopter Aerodynamics, 2nd. Ed., p.258 - 260, Cambridge University Press, New York, 2006.
// https://cdcl.umd.edu/papers/jahs16.pdf
// https://www.researchgate.net/figure/Flowfield-of-ring-source-potential-flow-model-evaluated-at-various-heights-depicting_fig4_282826775
// According to the Cheeseman and Bennett model it is only supposed to be applied
// when height ratio is in the range [0.5, 2] and assumes there is landing gear keeping it above that point
// However, our solution is to clamp the height to never be less than half the prop radius
// TODO(trevor): Research extreme ground effect at height ratio < 0.5
// https://www.sciencedirect.com/science/article/pii/S1000936124000645
vector3 motor_position_world = vector3_add(*position_world, motor_offset_world);
number prop_radius_meters = drone_states->prop_diameter_meters[i] / (number)2;
number half_prop_radius_meters = prop_radius_meters / (number)2;
number height_above_ground_meters = __builtin_elementwise_max(drone_states->ground_plane_distance_world[i][m] +
vector3_dot(drone_states->ground_plane_normal_world[i][m], motor_position_world),
half_prop_radius_meters);
number normalized_height_ratio = height_above_ground_meters / prop_radius_meters;
number in_ground_effect = number_from_bool(normalized_height_ratio < (number)2);
number height_term = 16 * height_above_ground_meters * height_above_ground_meters;
number ground_effect_scale = height_term / (height_term - prop_radius_meters * prop_radius_meters);
number in_ground_effect_scale = ground_effect_scale * in_ground_effect + ((number)1 - in_ground_effect);
propeller_force *= in_ground_effect_scale;
vector3 propeller_force_world = vector3_scalar_multiply(propeller_force, up_world);
vector3 propeller_torque_arm_world = apply_force_get_torque_world(
propeller_force_world,
motor_offset_world);
// https://mechtex.com/blog/understanding-torque-and-thrust-in-drone-motor
number propeller_torque_rotation = propeller_force * drone_states->thrust_to_torque_ratio[i] * get_motor_direction(0, m);
vector3 propeller_torque_rotation_world = vector3_scalar_multiply(propeller_torque_rotation, up_world);
*forces_world = vector3_add(*forces_world, propeller_force_world);
*torques_world = vector3_add(*torques_world, vector3_add(propeller_torque_rotation_world, propeller_torque_arm_world));
drone_states->propeller_force[i][m] = propeller_force;
drone_states->propeller_torque_rotation[i][m] = propeller_torque_rotation;
drone_states->propeller_torque_arm[i][m] = vector3_length(propeller_torque_arm_world);
}
physics_states->debug_output[i] = physics_states->torques_world[i];
// https://en.wikipedia.org/wiki/Barometric_formula
number gravity_magnitude = vector3_length(*gravity_world);
vector3 gravity_up_direction = vector3_scalar_multiply(1 / -number_fix_positive_divisor(gravity_magnitude, EPSILON), *gravity_world);
number height = vector3_dot(gravity_up_direction, *position_world);
number height_above_reference = height - drone_states->barometer_reference_height[i];
drone_states->barometer_pressure_pa[i] =
drone_states->barometer_reference_pressure_pa[i] *
__builtin_elementwise_pow(E, -gravity_magnitude * MOLAR_MASS_AIR * height_above_reference / (UNIVERSAL_GAS_CONSTANT * drone_states->barometer_reference_temperature_kelvin[i]));
}
}
typedef union {
struct
{
number pitch;
number yaw;
number roll;
number throttle;
};
number array[4];
} motor_mix;
typedef struct {
number arm_command[BODY_GROUP_COUNT];
number mode_command[BODY_GROUP_COUNT];
number throttle_low[BODY_GROUP_COUNT];
number throttle_mid[BODY_GROUP_COUNT];
number throttle_hover_point[BODY_GROUP_COUNT];
number axis_command[BODY_GROUP_COUNT][ALL_AXIS_COUNT];
number axis_expo[BODY_GROUP_COUNT][ALL_AXIS_COUNT];
number rates_center_sensitivity[BODY_GROUP_COUNT][PRIMARY_AXIS_COUNT];
number rates_max_rate[BODY_GROUP_COUNT][PRIMARY_AXIS_COUNT];
motor_mix motor_mix[BODY_GROUP_COUNT][MOTOR_COUNT];
// If this is non-zero, then we disable the PID and use this as a direct scalar for stick inputs
number no_pid_stick_scale[BODY_GROUP_COUNT];
number pid_p[BODY_GROUP_COUNT][PRIMARY_AXIS_COUNT];
number pid_i[BODY_GROUP_COUNT][PRIMARY_AXIS_COUNT];
number pid_d[BODY_GROUP_COUNT][PRIMARY_AXIS_COUNT];
number pid_error[BODY_GROUP_COUNT][PRIMARY_AXIS_COUNT];
number pid_error_sum[BODY_GROUP_COUNT][PRIMARY_AXIS_COUNT];
number pid_p_term[BODY_GROUP_COUNT][PRIMARY_AXIS_COUNT];
number pid_i_term[BODY_GROUP_COUNT][PRIMARY_AXIS_COUNT];
number pid_d_term[BODY_GROUP_COUNT][PRIMARY_AXIS_COUNT];
} controller_states;
STATES_FUNCTIONS(controller_states)
INDEXED_GETTER_SETTER(controller_states, arm_command)
INDEXED_GETTER_SETTER(controller_states, mode_command)
INDEXED_GETTER_SETTER(controller_states, throttle_low)
INDEXED_GETTER_SETTER(controller_states, throttle_mid)
INDEXED_GETTER_SETTER(controller_states, throttle_hover_point)
INDEXED_GETTER_SETTER_ARRAY(controller_states, axis_command)
INDEXED_GETTER_SETTER_ARRAY(controller_states, axis_expo)
INDEXED_GETTER_SETTER_ARRAY(controller_states, rates_center_sensitivity)
INDEXED_GETTER_SETTER_ARRAY(controller_states, rates_max_rate)
INDEXED_GETTER_SETTER(controller_states, no_pid_stick_scale)
INDEXED_GETTER_SETTER_ARRAY_VECTOR(controller_states, motor_mix)
INDEXED_GETTER_SETTER_ARRAY(controller_states, pid_p)
INDEXED_GETTER_SETTER_ARRAY(controller_states, pid_i)
INDEXED_GETTER_SETTER_ARRAY(controller_states, pid_d)
INDEXED_GETTER_SETTER_ARRAY(controller_states, pid_error)
INDEXED_GETTER_SETTER_ARRAY(controller_states, pid_error_sum)
INDEXED_GETTER_SETTER_ARRAY(controller_states, pid_p_term)
INDEXED_GETTER_SETTER_ARRAY(controller_states, pid_i_term)
INDEXED_GETTER_SETTER_ARRAY(controller_states, pid_d_term)
void controller_states_init_index(controller_states *const controller_states, const uintptr_t group_index) {
controller_states->arm_command[group_index] = -1;
controller_states->mode_command[group_index] = -1;
controller_states->throttle_low[group_index] = CONSTANT(0.05);
controller_states->throttle_mid[group_index] = 0.4;
controller_states->throttle_hover_point[group_index] = 0;
controller_states->axis_command[group_index][AXIS_THROTTLE] = -1;
controller_states->axis_expo[group_index][AXIS_THROTTLE] = 0.7;
controller_states->no_pid_stick_scale[group_index] = 0;
for (uintptr_t axis = 0; axis < PRIMARY_AXIS_COUNT; ++axis) {
controller_states->axis_command[group_index][axis] = 0;
controller_states->axis_expo[group_index][axis] = 0;
controller_states->rates_center_sensitivity[group_index][axis] = 70;
controller_states->rates_max_rate[group_index][axis] = 670;
controller_states->pid_error[group_index][axis] = 0;
controller_states->pid_error_sum[group_index][axis] = 0;
controller_states->pid_p_term[group_index][axis] = 0;
controller_states->pid_i_term[group_index][axis] = 0;
controller_states->pid_d_term[group_index][axis] = 0;
}
controller_states->pid_p[group_index][AXIS_PITCH] = CONSTANT(0.005);
controller_states->pid_i[group_index][AXIS_PITCH] = CONSTANT(0.00001);
controller_states->pid_d[group_index][AXIS_PITCH] = CONSTANT(0.005);
controller_states->pid_p[group_index][AXIS_YAW] = CONSTANT(0.003);
controller_states->pid_i[group_index][AXIS_YAW] = CONSTANT(0.000005);
controller_states->pid_d[group_index][AXIS_YAW] = CONSTANT(0.003);
controller_states->pid_p[group_index][AXIS_ROLL] = CONSTANT(0.005);
controller_states->pid_i[group_index][AXIS_ROLL] = CONSTANT(0.00001);
controller_states->pid_d[group_index][AXIS_ROLL] = CONSTANT(0.005);
// Setup default quadcopter motor mixing: P Y R T
controller_states->motor_mix[group_index][MOTOR_BR] = (motor_mix){+1, -1, -1, +1};
controller_states->motor_mix[group_index][MOTOR_FR] = (motor_mix){-1, +1, -1, +1};
controller_states->motor_mix[group_index][MOTOR_BL] = (motor_mix){+1, +1, +1, +1};
controller_states->motor_mix[group_index][MOTOR_FL] = (motor_mix){-1, -1, +1, +1};
}
EXPORT void controller_states_init(controller_states *const controller_states) {
for (uintptr_t i = 0; i < BODY_GROUP_COUNT; ++i) {
controller_states_init_index(controller_states, i);
}
}
number controller_states_angular_rate_degrees(number command, number center_sensitivity, number max_rate, number expo) {
// This is based on "Actual" rates mode, the easiest to configure and understand
number expo_curved = __builtin_elementwise_abs(command) * (__builtin_elementwise_pow(command, CONSTANT(5)) * expo + command * (1 - expo));
number stick_movement = __builtin_elementwise_max(CONSTANT(0), max_rate - center_sensitivity);
return command * center_sensitivity + stick_movement * expo_curved;
}
EXPORT float controller_states_angular_rate_degrees_query(float command, float center_sensitivity, float max_rate, float expo) {
// This is not the most efficient since it uses vector operations but we don't intend to query this function often
return NUMBER_ACCESS(controller_states_angular_rate_degrees(command, center_sensitivity, max_rate, expo), 0);
}
EXPORT void controller_states_update(controller_states *const controller_states, physics_states *const physics_states, drone_states *const drone_states) {
const uintptr_t group_count = ceil_division(physics_states->count, NUMBER_COMPONENTS);
for (uintptr_t i = 0; i < group_count; ++i) {
vector3 *angular_velocity_local = &physics_states->angular_velocity_local[i];
number normalized_throttle = controller_states->axis_command[i][AXIS_THROTTLE] * CONSTANT(0.5) + CONSTANT(0.5);
number is_armed = number_from_bool(controller_states->arm_command[i] > CONSTANT(0));
number is_throttle_active = number_from_bool(normalized_throttle > controller_states->throttle_low[i]);
number per_axis_command[PRIMARY_AXIS_COUNT];
for (uintptr_t axis = 0; axis < PRIMARY_AXIS_COUNT; ++axis) {
number current_angular_velocity_degrees = angular_velocity_local->array[axis] * RADIAN_TO_DEGREE;
// We need to map from stick direction to our reference frame
// For example when we pitch forward, the stick is going in the positive direction, and similarly the rotation
// around the X axis is also positive (same wih yaw). However, when we roll to the right (positive stick) the
// rotation is negative around the Z. This is due to us using a left handed coordinate system.
const number stick_to_axis_direction = axis == AXIS_ROLL ? -1 : 1;
number target_angular_velocity_degrees = controller_states_angular_rate_degrees(
controller_states->axis_command[i][axis] * stick_to_axis_direction,
controller_states->rates_center_sensitivity[i][axis],
controller_states->rates_max_rate[i][axis],
controller_states->axis_expo[i][axis]);
number *pid_error = &controller_states->pid_error[i][axis];
number *pid_error_sum = &controller_states->pid_error_sum[i][axis];
number error = (target_angular_velocity_degrees - current_angular_velocity_degrees) * is_armed;
number error_delta = error - *pid_error;
number error_sum = (*pid_error_sum + error) * is_armed * is_throttle_active;
*pid_error = error;
*pid_error_sum = error_sum;
number p_term = controller_states->pid_p[i][axis] * error;
number i_term = controller_states->pid_i[i][axis] * error_sum;
number d_term = controller_states->pid_d[i][axis] * error_delta;
controller_states->pid_p_term[i][axis] = p_term;
controller_states->pid_i_term[i][axis] = i_term;
controller_states->pid_d_term[i][axis] = d_term;
number use_pid = number_from_bool(controller_states->no_pid_stick_scale[i] == CONSTANT(0));
number pid_command = p_term + i_term + d_term;
number sticks_command = controller_states->no_pid_stick_scale[i] *
(target_angular_velocity_degrees / controller_states->rates_max_rate[i][axis]);
// The sticks_command will already be 0 if we're using the pid
number command = use_pid * pid_command + sticks_command;
per_axis_command[axis] = clamp(command * stick_to_axis_direction, CONSTANT(-1), CONSTANT(1));
}
// Motor mixing
for (uintptr_t m = 0; m < MOTOR_COUNT; ++m) {
number motor_output = 0;
for (uintptr_t axis = 0; axis < PRIMARY_AXIS_COUNT; ++axis) {
motor_output += controller_states->motor_mix[i][m].array[axis] * per_axis_command[axis];
}
motor_output += controller_states->motor_mix[i][m].array[AXIS_THROTTLE] * normalized_throttle;
drone_states->motor_outputs[i][m] = clamp(motor_output * is_armed, CONSTANT(0), CONSTANT(1));
}
}
}
#ifdef PHYSICS_MAIN
#define TEST_EPSILON 1e-5f
INLINE bool number_nearly_equal(number a, number b) {
return number_nearly_equal_epsilon(a, b, TEST_EPSILON);
}
INLINE bool component_nearly_equal(component a, component b) {
return __builtin_elementwise_abs(a - b) < TEST_EPSILON;
}
INLINE bool vector3_nearly_equal(vector3 a, vector3 b) {
return number_nearly_equal(a.x, b.x) &&
number_nearly_equal(a.y, b.y) &&
number_nearly_equal(a.z, b.z);
}
INLINE bool quaternion_nearly_equal(quaternion a, quaternion b) {
return number_nearly_equal(a.x, b.x) &&
number_nearly_equal(a.y, b.y) &&
number_nearly_equal(a.z, b.z) &&
number_nearly_equal(a.w, b.w);
}
INLINE void test(const uintptr_t result, const char *const file, const uintptr_t line, const char *const code) {
if (!result) {
printf("%s:%lu: failed test: %s\n", file, line, code);
abort();
}
}
#define TEST(result) test(result, __FILE__, __LINE__, #result)
void tests() {
// Test component_nearly_equal
{
TEST(component_nearly_equal(+1.0f, +1.0f));
TEST(component_nearly_equal(-1.0f, -1.0f));
TEST(component_nearly_equal(+0.0f, +0.0f));
TEST(!component_nearly_equal(+0.0f, +1.0f));
TEST(!component_nearly_equal(+1.0f, -1.0f));
}
// Test number_nearly_equal
{
TEST(number_nearly_equal(+1.0f, +1.0f));
TEST(number_nearly_equal(-1.0f, -1.0f));
TEST(number_nearly_equal(+0.0f, +0.0f));
TEST(!number_nearly_equal(+0.0f, +1.0f));
TEST(!number_nearly_equal(+1.0f, -1.0f));
}
// Test quaternion_length
{
TEST(number_nearly_equal(quaternion_length((quaternion){0, 0, 0, 0}), 0));
TEST(number_nearly_equal(quaternion_length((quaternion){0, 0, 0, 1}), 1));
TEST(number_nearly_equal(quaternion_length((quaternion){1, 2, 2, 1}), 3.16227766017f));
TEST(number_nearly_equal(quaternion_length((quaternion){2, 1, 1, 2}), 3.16227766017f));
}
// Test quaternion_normalize
{
TEST(quaternion_nearly_equal(quaternion_normalize((quaternion){0, 0, 0, 1}), (quaternion){0, 0, 0, 1}));
TEST(quaternion_nearly_equal(quaternion_normalize((quaternion){1, 1, 1, 1}), (quaternion){0.5f, 0.5f, 0.5f, 0.5f}));
TEST(quaternion_nearly_equal(quaternion_normalize((quaternion){0, 3, 0, 4}), (quaternion){0, 0.6f, 0, 0.8f}));
}
// Test quaternion_normalized_inverse
{
TEST(quaternion_nearly_equal(quaternion_normalized_inverse((quaternion){0, 0, 0, 1}), (quaternion){0, 0, 0, 1}));
TEST(quaternion_nearly_equal(quaternion_normalized_inverse((quaternion){0.2f, -0.4f, 0.6f, 0.65f}), (quaternion){-0.2f, 0.4f, -0.6f, 0.65f}));
TEST(quaternion_nearly_equal(quaternion_normalized_inverse((quaternion){0, 1, 0, 0}), (quaternion){0, -1, 0, 0}));
}
// Test vector3_length_squared
{
TEST(number_nearly_equal(vector3_length_squared((vector3){0, 0, 0}), 0));
TEST(number_nearly_equal(vector3_length_squared((vector3){0, 0, 1}), 1));
TEST(number_nearly_equal(vector3_length_squared((vector3){1, 2, 1}), 6));
TEST(number_nearly_equal(vector3_length_squared((vector3){2, 1, 1}), 6));
}
// Test vector3_length
{
TEST(number_nearly_equal(vector3_length((vector3){0, 0, 0}), 0));
TEST(number_nearly_equal(vector3_length((vector3){0, 0, 1}), 1));
TEST(number_nearly_equal(vector3_length((vector3){1, 2, 1}), 2.44948974278f));
TEST(number_nearly_equal(vector3_length((vector3){2, 1, 1}), 2.44948974278f));
}
// Test vector3_normalize
{
TEST(vector3_nearly_equal(vector3_normalize((vector3){0, 0, 1}, CONSTANT(0)), (vector3){0, 0, 1}));
TEST(vector3_nearly_equal(vector3_normalize((vector3){1, 1, 1}, CONSTANT(0)), (vector3){0.57735026919f, 0.57735026919f, 0.57735026919f}));
TEST(vector3_nearly_equal(vector3_normalize((vector3){0.1, 0.1, 0.1}, CONSTANT(0.5)), (vector3){0.1, 0.1, 0.1}));
}
// Test vector3_dot
{
TEST(number_nearly_equal(vector3_dot((vector3){+0, +0, +0}, (vector3){+0, +0, +0}), 0));
TEST(number_nearly_equal(vector3_dot((vector3){+1, +1, +1}, (vector3){+0, +0, +0}), 0));
TEST(number_nearly_equal(vector3_dot((vector3){+0, +0, +0}, (vector3){+1, +1, +1}), 0));
TEST(number_nearly_equal(vector3_dot((vector3){+1, +1, +1}, (vector3){+1, +1, +1}), 3));
TEST(number_nearly_equal(vector3_dot((vector3){+1, +3, -5}, (vector3){+4, -2, -1}), 3));
}
// Test vector3_cross
{
TEST(vector3_nearly_equal(vector3_cross((vector3){+1, +0, +0}, (vector3){+0, +1, +0}), (vector3){+0, +0, +1}));
TEST(vector3_nearly_equal(vector3_cross((vector3){+1, +0, +0}, (vector3){+0, +0, +1}), (vector3){+0, -1, +0}));
TEST(vector3_nearly_equal(vector3_cross((vector3){-1, -1, -1}, (vector3){+1, +2, +3}), (vector3){-1, +2, -1}));
TEST(vector3_nearly_equal(vector3_cross((vector3){+1, +1, +1}, (vector3){+1, +1, +1}), (vector3){+0, +0, +0}));
TEST(vector3_nearly_equal(vector3_cross((vector3){+2, +3, +4}, (vector3){+5, +6, +7}), (vector3){-3, +6, -3}));
}
// Test vector3_add
{
TEST(vector3_nearly_equal(vector3_add((vector3){+0, +0, +0}, (vector3){+0, +0, +0}), (vector3){+0, +0, +0}));
TEST(vector3_nearly_equal(vector3_add((vector3){+0, +0, +0}, (vector3){+1, +1, +1}), (vector3){+1, +1, +1}));
TEST(vector3_nearly_equal(vector3_add((vector3){+1, +1, +1}, (vector3){+0, +0, +0}), (vector3){+1, +1, +1}));
TEST(vector3_nearly_equal(vector3_add((vector3){+1, +1, +1}, (vector3){+1, +1, +1}), (vector3){+2, +2, +2}));
TEST(vector3_nearly_equal(vector3_add((vector3){+1, +1, +1}, (vector3){-1, -1, -1}), (vector3){+0, +0, +0}));
TEST(vector3_nearly_equal(vector3_add((vector3){+1, +2, +3}, (vector3){+4, +5, +6}), (vector3){+5, +7, +9}));
}
// Test vector3_subtract
{
TEST(vector3_nearly_equal(vector3_subtract((vector3){+0, +0, +0}, (vector3){+0, +0, +0}), (vector3){+0, +0, +0}));
TEST(vector3_nearly_equal(vector3_subtract((vector3){+0, +0, +0}, (vector3){+1, +1, +1}), (vector3){-1, -1, -1}));
TEST(vector3_nearly_equal(vector3_subtract((vector3){+1, +1, +1}, (vector3){+0, +0, +0}), (vector3){+1, +1, +1}));
}
// Test vector3_scalar_multiply
{
TEST(vector3_nearly_equal(vector3_scalar_multiply(0, (vector3){+1, +1, +1}), (vector3){+0, +0, +0}));
TEST(vector3_nearly_equal(vector3_scalar_multiply(4, (vector3){+0, -1, +2}), (vector3){+0, -4, +8}));
}
// Test vector3_abs
{
TEST(vector3_nearly_equal(vector3_abs((vector3){+1, +1, +1}), (vector3){+1, +1, +1}));
TEST(vector3_nearly_equal(vector3_abs((vector3){-1, -1, -1}), (vector3){+1, +1, +1}));
TEST(vector3_nearly_equal(vector3_abs((vector3){+0, +1, -2}), (vector3){+0, +1, +2}));
}
// Test rotate_vector_by_quaternion: Identity rotation
{
vector3 vec = {1, 2, 3};
quaternion identity = {0, 0, 0, 1};
vector3 result = rotate_vector_by_quaternion(vec, identity);
TEST(vector3_nearly_equal(result, vec));
}
// Test rotate_vector_by_quaternion: 180-degree rotation around Y axis
{
quaternion rot_y_180 = {0, 1, 0, 0};
vector3 result = rotate_vector_by_quaternion((vector3){1, 0, 0}, rot_y_180);
TEST(vector3_nearly_equal(result, (vector3){-1, 0, 0}));
}
// Test rotate_vector_by_quaternion: 90-degree rotation around X axis
{
quaternion rot_x_90 = {sqrtf(0.5f), 0, 0, sqrtf(0.5f)};
vector3 result = rotate_vector_by_quaternion((vector3){0, 1, 0}, rot_x_90);
TEST(vector3_nearly_equal(result, (vector3){0, 0, 1}));
}
// Test rotate_vector_by_quaternion: 90-degree rotation around Z axis
{
quaternion rot_z_90 = {0, 0, sqrtf(0.5f), sqrtf(0.5f)};
vector3 result = rotate_vector_by_quaternion((vector3){1, 0, 0}, rot_z_90);
TEST(vector3_nearly_equal(result, (vector3){0, 1, 0}));
}
// Test quaternion_add
{
TEST(quaternion_nearly_equal(quaternion_add((quaternion){0, 0, 0, 0}, (quaternion){1, 1, 1, 1}), (quaternion){1, 1, 1, 1}));
}
// Test quaternion_multiply
{
// Identity quaternion multiplication
const quaternion identity = (quaternion){0, 0, 0, 1};
const quaternion q = (quaternion){0.5f, -0.5f, 0.5f, -0.5f};
TEST(quaternion_nearly_equal(
quaternion_multiply(identity, q), q));
TEST(quaternion_nearly_equal(
quaternion_multiply(q, identity), q));
// 90-degree rotation around X then Y
const quaternion rot_x_90 = quaternion_normalize((quaternion){sqrtf(0.5f), 0, 0, sqrtf(0.5f)});
const quaternion rot_y_90 = quaternion_normalize((quaternion){0, sqrtf(0.5f), 0, sqrtf(0.5f)});
TEST(quaternion_nearly_equal(quaternion_multiply(rot_y_90, rot_x_90), quaternion_normalize((quaternion){0.5f, 0.5f, -0.5f, 0.5f})));
// Multiply quaternion by its inverse should yield identity
TEST(quaternion_nearly_equal(quaternion_multiply(q, quaternion_normalized_inverse(q)), identity));
}
// Test controller_states_angular_rate_degrees
{
TEST(number_nearly_equal(controller_states_angular_rate_degrees(CONSTANT(+0.0), 70, 670, 0), 0));
TEST(number_nearly_equal(controller_states_angular_rate_degrees(CONSTANT(+1.0), 70, 670, 0), +670));
TEST(number_nearly_equal(controller_states_angular_rate_degrees(CONSTANT(-1.0), 70, 670, 0), -670));
TEST(number_nearly_equal(controller_states_angular_rate_degrees(CONSTANT(+1.0), 70, 670, 1), +670));
TEST(number_nearly_equal(controller_states_angular_rate_degrees(CONSTANT(-1.0), 70, 670, 1), -670));
TEST(number_nearly_equal(controller_states_angular_rate_degrees(CONSTANT(+0.1), 70, 670, 0), +13));
TEST(number_nearly_equal(controller_states_angular_rate_degrees(CONSTANT(-0.1), 70, 670, 0), -13));
}
}
int main(void) {
tests();
const uintptr_t PHYSICS_HZ = 8000;
const float PHSICS_FRAME_TIME = 1.0f / (float)PHYSICS_HZ;
physics_states physics_states;
drone_states drone_states;
physics_states_init(&physics_states);
drone_states_init(&drone_states);
physics_states_set_count(&physics_states, 1);
physics_states_set_position_world(&physics_states, 0, Y, 15.0f);
physics_states_set_angular_velocity_world(&physics_states, 0, X, 3.0f);
// 90 degree rotation in the Z
/*
physics_states_set_orientation_world(&physics_states, 0, X, 0);
physics_states_set_orientation_world(&physics_states, 0, Y, 0);
physics_states_set_orientation_world(&physics_states, 0, Z, 0.707106829);
physics_states_set_orientation_world(&physics_states, 0, W, 0.707106829);
//*/
// One second of simulation
for (uintptr_t i = 0; i < PHYSICS_HZ; ++i) {
drone_states_update(&drone_states, &physics_states);
physics_states_update(&physics_states, PHSICS_FRAME_TIME);
}
printf("POSITION Y: %f\n", physics_states_get_position_world(&physics_states, 0, Y));
printf("ANGULAR VELOCITY X: %f\n", physics_states_get_angular_velocity_world(&physics_states, 0, X));
printf("BAROMETER: %f\n", drone_states_get_barometer_pressure_pa(&drone_states, 0));
return 0;
}
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment