Ymir/native/Ymir.Box3D/ymir_box3d_soa.c
Nibu b96ad34b0b SoA world: rolling resistance on the body descriptor
A bare sphere or capsule rolls on without it, so a fallen head never settles.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
2026-11-07 04:07:38 +00:00

709 lines
24 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 <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
};
enum
{
YMIR_SOA_JOINT_REVOLUTE = 0,
YMIR_SOA_JOINT_SPHERICAL = 1,
YMIR_SOA_JOINT_WELD = 2
};
#define YMIR_SOA_ABI_VERSION 2u
// 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.
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
} 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.
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 reserved_float;
} 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;
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;
} ymir_box3d_soa_view;
#define YMIR_SOA_STATE_LANES 13
#define YMIR_SOA_WRITE_LANES 6
#define YMIR_SOA_BODY_FLOAT_LANES (YMIR_SOA_STATE_LANES + YMIR_SOA_WRITE_LANES)
#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;
float* lanes; // YMIR_SOA_BODY_FLOAT_LANES arrays of `capacity` floats, lane-major
b3JointId* joints;
uint32_t* joint_kind;
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
} 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_dynamic(uint32_t kind)
{
return kind != YMIR_SOA_KIND_STATIC_BOX;
}
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);
}
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->lanes);
free(world->joints);
free(world->joint_kind);
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);
}
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,
ymir_box3d_soa_world** output)
{
if (output == NULL)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
*output = NULL;
if (capacity <= 0 || joint_capacity < 0 || !ymir_soa_finite(gravity_x) || !ymir_soa_finite(gravity_y) ||
!ymir_soa_finite(gravity_z))
{
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->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_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));
if (world->bodies == NULL || world->stable_id == NULL || world->kind == NULL || world->parent == NULL ||
world->limb == 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)
{
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.workerCount = 1; // one worker: the step order is fixed, which determinism rests on
world->world_id = b3CreateWorld(&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))
{
b3DestroyWorld(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_BOX || !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_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_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;
b3BodyId body = b3CreateBody(world->world_id, &body_def);
if (B3_IS_NULL(body))
{
return YMIR_SOA_INTERNAL_ERROR;
}
b3ShapeDef shape_def = b3DefaultShapeDef();
shape_def.userData = (void*)(uintptr_t)d->stable_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;
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;
}
// 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_WELD || 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)
{
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;
}
if (world->joint_count >= world->joint_capacity)
{
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
{
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 = 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;
}
// 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;
}
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);
for (int i = 0; i < world->count; ++i)
{
ymir_soa_refresh(world, i);
}
for (int j = 0; j < world->joint_count; ++j)
{
ymir_soa_refresh_joint(world, j);
}
world->step_count += 1;
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,
.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,
};
return YMIR_SOA_OK;
}