SoA world: kinematic anchors, motor joint spring, point impulses, closest-hit ray (ABI 5)
Anchor body kind (kinematic, no shape) with set_anchor_target; Motor joint kind with position springs bounded by max force and torque, taking the slot of the joint desc's reserved float; apply_impulse at a world point; cast_ray_closest. Ymir executes the commands and decides nothing about grabbing. Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
This commit is contained in:
parent
e4cf4cd444
commit
7efcefc8a0
3 changed files with 528 additions and 47 deletions
|
|
@ -44,21 +44,26 @@ 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_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_WELD = 2,
|
||||
YMIR_SOA_JOINT_MOTOR = 3
|
||||
};
|
||||
|
||||
#define YMIR_SOA_ABI_VERSION 4u
|
||||
#define YMIR_SOA_ABI_VERSION 5u
|
||||
|
||||
// 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;
|
||||
|
|
@ -81,6 +86,10 @@ typedef struct ymir_box3d_soa_body_desc
|
|||
// 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.
|
||||
typedef struct ymir_box3d_soa_joint_desc
|
||||
{
|
||||
uint32_t kind;
|
||||
|
|
@ -92,7 +101,12 @@ typedef struct ymir_box3d_soa_joint_desc
|
|||
float lower;
|
||||
float upper;
|
||||
float cone_angle;
|
||||
float reserved_float;
|
||||
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
|
||||
|
|
@ -211,6 +225,19 @@ typedef struct ymir_box3d_soa_facts
|
|||
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)
|
||||
|
|
@ -269,9 +296,14 @@ 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;
|
||||
return kind != YMIR_SOA_KIND_STATIC_BOX && !ymir_soa_is_anchor(kind);
|
||||
}
|
||||
|
||||
static void ymir_soa_refresh(ymir_box3d_soa_world* world, int index)
|
||||
|
|
@ -342,6 +374,11 @@ 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);
|
||||
|
|
@ -470,7 +507,7 @@ YMIR_EXPORT void YMIR_CALL ymir_box3d_soa_destroy(ymir_box3d_soa_world* 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) ||
|
||||
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)
|
||||
|
|
@ -500,6 +537,8 @@ static int32_t ymir_soa_check_body(const ymir_box3d_soa_world* world, const ymir
|
|||
}
|
||||
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)
|
||||
{
|
||||
|
|
@ -548,7 +587,9 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_body(
|
|||
}
|
||||
|
||||
b3BodyDef body_def = b3DefaultBodyDef();
|
||||
body_def.type = ymir_soa_is_dynamic(d->kind) ? b3_dynamicBody : b3_staticBody;
|
||||
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]};
|
||||
|
|
@ -559,39 +600,42 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_body(
|
|||
{
|
||||
return YMIR_SOA_INTERNAL_ERROR;
|
||||
}
|
||||
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))
|
||||
if (!ymir_soa_is_anchor(d->kind)) // an anchor has no shape
|
||||
{
|
||||
shape_def.density = d->density;
|
||||
}
|
||||
shape_def.baseMaterial.friction = d->friction;
|
||||
shape_def.baseMaterial.restitution = d->restitution;
|
||||
shape_def.baseMaterial.rollingResistance = d->rolling_resistance;
|
||||
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;
|
||||
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++;
|
||||
|
|
@ -600,9 +644,9 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_body(
|
|||
world->kind[i] = d->kind;
|
||||
world->parent[i] = d->parent;
|
||||
world->limb[i] = d->limb;
|
||||
world->material_id[i] = d->material_id;
|
||||
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 = d->kind == YMIR_SOA_KIND_SPHERE ? 1 : d->kind == YMIR_SOA_KIND_CAPSULE ? 2 : 3;
|
||||
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;
|
||||
|
|
@ -627,10 +671,14 @@ static b3Transform ymir_soa_body_transform(b3BodyId body)
|
|||
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 ||
|
||||
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->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;
|
||||
}
|
||||
|
|
@ -677,6 +725,21 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_joint(
|
|||
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();
|
||||
|
|
@ -882,6 +945,85 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_step(ymir_box3d_soa_world* world, f
|
|||
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.
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -7,14 +7,18 @@ public enum Box3DSoaKind : uint
|
|||
Sphere = 0,
|
||||
StaticBox = 1,
|
||||
Capsule = 2,
|
||||
Box = 3
|
||||
Box = 3,
|
||||
/// <summary>A kinematic body with no shape: it never collides or is hit, and only <see cref="Box3DSoaWorld.SetAnchorTarget"/> moves it.</summary>
|
||||
Anchor = 4
|
||||
}
|
||||
|
||||
public enum Box3DSoaJointKind : uint
|
||||
{
|
||||
Revolute = 0,
|
||||
Spherical = 1,
|
||||
Weld = 2
|
||||
Weld = 2,
|
||||
/// <summary>Position springs pulling body B's frame onto body A's, bounded by a maximum force and torque.</summary>
|
||||
Motor = 3
|
||||
}
|
||||
|
||||
/// <summary>Native layout of <c>ymir_box3d_soa_body_desc</c>.</summary>
|
||||
|
|
@ -44,9 +48,27 @@ internal struct Box3DSoaJointDesc
|
|||
public uint EnableLimit;
|
||||
public float AnchorX, AnchorY, AnchorZ;
|
||||
public float AxisX, AxisY, AxisZ;
|
||||
public float Lower, Upper, ConeAngle, ReservedFloat;
|
||||
public float Lower, Upper, ConeAngle;
|
||||
public float LinearHertz, LinearDamping, MaxForce, AngularHertz, AngularDamping, MaxTorque;
|
||||
}
|
||||
|
||||
/// <summary>Native layout of <c>ymir_box3d_soa_ray_hit</c>.</summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
internal struct Box3DSoaRayHitNative
|
||||
{
|
||||
public int Hit;
|
||||
public int Lane;
|
||||
public ulong StableId;
|
||||
public float PointX, PointY, PointZ;
|
||||
public float NormalX, NormalY, NormalZ;
|
||||
public float Fraction;
|
||||
public uint Reserved;
|
||||
}
|
||||
|
||||
/// <summary>The closest hit of a ray. <see cref="Hit"/> false is a miss and every other field is then default.</summary>
|
||||
public readonly record struct Box3DSoaRayHit(
|
||||
bool Hit, int Lane, ulong StableId, (float X, float Y, float Z) Point, (float X, float Y, float Z) Normal, float Fraction);
|
||||
|
||||
/// <summary>Native layout of <c>ymir_box3d_soa_view</c>: counts, then one pointer per lane.</summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
internal unsafe struct Box3DSoaViewNative
|
||||
|
|
@ -209,6 +231,10 @@ internal static unsafe partial class Box3DSoaNative
|
|||
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
|
||||
internal static partial uint ymir_box3d_soa_get_touch_fact_size();
|
||||
|
||||
[LibraryImport(Library)]
|
||||
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
|
||||
internal static partial uint ymir_box3d_soa_get_ray_hit_size();
|
||||
|
||||
[LibraryImport(Library)]
|
||||
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
|
||||
internal static partial uint ymir_box3d_soa_get_hit_fact_size();
|
||||
|
|
@ -240,6 +266,18 @@ internal static unsafe partial class Box3DSoaNative
|
|||
[LibraryImport(Library)]
|
||||
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
|
||||
internal static partial int ymir_box3d_soa_get_facts(nint world, out Box3DSoaFactsNative facts);
|
||||
|
||||
[LibraryImport(Library)]
|
||||
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
|
||||
internal static partial int ymir_box3d_soa_set_anchor_target(nint world, int lane, float* position, float* rotation, float timeStep);
|
||||
|
||||
[LibraryImport(Library)]
|
||||
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
|
||||
internal static partial int ymir_box3d_soa_apply_impulse(nint world, int lane, float* impulse, float* point);
|
||||
|
||||
[LibraryImport(Library)]
|
||||
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
|
||||
internal static partial int ymir_box3d_soa_cast_ray_closest(nint world, float* origin, float* translation, out Box3DSoaRayHitNative hit);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
|
@ -355,7 +393,7 @@ public readonly unsafe struct Box3DSoaView
|
|||
/// </summary>
|
||||
public sealed class Box3DSoaWorld : IDisposable
|
||||
{
|
||||
private const uint ExpectedAbi = 4;
|
||||
private const uint ExpectedAbi = 5;
|
||||
private nint _handle;
|
||||
|
||||
/// <summary>
|
||||
|
|
@ -372,7 +410,8 @@ public sealed class Box3DSoaWorld : IDisposable
|
|||
Box3DSoaNative.ymir_box3d_soa_get_joint_desc_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf<Box3DSoaJointDesc>() ||
|
||||
Box3DSoaNative.ymir_box3d_soa_get_facts_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf<Box3DSoaFactsNative>() ||
|
||||
Box3DSoaNative.ymir_box3d_soa_get_touch_fact_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf<Box3DSoaTouch>() ||
|
||||
Box3DSoaNative.ymir_box3d_soa_get_hit_fact_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf<Box3DSoaHit>())
|
||||
Box3DSoaNative.ymir_box3d_soa_get_hit_fact_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf<Box3DSoaHit>() ||
|
||||
Box3DSoaNative.ymir_box3d_soa_get_ray_hit_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf<Box3DSoaRayHitNative>())
|
||||
{
|
||||
throw new InvalidOperationException("The native Ymir Box3D SoA ABI does not match this build.");
|
||||
}
|
||||
|
|
@ -469,6 +508,73 @@ public sealed class Box3DSoaWorld : IDisposable
|
|||
return index;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// A motor joint: position springs that pull body B's frame onto body A's. The frame sits at the world-space
|
||||
/// <paramref name="anchor"/> at creation (axes aligned with the world), so the spring pulls B's anchor point onto A's.
|
||||
/// <c>Hertz</c> 0 turns that spring off; <c>MaxForce</c> (N) and <c>MaxTorque</c> (N m) bound it. All are finite and
|
||||
/// not negative. Pair an anchor (body A) with the body to grab (body B). Returns the joint's lane index.
|
||||
/// </summary>
|
||||
public int AddMotorJoint(
|
||||
int bodyA, int bodyB, (float X, float Y, float Z) anchor,
|
||||
(float Hertz, float Damping, float MaxForce) linear, (float Hertz, float Damping, float MaxTorque) angular)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_handle == 0, this);
|
||||
var desc = new Box3DSoaJointDesc
|
||||
{
|
||||
Kind = (uint)Box3DSoaJointKind.Motor, BodyA = bodyA, BodyB = bodyB,
|
||||
AnchorX = anchor.X, AnchorY = anchor.Y, AnchorZ = anchor.Z, AxisZ = 1f,
|
||||
LinearHertz = linear.Hertz, LinearDamping = linear.Damping, MaxForce = linear.MaxForce,
|
||||
AngularHertz = angular.Hertz, AngularDamping = angular.Damping, MaxTorque = angular.MaxTorque,
|
||||
};
|
||||
Check(Box3DSoaNative.ymir_box3d_soa_add_joint(_handle, desc, out var index));
|
||||
return index;
|
||||
}
|
||||
|
||||
/// <summary>Adds a kinematic anchor with no shape (rotation as for <see cref="AddCapsule"/>); returns its lane index.</summary>
|
||||
public int AddAnchor(ulong stableId, (float X, float Y, float Z) position, (float X, float Y, float Z, float W) rotation = default) =>
|
||||
AddBody(Describe(Box3DSoaKind.Anchor, stableId, position, rotation, default, 0f, default, 0f, 0f, -1, 0));
|
||||
|
||||
/// <summary>
|
||||
/// Sets an anchor's pose for the coming step. Box3D gives the anchor the velocity that reaches it after
|
||||
/// <paramref name="timeStep"/>, so pass the same dt as the next <see cref="Step"/> and the anchor arrives at its end.
|
||||
/// Throws for a lane that is not an anchor, a non-finite or non-unit input, or a non-positive time step.
|
||||
/// </summary>
|
||||
public unsafe void SetAnchorTarget(
|
||||
int lane, (float X, float Y, float Z) position, (float X, float Y, float Z, float W) rotation, float timeStep)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_handle == 0, this);
|
||||
if (rotation == default) rotation = (0f, 0f, 0f, 1f);
|
||||
var p = stackalloc float[] { position.X, position.Y, position.Z };
|
||||
var q = stackalloc float[] { rotation.X, rotation.Y, rotation.Z, rotation.W };
|
||||
Check(Box3DSoaNative.ymir_box3d_soa_set_anchor_target(_handle, lane, p, q, timeStep));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// A one-shot world impulse (N s) at a world <paramref name="point"/>: it adds <c>impulse / mass</c> to the linear
|
||||
/// velocity and turns the body by <c>(point - centre of mass) x impulse</c>. Dynamic bodies only; throws otherwise
|
||||
/// or for non-finite input.
|
||||
/// </summary>
|
||||
public unsafe void ApplyImpulse(int lane, (float X, float Y, float Z) impulse, (float X, float Y, float Z) point)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_handle == 0, this);
|
||||
var j = stackalloc float[] { impulse.X, impulse.Y, impulse.Z };
|
||||
var p = stackalloc float[] { point.X, point.Y, point.Z };
|
||||
Check(Box3DSoaNative.ymir_box3d_soa_apply_impulse(_handle, lane, j, p));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The closest shape along <paramref name="origin"/> + t * <paramref name="translation"/>, t in [0, 1], with Box3D's
|
||||
/// default filter; anchors have no shape and are never hit. Throws for non-finite input or a zero translation.
|
||||
/// </summary>
|
||||
public unsafe Box3DSoaRayHit CastRayClosest((float X, float Y, float Z) origin, (float X, float Y, float Z) translation)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_handle == 0, this);
|
||||
var o = stackalloc float[] { origin.X, origin.Y, origin.Z };
|
||||
var t = stackalloc float[] { translation.X, translation.Y, translation.Z };
|
||||
Check(Box3DSoaNative.ymir_box3d_soa_cast_ray_closest(_handle, o, t, out var h));
|
||||
return new Box3DSoaRayHit(h.Hit != 0, h.Lane, h.StableId, (h.PointX, h.PointY, h.PointZ), (h.NormalX, h.NormalY, h.NormalZ), h.Fraction);
|
||||
}
|
||||
|
||||
private static Box3DSoaBodyDesc Describe(
|
||||
Box3DSoaKind kind, ulong stableId, (float X, float Y, float Z) position,
|
||||
(float X, float Y, float Z, float W) rotation, (float, float, float) size, float density,
|
||||
|
|
|
|||
233
tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs
Normal file
233
tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs
Normal file
|
|
@ -0,0 +1,233 @@
|
|||
using System.Runtime.InteropServices;
|
||||
using Ymir.Box3D;
|
||||
|
||||
namespace Ymir.Box3D.Soa.Tests;
|
||||
|
||||
/// <summary>Kinematic anchors, the motor joint, point impulses and the closest-hit ray: the input plumbing for grab and poke.</summary>
|
||||
public sealed class Box3DSoaGrabTests
|
||||
{
|
||||
private const float Dt = 1f / 60f;
|
||||
private const float Gravity = 10f;
|
||||
private const float Radius = 0.2f;
|
||||
private const float Density = 500f;
|
||||
private static readonly float Mass = Density * 4f / 3f * MathF.PI * Radius * Radius * Radius;
|
||||
|
||||
private static float Distance(Box3DSoaView v, int lane, (float X, float Y, float Z) p)
|
||||
{
|
||||
var dx = v.PositionX[lane] - p.X; var dy = v.PositionY[lane] - p.Y; var dz = v.PositionZ[lane] - p.Z;
|
||||
return MathF.Sqrt(dx * dx + dy * dy + dz * dz);
|
||||
}
|
||||
|
||||
/// <summary>A sphere hung from an anchor at the same point by a motor joint with the given linear spring.</summary>
|
||||
private static (Box3DSoaWorld World, int Anchor, int Sphere, int Joint) Grab(float maxForce, float hertz = 5f)
|
||||
{
|
||||
var world = new Box3DSoaWorld(capacity: 4, gravityY: -Gravity, jointCapacity: 2);
|
||||
var sphere = world.AddSphere(1, (0f, 5f, 0f), Radius, Density);
|
||||
var anchor = world.AddAnchor(2, (0f, 5f, 0f));
|
||||
var joint = world.AddMotorJoint(anchor, sphere, (0f, 5f, 0f), (hertz, 0.7f, maxForce), (0f, 0f, 0f));
|
||||
return (world, anchor, sphere, joint);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void A_strong_grab_follows_its_target_and_the_anchor_shows_its_pose()
|
||||
{
|
||||
var (world, anchor, sphere, joint) = Grab(maxForce: 20f * Mass * Gravity);
|
||||
using var _ = world;
|
||||
var view = world.View;
|
||||
Assert.Equal((uint)Box3DSoaKind.Anchor, view.Kind[anchor]);
|
||||
Assert.Equal((uint)Box3DSoaJointKind.Motor, view.JointKind[joint]);
|
||||
var target = (1f, 5f, 0f);
|
||||
for (var i = 0; i < 60; i++)
|
||||
{
|
||||
world.SetAnchorTarget(anchor, target, default, Dt);
|
||||
world.Step(Dt);
|
||||
}
|
||||
Assert.True(Distance(view, sphere, target) < 0.02f, $"sphere {Distance(view, sphere, target)} m from the target");
|
||||
Assert.Equal(target.Item1, view.PositionX[anchor], 1e-3f); // the anchor reaches the pose at the end of the step
|
||||
Assert.Equal(0f, view.JointAngle[joint]);
|
||||
Assert.Equal(0f, view.Size0[anchor]); // no shape, so no size
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void A_weak_grab_is_bounded_by_its_max_force()
|
||||
{
|
||||
var maxForce = 0.5f * Mass * Gravity;
|
||||
var (world, anchor, sphere, _) = Grab(maxForce);
|
||||
using var _ = world;
|
||||
var view = world.View;
|
||||
var target = (0f, 6f, 0f);
|
||||
for (var i = 0; i < 60; i++)
|
||||
{
|
||||
world.SetAnchorTarget(anchor, target, default, Dt);
|
||||
world.Step(Dt);
|
||||
}
|
||||
// Whatever the spring does, it adds at most maxForce / m upward: the fall is at least that of a body
|
||||
// under g - F/m = 5 m/s^2, 2.5 m in a second. A spring that exceeded its bound would hold it higher.
|
||||
Assert.True(5f - view.PositionY[sphere] >= 2.4f, $"fell only {5f - view.PositionY[sphere]} m");
|
||||
Assert.True(view.PositionY[sphere] < target.Item2 - 1f, "the sphere hangs below the target and never reaches it");
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void Anchors_do_not_collide_and_are_never_hit_by_a_ray()
|
||||
{
|
||||
static float DropY(bool withAnchor)
|
||||
{
|
||||
using var world = new Box3DSoaWorld(capacity: 3, gravityY: -Gravity);
|
||||
var sphere = world.AddSphere(1, (0f, 3f, 0f), Radius, Density);
|
||||
if (withAnchor) world.AddAnchor(2, (0f, 1.5f, 0f));
|
||||
var view = world.View;
|
||||
for (var i = 0; i < 60; i++) world.Step(Dt);
|
||||
return view.PositionY[sphere];
|
||||
}
|
||||
|
||||
Assert.True(DropY(false) < 1f, "the sphere fell past y = 1.5");
|
||||
Assert.Equal(BitConverter.SingleToInt32Bits(DropY(false)), BitConverter.SingleToInt32Bits(DropY(true)));
|
||||
|
||||
using var w = new Box3DSoaWorld(capacity: 2);
|
||||
w.AddAnchor(2, (0f, 1.5f, 0f));
|
||||
Assert.False(w.CastRayClosest((0f, 3f, 0f), (0f, -3f, 0f)).Hit);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void The_force_lanes_never_move_an_anchor()
|
||||
{
|
||||
using var world = new Box3DSoaWorld(capacity: 2, gravityY: -Gravity);
|
||||
var anchor = world.AddAnchor(1, (1f, 2f, 3f));
|
||||
var view = world.View;
|
||||
for (var i = 0; i < 30; i++)
|
||||
{
|
||||
view.ForceY[anchor] = 1e6f;
|
||||
view.TorqueZ[anchor] = 1e6f;
|
||||
world.Step(Dt);
|
||||
}
|
||||
Assert.Equal((1f, 2f, 3f), (view.PositionX[anchor], view.PositionY[anchor], view.PositionZ[anchor]));
|
||||
Assert.Equal(1f, view.RotationW[anchor]);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void An_off_centre_impulse_turns_the_body()
|
||||
{
|
||||
const float r = 0.25f, density = 100f;
|
||||
var mass = density * 4f / 3f * MathF.PI * r * r * r;
|
||||
using var world = new Box3DSoaWorld(capacity: 2, gravityY: 0f);
|
||||
var sphere = world.AddSphere(1, (0f, 0f, 0f), r, density);
|
||||
var centred = world.AddSphere(2, (5f, 0f, 0f), r, density);
|
||||
var view = world.View;
|
||||
const float j = 2f;
|
||||
world.ApplyImpulse(sphere, (j, 0f, 0f), (0f, 0.1f, 0f));
|
||||
world.ApplyImpulse(centred, (j, 0f, 0f), (5f, 0f, 0f));
|
||||
world.Step(Dt);
|
||||
Assert.Equal(j / mass, view.VelocityX[sphere], 1e-4f);
|
||||
Assert.Equal(0f, view.VelocityY[sphere], 1e-4f);
|
||||
// r x J = (0, 0.1, 0) x (j, 0, 0) = (0, 0, -0.1 j): the spin is about -z, I = 2/5 m r^2.
|
||||
Assert.True(view.AngularVelocityZ[sphere] < 0f);
|
||||
Assert.Equal(-0.1f * j / (0.4f * mass * r * r), view.AngularVelocityZ[sphere], 0.02f);
|
||||
Assert.Equal(0f, view.AngularVelocityX[sphere], 1e-4f);
|
||||
// The same impulse through the centre of mass gives no spin.
|
||||
Assert.Equal(j / mass, view.VelocityX[centred], 1e-4f);
|
||||
Assert.Equal(0f, view.AngularVelocityZ[centred], 1e-4f);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void The_ray_finds_the_nearest_body_with_its_lane_and_stable_id()
|
||||
{
|
||||
using var world = new Box3DSoaWorld(capacity: 4, gravityY: 0f);
|
||||
var far = world.AddSphere(12, (6f, 0f, 0f), 0.5f, 1f);
|
||||
var near = world.AddSphere(11, (3f, 0f, 0f), 0.5f, 1f);
|
||||
var hit = world.CastRayClosest((0f, 0f, 0f), (10f, 0f, 0f));
|
||||
Assert.True(hit.Hit);
|
||||
Assert.Equal(near, hit.Lane);
|
||||
Assert.NotEqual(far, hit.Lane);
|
||||
Assert.Equal(11ul, hit.StableId);
|
||||
Assert.Equal(2.5f, hit.Point.X, 1e-3f);
|
||||
Assert.Equal(0.25f, hit.Fraction, 1e-4f);
|
||||
Assert.Equal(-1f, hit.Normal.X, 1e-3f);
|
||||
Assert.False(world.CastRayClosest((0f, 0f, 0f), (0f, 10f, 0f)).Hit);
|
||||
Assert.Equal(default, world.CastRayClosest((0f, 0f, 0f), (0f, 10f, 0f)));
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void Refused_calls_throw_and_change_nothing()
|
||||
{
|
||||
static Box3DSoaWorld Build(out int anchor, out int sphere, out int box)
|
||||
{
|
||||
var w = new Box3DSoaWorld(capacity: 5, gravityY: -Gravity, jointCapacity: 2);
|
||||
sphere = w.AddSphere(1, (0f, 2f, 0f), Radius, Density);
|
||||
box = w.AddStaticBox(2, (0f, -1f, 0f), (5f, 1f, 5f));
|
||||
anchor = w.AddAnchor(3, (0f, 2f, 0f));
|
||||
return w;
|
||||
}
|
||||
|
||||
using var world = Build(out var anchor, out var sphere, out var box);
|
||||
using var control = Build(out _, out _, out _);
|
||||
var nan = float.NaN;
|
||||
Assert.Throws<InvalidOperationException>(() => world.SetAnchorTarget(sphere, (1f, 1f, 1f), default, Dt));
|
||||
Assert.Throws<InvalidOperationException>(() => world.SetAnchorTarget(box, (1f, 1f, 1f), default, Dt));
|
||||
Assert.Throws<InvalidOperationException>(() => world.SetAnchorTarget(9, (1f, 1f, 1f), default, Dt));
|
||||
Assert.Throws<InvalidOperationException>(() => world.SetAnchorTarget(anchor, (nan, 1f, 1f), default, Dt));
|
||||
Assert.Throws<InvalidOperationException>(() => world.SetAnchorTarget(anchor, (1f, 1f, 1f), (0f, 0f, nan, 1f), Dt));
|
||||
Assert.Throws<InvalidOperationException>(() => world.SetAnchorTarget(anchor, (1f, 1f, 1f), (0f, 0f, 0f, 2f), Dt));
|
||||
Assert.Throws<InvalidOperationException>(() => world.SetAnchorTarget(anchor, (1f, 1f, 1f), default, 0f));
|
||||
Assert.Throws<InvalidOperationException>(() => world.ApplyImpulse(box, (1f, 0f, 0f), (0f, 0f, 0f)));
|
||||
Assert.Throws<InvalidOperationException>(() => world.ApplyImpulse(anchor, (1f, 0f, 0f), (0f, 2f, 0f)));
|
||||
Assert.Throws<InvalidOperationException>(() => world.ApplyImpulse(sphere, (nan, 0f, 0f), (0f, 2f, 0f)));
|
||||
Assert.Throws<InvalidOperationException>(() => world.ApplyImpulse(sphere, (1f, 0f, 0f), (0f, float.PositiveInfinity, 0f)));
|
||||
Assert.Throws<InvalidOperationException>(() => world.CastRayClosest((nan, 0f, 0f), (1f, 0f, 0f)));
|
||||
Assert.Throws<InvalidOperationException>(() => world.CastRayClosest((0f, 0f, 0f), (0f, 0f, 0f)));
|
||||
foreach (var bad in new[] { -1f, nan, float.PositiveInfinity })
|
||||
{
|
||||
Assert.Throws<InvalidOperationException>(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (bad, 0.7f, 100f), (0f, 0f, 0f)));
|
||||
Assert.Throws<InvalidOperationException>(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (5f, bad, 100f), (0f, 0f, 0f)));
|
||||
Assert.Throws<InvalidOperationException>(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (5f, 0.7f, bad), (0f, 0f, 0f)));
|
||||
Assert.Throws<InvalidOperationException>(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (5f, 0.7f, 100f), (bad, 0f, 0f)));
|
||||
Assert.Throws<InvalidOperationException>(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (5f, 0.7f, 100f), (1f, bad, 0f)));
|
||||
Assert.Throws<InvalidOperationException>(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (5f, 0.7f, 100f), (1f, 0f, bad)));
|
||||
}
|
||||
Assert.Equal(0, world.View.JointCount);
|
||||
|
||||
for (var i = 0; i < 30; i++) { world.Step(Dt); control.Step(Dt); }
|
||||
Assert.Equal(Snapshot(control), Snapshot(world));
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void Two_identical_worlds_with_the_same_grab_and_impulses_match_bit_for_bit()
|
||||
{
|
||||
var a = Snapshot(RunGrab(0f));
|
||||
Assert.Equal(a, Snapshot(RunGrab(0f)));
|
||||
Assert.NotEqual(a, Snapshot(RunGrab(MathF.BitIncrement(1f) - 1f))); // one ulp of impulse is seen
|
||||
}
|
||||
|
||||
private static Box3DSoaWorld RunGrab(float impulseNudge)
|
||||
{
|
||||
var world = new Box3DSoaWorld(capacity: 5, gravityY: -Gravity, jointCapacity: 2);
|
||||
var slab = world.AddStaticBox(1, (0f, -1f, 0f), (10f, 1f, 10f));
|
||||
var ball = world.AddSphere(2, (0f, 1f, 0f), Radius, Density);
|
||||
var other = world.AddSphere(3, (1.5f, 0.2f, 0f), Radius, Density);
|
||||
var anchor = world.AddAnchor(4, (0f, 1f, 0f));
|
||||
world.AddMotorJoint(anchor, ball, (0f, 1f, 0f), (4f, 0.5f, 8f * Mass * Gravity), (2f, 0.5f, 5f));
|
||||
for (var i = 0; i < 300; i++)
|
||||
{
|
||||
var t = i * Dt;
|
||||
world.SetAnchorTarget(anchor, (MathF.Sin(t) * 1.5f, 1f + 0.5f * MathF.Cos(2f * t), 0.3f * t), default, Dt);
|
||||
if (i == 100) world.ApplyImpulse(other, (0.5f + impulseNudge, 0.2f, 0f), (1.5f, 0.3f, 0f));
|
||||
world.Step(Dt);
|
||||
}
|
||||
_ = slab;
|
||||
return world;
|
||||
}
|
||||
|
||||
/// <summary>Every state lane of the world as bytes, disposing the world.</summary>
|
||||
private static byte[] Snapshot(Box3DSoaWorld world)
|
||||
{
|
||||
using var _ = world;
|
||||
var v = world.View;
|
||||
var bytes = new List<byte>();
|
||||
void Add(ReadOnlySpan<float> lane) => bytes.AddRange(MemoryMarshal.AsBytes(lane).ToArray());
|
||||
Add(v.PositionX); Add(v.PositionY); Add(v.PositionZ);
|
||||
Add(v.RotationX); Add(v.RotationY); Add(v.RotationZ); Add(v.RotationW);
|
||||
Add(v.VelocityX); Add(v.VelocityY); Add(v.VelocityZ);
|
||||
Add(v.AngularVelocityX); Add(v.AngularVelocityY); Add(v.AngularVelocityZ);
|
||||
for (var j = 0; j < v.JointCount; j++) bytes.AddRange(BitConverter.GetBytes(v.JointRotationW[j]));
|
||||
return [.. bytes];
|
||||
}
|
||||
}
|
||||
Loading…
Reference in a new issue