Ymir/native/Ymir.Box3D/ymir_box3d_soa.c
Meta Vens 55d0e0f0bd SoA world: joint handles carry a slot generation; ABI 6
DestroyJoint takes (lane, generation) and refuses a stale handle, so a repeated
release cannot destroy a later grab in a reused slot. The ABI number moves to 6
so an old and a new native/managed pair fail closed at construction. Fixes Soul
s2 findings.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
2026-11-20 02:41:42 +01:00

1193 lines
45 KiB
C

// Headless 3D Box3D world with a structure-of-arrays state view. Box3D owns every physical
// decision (integration, collision, contact, joints); this file creates bodies and joints, applies
// the caller's force and torque lanes, steps the world and copies each body's and joint's state into
// contiguous lanes once per step. A caller reads the state lanes and writes the force/torque lanes in
// place through `ymir_box3d_soa_get_view`: the pointers are stable for the world's whole life
// (capacities are fixed at creation) and the state lanes change only inside `ymir_box3d_soa_step`.
//
// This world is not the retained planar session in ymir_box3d.c: that one locks Y, zeroes gravity
// and hands out an array of structs by copy. See docs/soa-world.md.
#include <box3d/base.h>
#include <box3d/box3d.h>
#include <box3d/collision.h>
#include <box3d/math_functions.h>
#include <box3d/types.h>
#include "ymir_world_lock.h"
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#if defined(_WIN32)
#define YMIR_EXPORT __declspec(dllexport)
#define YMIR_CALL __cdecl
#else
#define YMIR_EXPORT __attribute__((visibility("default")))
#define YMIR_CALL
#endif
enum
{
YMIR_SOA_OK = 0,
YMIR_SOA_INVALID_ARGUMENT = 1,
YMIR_SOA_DUPLICATE_ID = 2,
YMIR_SOA_FULL = 3,
YMIR_SOA_OUT_OF_MEMORY = 4,
YMIR_SOA_INTERNAL_ERROR = 5
};
enum
{
YMIR_SOA_KIND_SPHERE = 0,
YMIR_SOA_KIND_STATIC_BOX = 1,
YMIR_SOA_KIND_CAPSULE = 2,
YMIR_SOA_KIND_BOX = 3,
YMIR_SOA_KIND_ANCHOR = 4
};
enum
{
YMIR_SOA_JOINT_REVOLUTE = 0,
YMIR_SOA_JOINT_SPHERICAL = 1,
YMIR_SOA_JOINT_WELD = 2,
YMIR_SOA_JOINT_MOTOR = 3,
YMIR_SOA_JOINT_REMOVED = 4 // a destroyed joint's lane: never accepted by add_joint
};
#define YMIR_SOA_ABI_VERSION 6u
// A body to create. `size` by kind: sphere {radius}, capsule {radius, half_length} (segment along
// local Y), box and static box {half_x, half_y, half_z}. `parent` is the lane index of an earlier body
// or -1; `limb` is the caller's label. Neither is used by Box3D: they are lanes for the consumer.
// An anchor (kind 4) is a kinematic body with no shape: it is never hit and never collides, and size,
// density, friction, restitution, rolling resistance and material are ignored. Its pose is set with
// `ymir_box3d_soa_set_anchor_target`; the force and torque lanes never move it.
typedef struct ymir_box3d_soa_body_desc
{
uint64_t stable_id;
uint32_t kind;
int32_t parent;
int32_t limb;
uint32_t reserved;
float position[3];
float rotation[4]; // x y z w, unit
float velocity[3];
float size[3];
float density;
float friction;
float restitution;
float rolling_resistance; // Box3D's surface rolling resistance, >= 0; spheres and capsules roll without it
uint64_t material_id; // the caller's surface material id: set as the shape's user material, echoed on hit facts
} ymir_box3d_soa_body_desc;
// A joint between two bodies, given in world space at creation: the frame has its origin at `anchor`
// and its z axis along `axis`; the creation pose is the joint's zero. Revolute turns about that z axis.
// `enable_limit` applies `lower`/`upper` (revolute angle, spherical twist), and `cone_angle` (spherical
// swing) when it is positive. Weld ignores limits.
// A motor joint (kind 3) is a pair of position springs, no velocity target: the linear spring pulls body B's
// frame origin onto body A's (so B's anchor point onto A's), the angular spring pulls B's frame orientation onto
// A's. `*_hertz` 0 turns that spring off; `max_force` (N) and `max_torque` (N m) bound what it may apply, so a
// weak grab hangs below its target. All six are finite and >= 0 and are ignored by the other kinds.
// `ymir_box3d_soa_destroy_joint` releases a motor joint: its lane becomes a tombstone (kind REMOVED = 4, bodies -1,
// every lane 0) that add_joint may reuse only once a step has completed since.
typedef struct ymir_box3d_soa_joint_desc
{
uint32_t kind;
int32_t body_a;
int32_t body_b;
uint32_t enable_limit;
float anchor[3];
float axis[3];
float lower;
float upper;
float cone_angle;
float linear_hertz;
float linear_damping;
float max_force;
float angular_hertz;
float angular_damping;
float max_torque;
} ymir_box3d_soa_joint_desc;
// Pointers first and in lane order, so the layout is the same on every 64-bit target. Lanes the caller
// writes (force and torque per body) are mutable; every other lane is read-only to the caller.
typedef struct ymir_box3d_soa_view
{
uint32_t abi_version;
int32_t count;
int32_t capacity;
int32_t joint_count;
int32_t joint_capacity;
uint32_t reserved;
uint64_t step_count;
// bodies
const uint64_t* stable_id;
const uint32_t* kind;
const int32_t* parent;
const int32_t* limb;
// The body's shape size, read-only, same per-kind meaning as the body desc size: sphere {radius},
// capsule {radius, half_length}, box and static box {half_x, half_y, half_z}; unused components are 0.
const float* size_0;
const float* size_1;
const float* size_2;
const float* position_x;
const float* position_y;
const float* position_z;
const float* rotation_x;
const float* rotation_y;
const float* rotation_z;
const float* rotation_w;
const float* velocity_x;
const float* velocity_y;
const float* velocity_z;
const float* angular_velocity_x;
const float* angular_velocity_y;
const float* angular_velocity_z;
float* force_x;
float* force_y;
float* force_z;
float* torque_x;
float* torque_y;
float* torque_z;
// joints
const uint32_t* joint_kind;
const int32_t* joint_body_a;
const int32_t* joint_body_b;
const float* joint_angle;
const float* joint_frame_x;
const float* joint_frame_y;
const float* joint_frame_z;
const float* joint_frame_w;
const float* joint_rotation_x;
const float* joint_rotation_y;
const float* joint_rotation_z;
const float* joint_rotation_w;
const float* joint_angular_velocity_x;
const float* joint_angular_velocity_y;
const float* joint_angular_velocity_z;
// Contact lanes, read-only, refreshed every step. last_substep_force is N: the sum over the body's touching
// contact manifold points of the final sub-step's normalImpulse * sub_steps / dt (0 for a body touching
// nothing). It is the load a body carries at rest and NOT an impact measure: it sees one sub-step, so it
// reads 0.02x to 1.9x the true force on an impact step. For impact severity see the hit fact's impulse_estimate and
// the limits stated there.
// contact_count is the number of touching contacts. material_id is the body's desc material_id.
const float* last_substep_force;
const int32_t* contact_count;
const uint64_t* material_id;
// Per joint slot: counts the destroys of that slot. A grab's handle is (lane, generation); destroy_joint refuses
// a handle whose generation is not the slot's, so a stale release cannot destroy a later grab in a reused slot.
const uint32_t* joint_generation;
} ymir_box3d_soa_view;
// A begin or end of touch between two bodies: their lane indices and the shapes' stable ids.
typedef struct ymir_box3d_soa_touch_fact
{
int32_t lane_a;
int32_t lane_b;
uint64_t stable_id_a;
uint64_t stable_id_b;
} ymir_box3d_soa_touch_fact;
// A hit: a touch approaching faster than the world's hit threshold. `normal` points from A to B.
typedef struct ymir_box3d_soa_hit_fact
{
int32_t lane_a;
int32_t lane_b;
uint64_t stable_id_a;
uint64_t stable_id_b;
float point[3];
float normal[3];
float approach_speed;
// The normal impulse (N s) this collision must deliver, from the approach speed, the two bodies' masses
// and the pair's restitution: v (1 + e) / (1/mA + 1/mB), with e applying only above Box3D's restitution
// threshold and a static body's 1/m taken as 0.
// Exact for a centre-to-centre hit between free bodies (a sphere on a plane, two spheres). It can over-count
// when one collision yields several hit facts (box corners, tilted capsules), which sum, and it undercounts
// heavily for jointed bodies (it takes the limb's own mass, not the joint-coupled effective mass). It is not
// a bound either way. A mass-aware per-body impulse that holds for jointed bodies is being designed.
float impulse_estimate;
uint64_t material_a;
uint64_t material_b;
} ymir_box3d_soa_hit_fact;
// What Box3D reported for the step just taken, in Box3D's event order. The arrays are fixed for the
// world's life and rewritten by every step: facts last one step. `dropped` counts facts past
// `fact_capacity` that this step could not keep.
typedef struct ymir_box3d_soa_facts
{
uint32_t begin_count;
uint32_t end_count;
uint32_t hit_count;
uint32_t fact_capacity;
uint32_t begin_dropped;
uint32_t end_dropped;
uint32_t hit_dropped;
uint32_t reserved;
const ymir_box3d_soa_touch_fact* begin;
const ymir_box3d_soa_touch_fact* end;
const ymir_box3d_soa_hit_fact* hit;
} ymir_box3d_soa_facts;
// The closest hit of a ray. `hit` is 0 for a miss and every other field is then 0. `lane` and `stable_id` name
// the body and shape hit; `fraction` is of the ray's translation.
typedef struct ymir_box3d_soa_ray_hit
{
int32_t hit;
int32_t lane;
uint64_t stable_id;
float point[3];
float normal[3];
float fraction;
uint32_t reserved;
} ymir_box3d_soa_ray_hit;
#define YMIR_SOA_STATE_LANES 13
#define YMIR_SOA_WRITE_LANES 6
#define YMIR_SOA_LAST_SUBSTEP_FORCE_LANE (YMIR_SOA_STATE_LANES + YMIR_SOA_WRITE_LANES)
#define YMIR_SOA_BODY_FLOAT_LANES (YMIR_SOA_LAST_SUBSTEP_FORCE_LANE + 1)
#define YMIR_SOA_JOINT_FLOAT_LANES 12
typedef struct ymir_box3d_soa_world
{
b3WorldId world_id;
int32_t count;
int32_t capacity;
int32_t joint_count;
int32_t joint_capacity;
uint64_t step_count;
b3BodyId* bodies;
uint64_t* stable_id;
uint32_t* kind;
int32_t* parent;
int32_t* limb;
int32_t* contact_count;
uint64_t* material_id;
float* size; // 3 arrays of `capacity` floats, lane-major; written once in add_body
float* lanes; // YMIR_SOA_BODY_FLOAT_LANES arrays of `capacity` floats, lane-major
b3JointId* joints;
uint32_t* joint_kind;
uint32_t* joint_generation;
uint64_t* joint_removed_at; // step_count when the slot was destroyed; reusable once a step has passed
int32_t* joint_body_a;
int32_t* joint_body_b;
b3Quat* joint_local_a; // joint frame in body A, kept so the state is read without asking Box3D
b3Quat* joint_local_b;
float* joint_lanes; // YMIR_SOA_JOINT_FLOAT_LANES arrays of `joint_capacity` floats, lane-major
uint32_t fact_capacity;
ymir_box3d_soa_facts facts; // counts and dropped counters live here; the arrays below back its pointers
ymir_box3d_soa_touch_fact* begin_facts;
ymir_box3d_soa_touch_fact* end_facts;
ymir_box3d_soa_hit_fact* hit_facts;
b3ContactData* contact_scratch; // `capacity` entries: a body has one shape, so at most capacity - 1 contacts
} ymir_box3d_soa_world;
static float* ymir_soa_lane(const ymir_box3d_soa_world* world, int lane)
{
return world->lanes + (size_t)lane * (size_t)world->capacity;
}
static float* ymir_soa_joint_lane(const ymir_box3d_soa_world* world, int lane)
{
return world->joint_lanes + (size_t)lane * (size_t)world->joint_capacity;
}
static int ymir_soa_finite(float v)
{
return isfinite(v);
}
static int ymir_soa_finite3(const float* v)
{
return ymir_soa_finite(v[0]) && ymir_soa_finite(v[1]) && ymir_soa_finite(v[2]);
}
static int ymir_soa_is_anchor(uint32_t kind)
{
return kind == YMIR_SOA_KIND_ANCHOR;
}
static int ymir_soa_is_dynamic(uint32_t kind)
{
return kind != YMIR_SOA_KIND_STATIC_BOX && !ymir_soa_is_anchor(kind);
}
static void ymir_soa_refresh(ymir_box3d_soa_world* world, int index)
{
b3BodyId body = world->bodies[index];
b3Pos p = b3Body_GetPosition(body);
b3Quat q = b3Body_GetRotation(body);
b3Vec3 v = b3Body_GetLinearVelocity(body);
b3Vec3 w = b3Body_GetAngularVelocity(body);
const float values[YMIR_SOA_STATE_LANES] = {
p.x, p.y, p.z, q.v.x, q.v.y, q.v.z, q.s, v.x, v.y, v.z, w.x, w.y, w.z};
for (int lane = 0; lane < YMIR_SOA_STATE_LANES; ++lane)
{
ymir_soa_lane(world, lane)[index] = values[lane];
}
}
// Joint state from the bodies: the world orientation of frame A, frame B relative to frame A (as a
// quaternion with w >= 0, the shorter arc) and the relative angular velocity in frame A. The angle
// lane is Box3D's own: the revolute angle or the spherical twist angle, zero for a weld.
static void ymir_soa_refresh_joint(ymir_box3d_soa_world* world, int j)
{
int a = world->joint_body_a[j];
int b = world->joint_body_b[j];
b3Quat qa = b3MulQuat(b3Body_GetRotation(world->bodies[a]), world->joint_local_a[j]);
b3Quat qb = b3MulQuat(b3Body_GetRotation(world->bodies[b]), world->joint_local_b[j]);
b3Quat rel = b3InvMulQuat(qa, qb);
if (rel.s < 0.0f)
{
rel = b3NegateQuat(rel);
}
b3Vec3 w = b3InvRotateVector(
qa, b3Sub(b3Body_GetAngularVelocity(world->bodies[b]), b3Body_GetAngularVelocity(world->bodies[a])));
float angle = 0.0f;
if (world->joint_kind[j] == YMIR_SOA_JOINT_REVOLUTE)
{
angle = b3RevoluteJoint_GetAngle(world->joints[j]);
}
else if (world->joint_kind[j] == YMIR_SOA_JOINT_SPHERICAL)
{
angle = b3SphericalJoint_GetTwistAngle(world->joints[j]);
}
const float values[YMIR_SOA_JOINT_FLOAT_LANES] = {
angle, qa.v.x, qa.v.y, qa.v.z, qa.s, rel.v.x, rel.v.y, rel.v.z, rel.s, w.x, w.y, w.z};
for (int lane = 0; lane < YMIR_SOA_JOINT_FLOAT_LANES; ++lane)
{
ymir_soa_joint_lane(world, lane)[j] = values[lane];
}
}
YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_abi_version(void)
{
return YMIR_SOA_ABI_VERSION;
}
YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_view_size(void)
{
return (uint32_t)sizeof(ymir_box3d_soa_view);
}
YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_body_desc_size(void)
{
return (uint32_t)sizeof(ymir_box3d_soa_body_desc);
}
YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_joint_desc_size(void)
{
return (uint32_t)sizeof(ymir_box3d_soa_joint_desc);
}
YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_ray_hit_size(void)
{
return (uint32_t)sizeof(ymir_box3d_soa_ray_hit);
}
YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_touch_fact_size(void)
{
return (uint32_t)sizeof(ymir_box3d_soa_touch_fact);
}
YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_hit_fact_size(void)
{
return (uint32_t)sizeof(ymir_box3d_soa_hit_fact);
}
YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_facts_size(void)
{
return (uint32_t)sizeof(ymir_box3d_soa_facts);
}
static void ymir_soa_free(ymir_box3d_soa_world* world)
{
free(world->bodies);
free(world->stable_id);
free(world->kind);
free(world->parent);
free(world->limb);
free(world->contact_count);
free(world->material_id);
free(world->size);
free(world->lanes);
free(world->joints);
free(world->joint_kind);
free(world->joint_removed_at);
free(world->joint_generation);
free(world->joint_body_a);
free(world->joint_body_b);
free(world->joint_local_a);
free(world->joint_local_b);
free(world->joint_lanes);
free(world->begin_facts);
free(world->end_facts);
free(world->hit_facts);
free(world->contact_scratch);
free(world);
}
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_create(
int32_t capacity, int32_t joint_capacity, float gravity_x, float gravity_y, float gravity_z,
int32_t fact_capacity, float hit_threshold, ymir_box3d_soa_world** output)
{
if (output == NULL)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
*output = NULL;
if (capacity <= 0 || joint_capacity < 0 || fact_capacity <= 0 || !ymir_soa_finite(gravity_x) ||
!ymir_soa_finite(gravity_y) || !ymir_soa_finite(gravity_z) || !ymir_soa_finite(hit_threshold) ||
hit_threshold < 0.0f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
ymir_box3d_soa_world* world = (ymir_box3d_soa_world*)calloc(1, sizeof(ymir_box3d_soa_world));
if (world == NULL)
{
return YMIR_SOA_OUT_OF_MEMORY;
}
world->capacity = capacity;
world->joint_capacity = joint_capacity;
size_t n = (size_t)capacity;
size_t m = (size_t)joint_capacity > 0 ? (size_t)joint_capacity : 1; // calloc(0) may return NULL
world->bodies = (b3BodyId*)calloc(n, sizeof(b3BodyId));
world->stable_id = (uint64_t*)calloc(n, sizeof(uint64_t));
world->kind = (uint32_t*)calloc(n, sizeof(uint32_t));
world->parent = (int32_t*)calloc(n, sizeof(int32_t));
world->limb = (int32_t*)calloc(n, sizeof(int32_t));
world->contact_count = (int32_t*)calloc(n, sizeof(int32_t));
world->material_id = (uint64_t*)calloc(n, sizeof(uint64_t));
world->size = (float*)calloc(n * 3, sizeof(float));
world->lanes = (float*)calloc(n * YMIR_SOA_BODY_FLOAT_LANES, sizeof(float));
world->joints = (b3JointId*)calloc(m, sizeof(b3JointId));
world->joint_kind = (uint32_t*)calloc(m, sizeof(uint32_t));
world->joint_removed_at = (uint64_t*)calloc(m, sizeof(uint64_t));
world->joint_generation = (uint32_t*)calloc(m, sizeof(uint32_t));
world->joint_body_a = (int32_t*)calloc(m, sizeof(int32_t));
world->joint_body_b = (int32_t*)calloc(m, sizeof(int32_t));
world->joint_local_a = (b3Quat*)calloc(m, sizeof(b3Quat));
world->joint_local_b = (b3Quat*)calloc(m, sizeof(b3Quat));
world->joint_lanes = (float*)calloc(m * YMIR_SOA_JOINT_FLOAT_LANES, sizeof(float));
world->fact_capacity = (uint32_t)fact_capacity;
world->begin_facts = (ymir_box3d_soa_touch_fact*)calloc((size_t)fact_capacity, sizeof(ymir_box3d_soa_touch_fact));
world->end_facts = (ymir_box3d_soa_touch_fact*)calloc((size_t)fact_capacity, sizeof(ymir_box3d_soa_touch_fact));
world->hit_facts = (ymir_box3d_soa_hit_fact*)calloc((size_t)fact_capacity, sizeof(ymir_box3d_soa_hit_fact));
world->contact_scratch = (b3ContactData*)calloc(n, sizeof(b3ContactData));
if (world->contact_count == NULL || world->material_id == NULL || world->begin_facts == NULL ||
world->end_facts == NULL || world->hit_facts == NULL || world->contact_scratch == NULL ||
world->bodies == NULL || world->stable_id == NULL || world->kind == NULL || world->parent == NULL ||
world->limb == NULL || world->size == NULL || world->lanes == NULL || world->joints == NULL || world->joint_kind == NULL ||
world->joint_body_a == NULL || world->joint_body_b == NULL || world->joint_local_a == NULL ||
world->joint_local_b == NULL || world->joint_lanes == NULL || world->joint_removed_at == NULL ||
world->joint_generation == NULL)
{
ymir_soa_free(world);
return YMIR_SOA_OUT_OF_MEMORY;
}
b3WorldDef def = b3DefaultWorldDef();
def.gravity = (b3Vec3){gravity_x, gravity_y, gravity_z};
def.enableSleep = false;
def.hitEventThreshold = hit_threshold;
def.workerCount = 1; // one worker: the step order is fixed, which determinism rests on
world->world_id = ymir_create_world_locked(&def);
if (B3_IS_NULL(world->world_id))
{
ymir_soa_free(world);
return YMIR_SOA_INTERNAL_ERROR;
}
*output = world;
return YMIR_SOA_OK;
}
YMIR_EXPORT void YMIR_CALL ymir_box3d_soa_destroy(ymir_box3d_soa_world* world)
{
if (world == NULL)
{
return;
}
if (B3_IS_NON_NULL(world->world_id) && b3World_IsValid(world->world_id))
{
ymir_destroy_world_locked(world->world_id);
}
ymir_soa_free(world);
}
static int32_t ymir_soa_check_body(const ymir_box3d_soa_world* world, const ymir_box3d_soa_body_desc* d)
{
if (d->kind > YMIR_SOA_KIND_ANCHOR || !ymir_soa_finite3(d->position) || !ymir_soa_finite3(d->velocity) ||
!ymir_soa_finite3(d->size) || !ymir_soa_finite(d->friction) || !ymir_soa_finite(d->restitution) ||
!ymir_soa_finite(d->density) || !ymir_soa_finite(d->rolling_resistance) || d->friction < 0.0f ||
d->restitution < 0.0f || d->rolling_resistance < 0.0f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
float q2 = 0.0f;
for (int i = 0; i < 4; ++i)
{
if (!ymir_soa_finite(d->rotation[i]))
{
return YMIR_SOA_INVALID_ARGUMENT;
}
q2 += d->rotation[i] * d->rotation[i];
}
if (fabsf(q2 - 1.0f) > 1e-3f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
if (d->parent < -1 || d->parent >= world->count)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
if (ymir_soa_is_dynamic(d->kind) && d->density <= 0.0f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
switch (d->kind)
{
case YMIR_SOA_KIND_ANCHOR:
break; // no shape: nothing to size
case YMIR_SOA_KIND_SPHERE:
if (d->size[0] <= 0.0f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
break;
case YMIR_SOA_KIND_CAPSULE:
if (d->size[0] <= 0.0f || d->size[1] <= 0.0f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
break;
default:
if (d->size[0] <= 0.0f || d->size[1] <= 0.0f || d->size[2] <= 0.0f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
break;
}
if (world->count >= world->capacity)
{
return YMIR_SOA_FULL;
}
for (int i = 0; i < world->count; ++i)
{
if (world->stable_id[i] == d->stable_id)
{
return YMIR_SOA_DUPLICATE_ID;
}
}
return YMIR_SOA_OK;
}
// Creates one body from a descriptor. Box3D owns mass: it follows from `density` and the shape.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_body(
ymir_box3d_soa_world* world, const ymir_box3d_soa_body_desc* d, int32_t* index)
{
if (world == NULL || d == NULL)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
int32_t status = ymir_soa_check_body(world, d);
if (status != YMIR_SOA_OK)
{
return status;
}
b3BodyDef body_def = b3DefaultBodyDef();
body_def.type = ymir_soa_is_anchor(d->kind) ? b3_kinematicBody
: ymir_soa_is_dynamic(d->kind) ? b3_dynamicBody
: b3_staticBody;
body_def.position = (b3Vec3){d->position[0], d->position[1], d->position[2]};
body_def.rotation = (b3Quat){{d->rotation[0], d->rotation[1], d->rotation[2]}, d->rotation[3]};
body_def.linearVelocity = (b3Vec3){d->velocity[0], d->velocity[1], d->velocity[2]};
body_def.enableSleep = false;
body_def.userData = (void*)((uintptr_t)world->count + 1u); // lane + 1: a shape resolves to its lane
b3BodyId body = b3CreateBody(world->world_id, &body_def);
if (B3_IS_NULL(body))
{
return YMIR_SOA_INTERNAL_ERROR;
}
if (!ymir_soa_is_anchor(d->kind)) // an anchor has no shape
{
b3ShapeDef shape_def = b3DefaultShapeDef();
shape_def.userData = (void*)(uintptr_t)d->stable_id;
shape_def.enableContactEvents = true; // off by default in Box3D
shape_def.enableHitEvents = true;
shape_def.baseMaterial.userMaterialId = d->material_id;
if (ymir_soa_is_dynamic(d->kind))
{
shape_def.density = d->density;
}
shape_def.baseMaterial.friction = d->friction;
shape_def.baseMaterial.restitution = d->restitution;
shape_def.baseMaterial.rollingResistance = d->rolling_resistance;
b3ShapeId shape;
if (d->kind == YMIR_SOA_KIND_SPHERE)
{
b3Sphere sphere = {{0.0f, 0.0f, 0.0f}, d->size[0]};
shape = b3CreateSphereShape(body, &shape_def, &sphere);
}
else if (d->kind == YMIR_SOA_KIND_CAPSULE)
{
b3Capsule capsule = {{0.0f, -d->size[1], 0.0f}, {0.0f, d->size[1], 0.0f}, d->size[0]};
shape = b3CreateCapsuleShape(body, &shape_def, &capsule);
}
else
{
b3BoxHull box = b3MakeBoxHull(d->size[0], d->size[1], d->size[2]);
shape = b3CreateHullShape(body, &shape_def, &box.base);
}
if (B3_IS_NULL(shape))
{
b3DestroyBody(body);
return YMIR_SOA_INTERNAL_ERROR;
}
}
int i = world->count++;
world->bodies[i] = body;
world->stable_id[i] = d->stable_id;
world->kind[i] = d->kind;
world->parent[i] = d->parent;
world->limb[i] = d->limb;
world->material_id[i] = ymir_soa_is_anchor(d->kind) ? 0 : d->material_id;
/* Publish only the components the kind uses; unused ones read 0 whatever the caller passed. */
int used = ymir_soa_is_anchor(d->kind) ? 0 : d->kind == YMIR_SOA_KIND_SPHERE ? 1 : d->kind == YMIR_SOA_KIND_CAPSULE ? 2 : 3;
for (int k = 0; k < 3; k++)
{
world->size[(size_t)k * (size_t)world->capacity + (size_t)i] = k < used ? d->size[k] : 0.0f;
}
ymir_soa_refresh(world, i);
if (index != NULL)
{
*index = i;
}
return YMIR_SOA_OK;
}
static b3Transform ymir_soa_body_transform(b3BodyId body)
{
b3Pos p = b3Body_GetPosition(body);
b3Transform t = {{p.x, p.y, p.z}, b3Body_GetRotation(body)};
return t;
}
// The lane a new joint takes: the lowest tombstone whose destroying step has completed, else the next fresh
// slot, else -1. A destroyed lane is not reused within the step that destroyed it, so lanes a caller read
// that step stay meaningful.
static int ymir_soa_joint_slot(const ymir_box3d_soa_world* world)
{
for (int j = 0; j < world->joint_count; ++j)
{
if (world->joint_kind[j] == YMIR_SOA_JOINT_REMOVED && world->step_count > world->joint_removed_at[j])
{
return j;
}
}
return world->joint_count < world->joint_capacity ? world->joint_count : -1;
}
// Joints that add_joint could create right now.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_free_joint_slots(const ymir_box3d_soa_world* world)
{
if (world == NULL)
{
return 0;
}
int32_t free_slots = world->joint_capacity - world->joint_count;
for (int j = 0; j < world->joint_count; ++j)
{
free_slots += world->joint_kind[j] == YMIR_SOA_JOINT_REMOVED && world->step_count > world->joint_removed_at[j];
}
return free_slots;
}
// Creates a joint between two existing bodies. The joint frame is built in world space (origin at the
// anchor, z along the axis) and expressed in each body's local frame, so the creation pose is zero.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_joint(
ymir_box3d_soa_world* world, const ymir_box3d_soa_joint_desc* d, int32_t* index)
{
if (world == NULL || d == NULL || d->kind > YMIR_SOA_JOINT_MOTOR || d->body_a < 0 || d->body_a >= world->count ||
d->body_b < 0 || d->body_b >= world->count || d->body_a == d->body_b || !ymir_soa_finite3(d->anchor) ||
!ymir_soa_finite3(d->axis) || !ymir_soa_finite(d->lower) || !ymir_soa_finite(d->upper) ||
!ymir_soa_finite(d->cone_angle) || d->cone_angle < 0.0f || d->cone_angle > B3_PI ||
!ymir_soa_finite(d->linear_hertz) || !ymir_soa_finite(d->linear_damping) || !ymir_soa_finite(d->max_force) ||
!ymir_soa_finite(d->angular_hertz) || !ymir_soa_finite(d->angular_damping) || !ymir_soa_finite(d->max_torque) ||
d->linear_hertz < 0.0f || d->linear_damping < 0.0f || d->max_force < 0.0f || d->angular_hertz < 0.0f ||
d->angular_damping < 0.0f || d->max_torque < 0.0f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
b3Vec3 axis = {d->axis[0], d->axis[1], d->axis[2]};
float length = sqrtf(b3Dot(axis, axis));
if (length < 1e-6f || (d->enable_limit != 0 && d->kind != YMIR_SOA_JOINT_WELD && d->lower > d->upper))
{
return YMIR_SOA_INVALID_ARGUMENT;
}
int slot = ymir_soa_joint_slot(world);
if (slot < 0)
{
return YMIR_SOA_FULL;
}
axis = b3MulSV(1.0f / length, axis);
b3Transform frame = {{d->anchor[0], d->anchor[1], d->anchor[2]}, b3ComputeQuatBetweenUnitVectors((b3Vec3){0.0f, 0.0f, 1.0f}, axis)};
b3Transform local_a = b3InvMulTransforms(ymir_soa_body_transform(world->bodies[d->body_a]), frame);
b3Transform local_b = b3InvMulTransforms(ymir_soa_body_transform(world->bodies[d->body_b]), frame);
b3JointId joint;
if (d->kind == YMIR_SOA_JOINT_REVOLUTE)
{
b3RevoluteJointDef def = b3DefaultRevoluteJointDef();
def.base.bodyIdA = world->bodies[d->body_a];
def.base.bodyIdB = world->bodies[d->body_b];
def.base.localFrameA = local_a;
def.base.localFrameB = local_b;
def.enableLimit = d->enable_limit != 0;
def.lowerAngle = d->lower;
def.upperAngle = d->upper;
joint = b3CreateRevoluteJoint(world->world_id, &def);
}
else if (d->kind == YMIR_SOA_JOINT_SPHERICAL)
{
b3SphericalJointDef def = b3DefaultSphericalJointDef();
def.base.bodyIdA = world->bodies[d->body_a];
def.base.bodyIdB = world->bodies[d->body_b];
def.base.localFrameA = local_a;
def.base.localFrameB = local_b;
def.enableConeLimit = d->enable_limit != 0 && d->cone_angle > 0.0f;
def.coneAngle = d->cone_angle;
def.enableTwistLimit = d->enable_limit != 0;
def.lowerTwistAngle = d->lower;
def.upperTwistAngle = d->upper;
joint = b3CreateSphericalJoint(world->world_id, &def);
}
else if (d->kind == YMIR_SOA_JOINT_MOTOR)
{
b3MotorJointDef def = b3DefaultMotorJointDef();
def.base.bodyIdA = world->bodies[d->body_a];
def.base.bodyIdB = world->bodies[d->body_b];
def.base.localFrameA = local_a;
def.base.localFrameB = local_b;
def.linearHertz = d->linear_hertz;
def.linearDampingRatio = d->linear_damping;
def.maxSpringForce = d->max_force;
def.angularHertz = d->angular_hertz;
def.angularDampingRatio = d->angular_damping;
def.maxSpringTorque = d->max_torque;
joint = b3CreateMotorJoint(world->world_id, &def);
}
else
{
b3WeldJointDef def = b3DefaultWeldJointDef();
def.base.bodyIdA = world->bodies[d->body_a];
def.base.bodyIdB = world->bodies[d->body_b];
def.base.localFrameA = local_a;
def.base.localFrameB = local_b;
joint = b3CreateWeldJoint(world->world_id, &def);
}
if (B3_IS_NULL(joint))
{
return YMIR_SOA_INTERNAL_ERROR;
}
int j = slot;
if (j == world->joint_count)
{
world->joint_count++;
}
world->joints[j] = joint;
world->joint_kind[j] = d->kind;
world->joint_body_a[j] = d->body_a;
world->joint_body_b[j] = d->body_b;
world->joint_local_a[j] = local_a.q;
world->joint_local_b[j] = local_b.q;
ymir_soa_refresh_joint(world, j);
if (index != NULL)
{
*index = j;
}
return YMIR_SOA_OK;
}
// Releases a motor joint (a grab) named by its lane and the slot's generation read when it was added. Only motor
// joints: general joint removal is another cut's. A generation that is not the slot's current one is refused, so
// a stale handle never destroys a later joint in a reused slot. The lane becomes a tombstone, kind REMOVED,
// bodies -1 and every lane 0, until a later step lets add_joint reuse it.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_destroy_joint(ymir_box3d_soa_world* world, int32_t joint, uint32_t generation)
{
if (world == NULL || joint < 0 || joint >= world->joint_count || world->joint_kind[joint] != YMIR_SOA_JOINT_MOTOR ||
world->joint_generation[joint] != generation)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
b3DestroyJoint(world->joints[joint], true);
world->joints[joint] = (b3JointId){0};
world->joint_kind[joint] = YMIR_SOA_JOINT_REMOVED;
world->joint_body_a[joint] = -1;
world->joint_body_b[joint] = -1;
world->joint_removed_at[joint] = world->step_count;
world->joint_generation[joint] += 1;
for (int lane = 0; lane < YMIR_SOA_JOINT_FLOAT_LANES; ++lane)
{
ymir_soa_joint_lane(world, lane)[joint] = 0.0f;
}
return YMIR_SOA_OK;
}
// Applies the caller's force and torque lanes to the dynamic bodies, then clears them: a lane holds one
// step's intent, so a stale force is never applied twice. A non-finite value refuses the whole step
// before any body is touched.
static int32_t ymir_soa_apply_write_lanes(ymir_box3d_soa_world* world)
{
float* lane[YMIR_SOA_WRITE_LANES];
for (int k = 0; k < YMIR_SOA_WRITE_LANES; ++k)
{
lane[k] = ymir_soa_lane(world, YMIR_SOA_STATE_LANES + k);
for (int i = 0; i < world->count; ++i)
{
if (!ymir_soa_finite(lane[k][i]))
{
return YMIR_SOA_INVALID_ARGUMENT;
}
}
}
for (int i = 0; i < world->count; ++i)
{
b3Vec3 force = {lane[0][i], lane[1][i], lane[2][i]};
b3Vec3 torque = {lane[3][i], lane[4][i], lane[5][i]};
if (ymir_soa_is_dynamic(world->kind[i]))
{
if (force.x != 0.0f || force.y != 0.0f || force.z != 0.0f)
{
b3Body_ApplyForceToCenter(world->bodies[i], force, true);
}
if (torque.x != 0.0f || torque.y != 0.0f || torque.z != 0.0f)
{
b3Body_ApplyTorque(world->bodies[i], torque, true);
}
}
for (int k = 0; k < YMIR_SOA_WRITE_LANES; ++k)
{
lane[k][i] = 0.0f;
}
}
return YMIR_SOA_OK;
}
// The lane of a shape's body, from the body user data set at add_body (lane + 1).
static int32_t ymir_soa_shape_lane(b3ShapeId shape)
{
return (int32_t)((uintptr_t)b3Body_GetUserData(b3Shape_GetBody(shape))) - 1;
}
static void ymir_soa_touch_fact(
ymir_box3d_soa_world* world, ymir_box3d_soa_touch_fact* out, uint32_t* count, uint32_t* dropped, b3ShapeId a,
b3ShapeId b)
{
if (*count >= world->fact_capacity)
{
*dropped += 1;
return;
}
out[(*count)++] = (ymir_box3d_soa_touch_fact){
ymir_soa_shape_lane(a), ymir_soa_shape_lane(b), (uint64_t)(uintptr_t)b3Shape_GetUserData(a),
(uint64_t)(uintptr_t)b3Shape_GetUserData(b)};
}
static float ymir_soa_inverse_mass(b3BodyId body)
{
return b3Body_GetType(body) == b3_dynamicBody ? 1.0f / b3Body_GetMass(body) : 0.0f;
}
static float ymir_soa_hit_impulse(const ymir_box3d_soa_world* world, const b3ContactHitEvent* e)
{
float inverse = ymir_soa_inverse_mass(b3Shape_GetBody(e->shapeIdA)) + ymir_soa_inverse_mass(b3Shape_GetBody(e->shapeIdB));
if (inverse <= 0.0f)
{
return 0.0f;
}
float restitution = fmaxf(b3Shape_GetRestitution(e->shapeIdA), b3Shape_GetRestitution(e->shapeIdB));
if (e->approachSpeed <= b3World_GetRestitutionThreshold(world->world_id))
{
restitution = 0.0f;
}
return e->approachSpeed * (1.0f + restitution) / inverse;
}
// Rewrites the fact arrays from Box3D's contact events for the step just taken, in Box3D's event order.
// Writes only into the buffers allocated at create: nothing is allocated here.
static void ymir_soa_collect_facts(ymir_box3d_soa_world* world)
{
ymir_box3d_soa_facts* f = &world->facts;
f->begin_count = f->end_count = f->hit_count = 0;
f->begin_dropped = f->end_dropped = f->hit_dropped = 0;
b3ContactEvents events = b3World_GetContactEvents(world->world_id);
for (int i = 0; i < events.beginCount; ++i)
{
const b3ContactBeginTouchEvent* e = events.beginEvents + i;
ymir_soa_touch_fact(world, world->begin_facts, &f->begin_count, &f->begin_dropped, e->shapeIdA, e->shapeIdB);
}
for (int i = 0; i < events.endCount; ++i)
{
const b3ContactEndTouchEvent* e = events.endEvents + i;
ymir_soa_touch_fact(world, world->end_facts, &f->end_count, &f->end_dropped, e->shapeIdA, e->shapeIdB);
}
for (int i = 0; i < events.hitCount; ++i)
{
const b3ContactHitEvent* e = events.hitEvents + i;
if (f->hit_count >= world->fact_capacity)
{
f->hit_dropped += 1;
continue;
}
world->hit_facts[f->hit_count++] = (ymir_box3d_soa_hit_fact){
.lane_a = ymir_soa_shape_lane(e->shapeIdA),
.lane_b = ymir_soa_shape_lane(e->shapeIdB),
.stable_id_a = (uint64_t)(uintptr_t)b3Shape_GetUserData(e->shapeIdA),
.stable_id_b = (uint64_t)(uintptr_t)b3Shape_GetUserData(e->shapeIdB),
.point = {(float)e->point.x, (float)e->point.y, (float)e->point.z},
.normal = {e->normal.x, e->normal.y, e->normal.z},
.approach_speed = e->approachSpeed,
.impulse_estimate = ymir_soa_hit_impulse(world, e),
.material_a = e->userMaterialIdA,
.material_b = e->userMaterialIdB,
};
}
}
// Per body: the touching contacts' count and the force of the final sub-step. normalImpulse is that
// sub-step's impulse, so it is a force of normalImpulse * sub_steps / time_step. Box3D exposes no
// whole-step impulse: totalNormalImpulse adds the accumulated impulse once per solve pass and once per
// relax pass (twice per sub-step, bias and no-bias, with restitution's delta on top), so it is neither the
// step's impulse nor a fixed multiple of it. The lane is honest at rest only.
static void ymir_soa_collect_contact_lanes(ymir_box3d_soa_world* world, float time_step, int32_t sub_step_count)
{
float* force = ymir_soa_lane(world, YMIR_SOA_LAST_SUBSTEP_FORCE_LANE);
for (int i = 0; i < world->count; ++i)
{
int n = b3Body_GetContactData(world->bodies[i], world->contact_scratch, world->capacity);
float impulse = 0.0f;
for (int c = 0; c < n; ++c)
{
const b3ContactData* data = world->contact_scratch + c;
for (int m = 0; m < data->manifoldCount; ++m)
{
const b3Manifold* manifold = data->manifolds + m;
for (int p = 0; p < manifold->pointCount; ++p)
{
impulse += manifold->points[p].normalImpulse;
}
}
}
world->contact_count[i] = n;
force[i] = impulse * (float)sub_step_count / time_step;
}
}
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_step(ymir_box3d_soa_world* world, float time_step, int32_t sub_step_count)
{
if (world == NULL || !ymir_soa_finite(time_step) || time_step <= 0.0f || sub_step_count <= 0)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
int32_t status = ymir_soa_apply_write_lanes(world);
if (status != YMIR_SOA_OK)
{
return status;
}
b3World_Step(world->world_id, time_step, sub_step_count);
// An anchor moves only on a target: the one just applied has been reached, so it holds still until the
// next target. Otherwise its velocity would carry it on past the pose (and fling what it holds).
for (int i = 0; i < world->count; ++i)
{
if (ymir_soa_is_anchor(world->kind[i]))
{
b3Body_SetLinearVelocity(world->bodies[i], b3Vec3_zero);
b3Body_SetAngularVelocity(world->bodies[i], b3Vec3_zero);
}
}
ymir_soa_collect_facts(world);
ymir_soa_collect_contact_lanes(world, time_step, sub_step_count);
for (int i = 0; i < world->count; ++i)
{
ymir_soa_refresh(world, i);
}
for (int j = 0; j < world->joint_count; ++j)
{
if (world->joint_kind[j] != YMIR_SOA_JOINT_REMOVED)
{
ymir_soa_refresh_joint(world, j);
}
}
world->step_count += 1;
return YMIR_SOA_OK;
}
// Grab and poke input, each an immediate command on the caller's say-so. Ymir decides nothing about grabbing.
// Sets an anchor's pose for the coming step: Box3D gives it the velocity that reaches the pose after
// `time_step` seconds, so pass the step's own dt and the anchor arrives at the end of that step, where `step`
// stops it (velocity 0) until the next target.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_set_anchor_target(
ymir_box3d_soa_world* world, int32_t lane, const float* position, const float* rotation, float time_step)
{
if (world == NULL || position == NULL || rotation == NULL || lane < 0 || lane >= world->count ||
!ymir_soa_is_anchor(world->kind[lane]) || !ymir_soa_finite3(position) || !ymir_soa_finite(time_step) ||
time_step <= 0.0f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
float q2 = 0.0f;
for (int i = 0; i < 4; ++i)
{
if (!ymir_soa_finite(rotation[i]))
{
return YMIR_SOA_INVALID_ARGUMENT;
}
q2 += rotation[i] * rotation[i];
}
if (fabsf(q2 - 1.0f) > 1e-3f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
b3WorldTransform target = {
{position[0], position[1], position[2]}, {{rotation[0], rotation[1], rotation[2]}, rotation[3]}};
b3Body_SetTargetTransform(world->bodies[lane], target, time_step, true);
return YMIR_SOA_OK;
}
// A one-shot world impulse (N s) applied at a world point, so it turns the body when the point is off the
// centre of mass. Dynamic bodies only.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_apply_impulse(
ymir_box3d_soa_world* world, int32_t lane, const float* impulse, const float* point)
{
if (world == NULL || impulse == NULL || point == NULL || lane < 0 || lane >= world->count ||
!ymir_soa_is_dynamic(world->kind[lane]) || !ymir_soa_finite3(impulse) || !ymir_soa_finite3(point))
{
return YMIR_SOA_INVALID_ARGUMENT;
}
b3Body_ApplyLinearImpulse(
world->bodies[lane], (b3Vec3){impulse[0], impulse[1], impulse[2]}, (b3Pos){point[0], point[1], point[2]}, true);
return YMIR_SOA_OK;
}
// The closest shape along origin + t * translation, t in [0, 1], with Box3D's default filter. A miss is a
// success with `hit` 0. Refuses a non-finite or zero-length translation.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_cast_ray_closest(
const ymir_box3d_soa_world* world, const float* origin, const float* translation, ymir_box3d_soa_ray_hit* out)
{
if (world == NULL || origin == NULL || translation == NULL || out == NULL || !ymir_soa_finite3(origin) ||
!ymir_soa_finite3(translation) ||
translation[0] * translation[0] + translation[1] * translation[1] + translation[2] * translation[2] <= 0.0f)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
memset(out, 0, sizeof(*out));
b3RayResult r = b3World_CastRayClosest(
world->world_id, (b3Pos){origin[0], origin[1], origin[2]},
(b3Vec3){translation[0], translation[1], translation[2]}, b3DefaultQueryFilter());
if (!r.hit)
{
return YMIR_SOA_OK;
}
out->hit = 1;
out->lane = ymir_soa_shape_lane(r.shapeId);
out->stable_id = (uint64_t)(uintptr_t)b3Shape_GetUserData(r.shapeId);
out->point[0] = (float)r.point.x;
out->point[1] = (float)r.point.y;
out->point[2] = (float)r.point.z;
out->normal[0] = r.normal.x;
out->normal[1] = r.normal.y;
out->normal[2] = r.normal.z;
out->fraction = r.fraction;
return YMIR_SOA_OK;
}
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_get_view(const ymir_box3d_soa_world* world, ymir_box3d_soa_view* view)
{
if (world == NULL || view == NULL)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
float* base = world->lanes;
size_t n = (size_t)world->capacity;
float* jb = world->joint_lanes;
size_t m = (size_t)world->joint_capacity;
*view = (ymir_box3d_soa_view){
.abi_version = YMIR_SOA_ABI_VERSION,
.count = world->count,
.capacity = world->capacity,
.joint_count = world->joint_count,
.joint_capacity = world->joint_capacity,
.step_count = world->step_count,
.stable_id = world->stable_id,
.kind = world->kind,
.parent = world->parent,
.limb = world->limb,
.size_0 = world->size + 0 * n,
.size_1 = world->size + 1 * n,
.size_2 = world->size + 2 * n,
.position_x = base + 0 * n,
.position_y = base + 1 * n,
.position_z = base + 2 * n,
.rotation_x = base + 3 * n,
.rotation_y = base + 4 * n,
.rotation_z = base + 5 * n,
.rotation_w = base + 6 * n,
.velocity_x = base + 7 * n,
.velocity_y = base + 8 * n,
.velocity_z = base + 9 * n,
.angular_velocity_x = base + 10 * n,
.angular_velocity_y = base + 11 * n,
.angular_velocity_z = base + 12 * n,
.force_x = base + 13 * n,
.force_y = base + 14 * n,
.force_z = base + 15 * n,
.torque_x = base + 16 * n,
.torque_y = base + 17 * n,
.torque_z = base + 18 * n,
.joint_kind = world->joint_kind,
.joint_body_a = world->joint_body_a,
.joint_body_b = world->joint_body_b,
.joint_angle = jb + 0 * m,
.joint_frame_x = jb + 1 * m,
.joint_frame_y = jb + 2 * m,
.joint_frame_z = jb + 3 * m,
.joint_frame_w = jb + 4 * m,
.joint_rotation_x = jb + 5 * m,
.joint_rotation_y = jb + 6 * m,
.joint_rotation_z = jb + 7 * m,
.joint_rotation_w = jb + 8 * m,
.joint_angular_velocity_x = jb + 9 * m,
.joint_angular_velocity_y = jb + 10 * m,
.joint_angular_velocity_z = jb + 11 * m,
.last_substep_force = base + YMIR_SOA_LAST_SUBSTEP_FORCE_LANE * n,
.contact_count = world->contact_count,
.material_id = world->material_id,
.joint_generation = world->joint_generation,
};
return YMIR_SOA_OK;
}
// The facts of the last step: pointers into buffers fixed for the world's life, valid until the next step.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_get_facts(const ymir_box3d_soa_world* world, ymir_box3d_soa_facts* facts)
{
if (world == NULL || facts == NULL)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
*facts = world->facts;
facts->fact_capacity = world->fact_capacity;
facts->begin = world->begin_facts;
facts->end = world->end_facts;
facts->hit = world->hit_facts;
return YMIR_SOA_OK;
}