diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index c3d5d8b..86d6277 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -7,13 +7,11 @@ on: - main pull_request: -# Pin: 0.841.0, the latest release, and aeb v0.333. The move from 0.839.0 -# is evidence-based, not a tracking bump: 0.840.0 and 0.841.0 add the -# ownership rules (0.840: a borrowed ptr put into a list or map is -# E0902 without @keep) and E0202 / E0305, and every module, test, -# benchmark and example checks clean on them with no source change, the -# golden trace bit for bit. aeb v0.333 is the one whose @keep session -# handle the family moved to together. The two functions that take over a +# Pin: 0.919.0, the latest release, and aeb v0.347, the one ae3d builds +# beside. Every module, test, benchmark and example checks clean on +# 0.919.0 with no source change from 0.841.0, the golden trace bit for +# bit. 0.840.0 and 0.841.0 added the ownership rules (a borrowed ptr put +# into a list or map is E0902 without @keep) and E0202 / E0305. The two functions that take over a # struct's buffers declare it `@keep`, which 0.838.0 checks; the # accessors that hand back a pointer into their argument declare it # `@into` (aether#2990), which 0.836.0 checks; the lane moves @@ -22,8 +20,8 @@ on: # bootstrap version: the floor this code needs, verified by every job # below. env: - AE_PIN: 0.841.0 - AEB_REF: v0.333 + AE_PIN: 0.919.0 + AEB_REF: v0.347 jobs: test: diff --git a/README.md b/README.md index e6e9026..d7f4dd7 100644 --- a/README.md +++ b/README.md @@ -54,7 +54,7 @@ Install Aether with the version-pinned installer (it puts `ae` and `aeb` in ```sh curl -fsSL https://raw.githubusercontent.com/aether-lang-dev/aeb/main/get.sh \ - | AE_PIN=0.841.0 AEB_REF=v0.333 sh + | AE_PIN=0.919.0 AEB_REF=v0.347 sh export PATH="$HOME/.local/bin:$PATH" ``` @@ -180,7 +180,7 @@ scripts/profile.sh file.ae # a sampling profile on Windows ``` CI runs the suite on Linux, on Linux with AVX2 and FMA, on macOS arm64, and -on Windows at `-O3` and `-O0`. It pins one Aether release, 0.841.0; older +on Windows at `-O3` and `-O0`. It pins one Aether release, 0.919.0; older ones are not tested. ## Credits diff --git a/aephysics/dynamics/module.ae b/aephysics/dynamics/module.ae index 7d7e610..626be12 100644 --- a/aephysics/dynamics/module.ae +++ b/aephysics/dynamics/module.ae @@ -94,7 +94,7 @@ exports ( intern_tag, clear_tags, intern_geometry, rec_commit_query, rec_aabb, rec_query_filter, rec_shape_proxy, rec_plane_result, rec_capsule_value, rec_float, rec_bool, rec_vec3, rec_quat, rec_transform, rec_id, rec_string, rec_raw, rec_geometry, finish_recording, recorded_geometry_at, OP_STATE_HASH, OP_BOUNDS, OP_END, OP_QUERY_TAG, OP_QUERY_OVERLAP_AABB, OP_QUERY_OVERLAP_SHAPE, OP_QUERY_CAST_RAY, - OP_QUERY_CAST_SHAPE, OP_QUERY_CAST_RAY_CLOSEST, OP_QUERY_CAST_MOVER, OP_QUERY_COLLIDE_MOVER, OP_CREATE_BODY, OP_DESTROY_BODY, OP_BODY_SET_TRANSFORM, OP_BODY_SET_LINEAR_VELOCITY, OP_BODY_SET_ANGULAR_VELOCITY, OP_BODY_SET_TARGET_TRANSFORM, OP_BODY_APPLY_FORCE, OP_BODY_APPLY_FORCE_TO_CENTER, OP_BODY_APPLY_TORQUE, OP_BODY_APPLY_LINEAR_IMPULSE, OP_BODY_APPLY_LINEAR_IMPULSE_TO_CENTER, OP_BODY_APPLY_ANGULAR_IMPULSE, OP_BODY_SET_BULLET, OP_BODY_SET_MASS_DATA, OP_BODY_APPLY_MASS_FROM_SHAPES, OP_BODY_SET_LINEAR_DAMPING, OP_BODY_SET_ANGULAR_DAMPING, OP_BODY_SET_GRAVITY_SCALE, OP_BODY_SET_AWAKE, OP_BODY_ENABLE_SLEEP, OP_BODY_SET_SLEEP_THRESHOLD, OP_BODY_SET_MOTION_LOCKS, OP_CREATE_SPHERE_SHAPE, OP_CREATE_CAPSULE_SHAPE, OP_CREATE_HULL_SHAPE, OP_CREATE_TRANSFORMED_HULL_SHAPE, OP_CREATE_MESH_SHAPE, OP_CREATE_HEIGHT_FIELD_SHAPE, OP_CREATE_COMPOUND_SHAPE, OP_DESTROY_SHAPE, OP_SHAPE_SET_DENSITY, OP_SHAPE_SET_FILTER, OP_SHAPE_SET_SURFACE_MATERIAL, OP_SHAPE_SET_SPHERE, OP_SHAPE_SET_CAPSULE, OP_SHAPE_SET_HULL, OP_SHAPE_SET_MESH, OP_SHAPE_SET_FRICTION, OP_SHAPE_SET_RESTITUTION, OP_SHAPE_SET_MESH_MATERIAL, OP_SHAPE_ENABLE_PRE_SOLVE_EVENTS, OP_BODY_ENABLE_HIT_EVENTS, OP_BODY_ALLOW_FAST_ROTATION, OP_BODY_ENABLE_CONTACT_RECYCLING, OP_BODY_SET_SAFETY_FACTOR, OP_SHAPE_APPLY_WIND, OP_BODY_SET_NAME, OP_SHAPE_SET_NAME, OP_CREATE_DISTANCE_JOINT, OP_CREATE_MOTOR_JOINT, OP_CREATE_FILTER_JOINT, OP_CREATE_PARALLEL_JOINT, OP_CREATE_PRISMATIC_JOINT, OP_CREATE_REVOLUTE_JOINT, OP_CREATE_SPHERICAL_JOINT, OP_CREATE_WELD_JOINT, OP_CREATE_WHEEL_JOINT, OP_DESTROY_JOINT, OP_JOINT_SET_LOCAL_FRAME_A, OP_JOINT_SET_LOCAL_FRAME_B, OP_JOINT_SET_COLLIDE_CONNECTED, OP_JOINT_WAKE_BODIES, OP_JOINT_SET_CONSTRAINT_TUNING, OP_JOINT_SET_FORCE_THRESHOLD, OP_JOINT_SET_TORQUE_THRESHOLD, OP_BODY_SET_TYPE, OP_BODY_DISABLE, OP_BODY_ENABLE, OP_SHAPE_ENABLE_SENSOR_EVENTS, OP_SHAPE_ENABLE_CONTACT_EVENTS, OP_SHAPE_ENABLE_HIT_EVENTS, OP_STEP, OP_ENABLE_SLEEPING, OP_ENABLE_WARM_STARTING, OP_ENABLE_CONTINUOUS, OP_ENABLE_SPECULATIVE, OP_SET_RESTITUTION_THRESHOLD, OP_SET_RESTITUTION_ITERATIONS, OP_ENABLE_RESTITUTION_PROPAGATION, OP_ENABLE_KINEMATIC_STATIC_SOFTNESS, OP_SET_HIT_EVENT_THRESHOLD, OP_SET_CONTACT_TUNING, OP_SET_CONTACT_RECYCLE_DISTANCE, OP_SET_MAXIMUM_LINEAR_SPEED, OP_SET_GRAVITY, OP_REBUILD_STATIC_TREE, OP_EXPLODE, OP_DISTANCE_JOINT_SET_LENGTH, OP_DISTANCE_JOINT_ENABLE_LIMIT, OP_DISTANCE_JOINT_SET_LENGTH_RANGE, OP_DISTANCE_JOINT_ENABLE_SPRING, OP_DISTANCE_JOINT_SET_SPRING_FORCE_RANGE, OP_DISTANCE_JOINT_SET_SPRING_HERTZ, OP_DISTANCE_JOINT_SET_SPRING_DAMPING_RATIO, OP_DISTANCE_JOINT_ENABLE_MOTOR, OP_DISTANCE_JOINT_SET_MOTOR_SPEED, OP_DISTANCE_JOINT_SET_MAX_MOTOR_FORCE, OP_MOTOR_JOINT_SET_LINEAR_VELOCITY, OP_MOTOR_JOINT_SET_ANGULAR_VELOCITY, OP_MOTOR_JOINT_SET_MAX_VELOCITY_TORQUE, OP_MOTOR_JOINT_SET_MAX_VELOCITY_FORCE, OP_MOTOR_JOINT_SET_LINEAR_HERTZ, OP_MOTOR_JOINT_SET_LINEAR_DAMPING_RATIO, OP_MOTOR_JOINT_SET_ANGULAR_HERTZ, OP_MOTOR_JOINT_SET_ANGULAR_DAMPING_RATIO, OP_MOTOR_JOINT_SET_MAX_SPRING_FORCE, OP_MOTOR_JOINT_SET_MAX_SPRING_TORQUE, OP_PARALLEL_JOINT_SET_SPRING_HERTZ, OP_PARALLEL_JOINT_SET_SPRING_DAMPING_RATIO, OP_PARALLEL_JOINT_SET_MAX_TORQUE, OP_PRISMATIC_JOINT_ENABLE_SPRING, OP_PRISMATIC_JOINT_SET_SPRING_HERTZ, OP_PRISMATIC_JOINT_SET_SPRING_DAMPING_RATIO, OP_PRISMATIC_JOINT_SET_TARGET_TRANSLATION, OP_PRISMATIC_JOINT_ENABLE_LIMIT, OP_PRISMATIC_JOINT_SET_LIMITS, OP_PRISMATIC_JOINT_ENABLE_MOTOR, OP_PRISMATIC_JOINT_SET_MOTOR_SPEED, OP_PRISMATIC_JOINT_SET_MAX_MOTOR_FORCE, OP_REVOLUTE_JOINT_ENABLE_LIMIT, OP_REVOLUTE_JOINT_SET_LIMITS, OP_REVOLUTE_JOINT_ENABLE_SPRING, OP_REVOLUTE_JOINT_SET_TARGET_ANGLE, OP_REVOLUTE_JOINT_SET_SPRING_HERTZ, OP_REVOLUTE_JOINT_SET_SPRING_DAMPING_RATIO, OP_REVOLUTE_JOINT_ENABLE_MOTOR, OP_REVOLUTE_JOINT_SET_MOTOR_SPEED, OP_REVOLUTE_JOINT_SET_MAX_MOTOR_TORQUE, OP_SPHERICAL_JOINT_ENABLE_CONE_LIMIT, OP_SPHERICAL_JOINT_SET_CONE_LIMIT, OP_SPHERICAL_JOINT_SET_CONE_AXIS, OP_SPHERICAL_JOINT_SET_CONE_ELLIPSE, OP_SPHERICAL_JOINT_ENABLE_TWIST_LIMIT, OP_SPHERICAL_JOINT_SET_TWIST_LIMITS, OP_SPHERICAL_JOINT_ENABLE_SPRING, OP_SPHERICAL_JOINT_SET_TARGET_ROTATION, OP_SPHERICAL_JOINT_SET_SPRING_HERTZ, OP_SPHERICAL_JOINT_SET_SPRING_DAMPING_RATIO, OP_SPHERICAL_JOINT_ENABLE_MOTOR, OP_SPHERICAL_JOINT_SET_MOTOR_VELOCITY, OP_SPHERICAL_JOINT_SET_MAX_MOTOR_TORQUE, OP_WELD_JOINT_SET_LINEAR_HERTZ, OP_WELD_JOINT_SET_LINEAR_DAMPING_RATIO, OP_WELD_JOINT_SET_ANGULAR_HERTZ, OP_WELD_JOINT_SET_ANGULAR_DAMPING_RATIO, OP_WHEEL_JOINT_ENABLE_SUSPENSION, OP_WHEEL_JOINT_SET_SUSPENSION_HERTZ, OP_WHEEL_JOINT_SET_SUSPENSION_DAMPING_RATIO, OP_WHEEL_JOINT_ENABLE_SUSPENSION_LIMIT, OP_WHEEL_JOINT_SET_SUSPENSION_LIMITS, OP_WHEEL_JOINT_ENABLE_SPIN_MOTOR, OP_WHEEL_JOINT_SET_SPIN_MOTOR_SPEED, OP_WHEEL_JOINT_SET_MAX_SPIN_TORQUE, OP_WHEEL_JOINT_ENABLE_STEERING, OP_WHEEL_JOINT_SET_STEERING_HERTZ, OP_WHEEL_JOINT_SET_STEERING_DAMPING_RATIO, OP_WHEEL_JOINT_SET_MAX_STEERING_TORQUE, OP_WHEEL_JOINT_ENABLE_STEERING_LIMIT, OP_WHEEL_JOINT_SET_STEERING_LIMITS, OP_WHEEL_JOINT_SET_TARGET_STEERING_ANGLE, OP_ENABLE_KINEMATIC_SPECULATION, OP_ENABLE_TRAVEL_SPECULATION, + OP_QUERY_CAST_SHAPE, OP_QUERY_CAST_RAY_CLOSEST, OP_QUERY_CAST_MOVER, OP_QUERY_COLLIDE_MOVER, OP_CREATE_BODY, OP_DESTROY_BODY, OP_BODY_SET_TRANSFORM, OP_BODY_SET_LINEAR_VELOCITY, OP_BODY_SET_ANGULAR_VELOCITY, OP_BODY_SET_TARGET_TRANSFORM, OP_BODY_APPLY_FORCE, OP_BODY_APPLY_FORCE_TO_CENTER, OP_BODY_APPLY_TORQUE, OP_BODY_APPLY_LINEAR_IMPULSE, OP_BODY_APPLY_LINEAR_IMPULSE_TO_CENTER, OP_BODY_APPLY_ANGULAR_IMPULSE, OP_BODY_SET_BULLET, OP_BODY_SET_MASS_DATA, OP_BODY_APPLY_MASS_FROM_SHAPES, OP_BODY_SET_LINEAR_DAMPING, OP_BODY_SET_ANGULAR_DAMPING, OP_BODY_SET_GRAVITY_SCALE, OP_BODY_SET_AWAKE, OP_BODY_ENABLE_SLEEP, OP_BODY_SET_SLEEP_THRESHOLD, OP_BODY_SET_MOTION_LOCKS, OP_CREATE_SPHERE_SHAPE, OP_CREATE_CAPSULE_SHAPE, OP_CREATE_HULL_SHAPE, OP_CREATE_TRANSFORMED_HULL_SHAPE, OP_CREATE_MESH_SHAPE, OP_CREATE_HEIGHT_FIELD_SHAPE, OP_CREATE_COMPOUND_SHAPE, OP_DESTROY_SHAPE, OP_SHAPE_SET_DENSITY, OP_SHAPE_SET_FILTER, OP_SHAPE_SET_SURFACE_MATERIAL, OP_SHAPE_SET_SPHERE, OP_SHAPE_SET_CAPSULE, OP_SHAPE_SET_HULL, OP_SHAPE_SET_MESH, OP_SHAPE_SET_FRICTION, OP_SHAPE_SET_RESTITUTION, OP_SHAPE_SET_MESH_MATERIAL, OP_SHAPE_ENABLE_PRE_SOLVE_EVENTS, OP_BODY_ENABLE_HIT_EVENTS, OP_BODY_ALLOW_FAST_ROTATION, OP_BODY_ENABLE_CONTACT_RECYCLING, OP_BODY_SET_SAFETY_FACTOR, OP_SHAPE_APPLY_WIND, OP_BODY_SET_NAME, OP_SHAPE_SET_NAME, OP_CREATE_DISTANCE_JOINT, OP_CREATE_MOTOR_JOINT, OP_CREATE_FILTER_JOINT, OP_CREATE_PARALLEL_JOINT, OP_CREATE_PRISMATIC_JOINT, OP_CREATE_REVOLUTE_JOINT, OP_CREATE_SPHERICAL_JOINT, OP_CREATE_WELD_JOINT, OP_CREATE_WHEEL_JOINT, OP_DESTROY_JOINT, OP_JOINT_SET_LOCAL_FRAME_A, OP_JOINT_SET_LOCAL_FRAME_B, OP_JOINT_SET_COLLIDE_CONNECTED, OP_JOINT_WAKE_BODIES, OP_JOINT_SET_CONSTRAINT_TUNING, OP_JOINT_SET_FORCE_THRESHOLD, OP_JOINT_SET_TORQUE_THRESHOLD, OP_BODY_SET_TYPE, OP_BODY_DISABLE, OP_BODY_ENABLE, OP_SHAPE_ENABLE_SENSOR_EVENTS, OP_SHAPE_ENABLE_CONTACT_EVENTS, OP_SHAPE_ENABLE_HIT_EVENTS, OP_STEP, OP_ENABLE_SLEEPING, OP_ENABLE_WARM_STARTING, OP_ENABLE_CONTINUOUS, OP_ENABLE_SPECULATIVE, OP_SET_RESTITUTION_THRESHOLD, OP_SET_RESTITUTION_ITERATIONS, OP_ENABLE_RESTITUTION_PROPAGATION, OP_ENABLE_KINEMATIC_STATIC_SOFTNESS, OP_SET_HIT_EVENT_THRESHOLD, OP_SET_CONTACT_TUNING, OP_SET_CONTACT_RECYCLE_DISTANCE, OP_SET_MAXIMUM_LINEAR_SPEED, OP_SET_GRAVITY, OP_REBUILD_STATIC_TREE, OP_EXPLODE, OP_DISTANCE_JOINT_SET_LENGTH, OP_DISTANCE_JOINT_ENABLE_LIMIT, OP_DISTANCE_JOINT_SET_LENGTH_RANGE, OP_DISTANCE_JOINT_ENABLE_SPRING, OP_DISTANCE_JOINT_SET_SPRING_FORCE_RANGE, OP_DISTANCE_JOINT_SET_SPRING_HERTZ, OP_DISTANCE_JOINT_SET_SPRING_DAMPING_RATIO, OP_DISTANCE_JOINT_ENABLE_MOTOR, OP_DISTANCE_JOINT_SET_MOTOR_SPEED, OP_DISTANCE_JOINT_SET_MAX_MOTOR_FORCE, OP_MOTOR_JOINT_SET_LINEAR_VELOCITY, OP_MOTOR_JOINT_SET_ANGULAR_VELOCITY, OP_MOTOR_JOINT_SET_MAX_VELOCITY_TORQUE, OP_MOTOR_JOINT_SET_MAX_VELOCITY_FORCE, OP_MOTOR_JOINT_SET_LINEAR_HERTZ, OP_MOTOR_JOINT_SET_LINEAR_DAMPING_RATIO, OP_MOTOR_JOINT_SET_ANGULAR_HERTZ, OP_MOTOR_JOINT_SET_ANGULAR_DAMPING_RATIO, OP_MOTOR_JOINT_SET_MAX_SPRING_FORCE, OP_MOTOR_JOINT_SET_MAX_SPRING_TORQUE, OP_PARALLEL_JOINT_SET_SPRING_HERTZ, OP_PARALLEL_JOINT_SET_SPRING_DAMPING_RATIO, OP_PARALLEL_JOINT_SET_MAX_TORQUE, OP_PRISMATIC_JOINT_ENABLE_SPRING, OP_PRISMATIC_JOINT_SET_SPRING_HERTZ, OP_PRISMATIC_JOINT_SET_SPRING_DAMPING_RATIO, OP_PRISMATIC_JOINT_SET_TARGET_TRANSLATION, OP_PRISMATIC_JOINT_ENABLE_LIMIT, OP_PRISMATIC_JOINT_SET_LIMITS, OP_PRISMATIC_JOINT_ENABLE_MOTOR, OP_PRISMATIC_JOINT_SET_MOTOR_SPEED, OP_PRISMATIC_JOINT_SET_MAX_MOTOR_FORCE, OP_REVOLUTE_JOINT_ENABLE_LIMIT, OP_REVOLUTE_JOINT_SET_LIMITS, OP_REVOLUTE_JOINT_ENABLE_SPRING, OP_REVOLUTE_JOINT_SET_TARGET_ANGLE, OP_REVOLUTE_JOINT_SET_SPRING_HERTZ, OP_REVOLUTE_JOINT_SET_SPRING_DAMPING_RATIO, OP_REVOLUTE_JOINT_ENABLE_MOTOR, OP_REVOLUTE_JOINT_SET_MOTOR_SPEED, OP_REVOLUTE_JOINT_SET_MAX_MOTOR_TORQUE, OP_SPHERICAL_JOINT_ENABLE_CONE_LIMIT, OP_SPHERICAL_JOINT_SET_CONE_LIMIT, OP_SPHERICAL_JOINT_SET_CONE_AXIS, OP_SPHERICAL_JOINT_SET_CONE_ELLIPSE, OP_SPHERICAL_JOINT_ENABLE_TWIST_LIMIT, OP_SPHERICAL_JOINT_SET_TWIST_LIMITS, OP_SPHERICAL_JOINT_ENABLE_SPRING, OP_SPHERICAL_JOINT_SET_TARGET_ROTATION, OP_SPHERICAL_JOINT_SET_SPRING_HERTZ, OP_SPHERICAL_JOINT_SET_SPRING_DAMPING_RATIO, OP_SPHERICAL_JOINT_ENABLE_MOTOR, OP_SPHERICAL_JOINT_SET_MOTOR_VELOCITY, OP_SPHERICAL_JOINT_SET_MAX_MOTOR_TORQUE, OP_WELD_JOINT_SET_LINEAR_HERTZ, OP_WELD_JOINT_SET_LINEAR_DAMPING_RATIO, OP_WELD_JOINT_SET_ANGULAR_HERTZ, OP_WELD_JOINT_SET_ANGULAR_DAMPING_RATIO, OP_WHEEL_JOINT_ENABLE_SUSPENSION, OP_WHEEL_JOINT_SET_SUSPENSION_HERTZ, OP_WHEEL_JOINT_SET_SUSPENSION_DAMPING_RATIO, OP_WHEEL_JOINT_ENABLE_SUSPENSION_LIMIT, OP_WHEEL_JOINT_SET_SUSPENSION_LIMITS, OP_WHEEL_JOINT_ENABLE_SPIN_MOTOR, OP_WHEEL_JOINT_SET_SPIN_MOTOR_SPEED, OP_WHEEL_JOINT_SET_MAX_SPIN_TORQUE, OP_WHEEL_JOINT_ENABLE_STEERING, OP_WHEEL_JOINT_SET_STEERING_HERTZ, OP_WHEEL_JOINT_SET_STEERING_DAMPING_RATIO, OP_WHEEL_JOINT_SET_MAX_STEERING_TORQUE, OP_WHEEL_JOINT_ENABLE_STEERING_LIMIT, OP_WHEEL_JOINT_SET_STEERING_LIMITS, OP_WHEEL_JOINT_SET_TARGET_STEERING_ANGLE, OP_ENABLE_KINEMATIC_SPECULATION, OP_ENABLE_TRAVEL_SPECULATION, OP_SPHERICAL_JOINT_LOCK_CONE_MINOR, create_compound_shape, destroy_shape, shape_is_valid, get_shape_full_id, get_shape, shape_get_body, shape_get_aabb, shape_get_type, shape_get_density, shape_set_density, shape_get_filter, shape_set_filter, shape_get_surface_material, shape_set_surface_material, shape_get_material_count, shape_is_sensor, @@ -135,7 +135,7 @@ exports ( default_weld_joint_def, default_wheel_joint_def, null_joint_id, distance_joint_of, motor_joint_of, parallel_joint_of, prismatic_joint_of, revolute_joint_of, spherical_joint_of, weld_joint_of, wheel_joint_of, set_spherical_cone_axis, spherical_cone_off_axis, clamp_spherical_cone, spherical_swing_angle, - spherical_cone_elliptical, spherical_cone_minor, spherical_cone_gap, spherical_cone_push_axis, + spherical_cone_elliptical, spherical_cone_minor, spherical_cone_gap, spherical_cone_push_axis, spherical_cone_major_axis, spherical_cone_minor_axis, spherical_lock_error, joint_is_valid, get_joint_full_id, make_joint_id, get_joint_sim, get_joint_sim_of_kind, joint_edge_next, create_distance_joint, create_motor_joint, create_filter_joint, create_parallel_joint, create_prismatic_joint, create_revolute_joint, create_spherical_joint, create_weld_joint, create_wheel_joint, destroy_joint, destroy_joint_internal, @@ -212,7 +212,7 @@ const JOINT_SPHERICAL = 6 const JOINT_WELD = 7 const JOINT_WHEEL = 8 const TIME_TO_SLEEP = 0.5 -const JOINT_DATA_BYTES = 560 // JointSim.data: room for the largest kind's data (the spherical joint's 560) +const JOINT_DATA_BYTES = 608 // JointSim.data: room for the largest kind's data (the spherical joint's 608) // The shape kinds (the shape module's, restated). const KIND_SPHERE = 0 const KIND_CAPSULE = 1 @@ -968,6 +968,11 @@ struct SphericalJoint { enable_motor: bool enable_cone_limit: bool enable_twist_limit: bool + // A hinge that twists (#190, past the reference): the cone's minor + // half-angle locked at zero, frame B's z held in the plane of the + // cone's centre and its major axis by an equality (as a revolute holds + // its axis), the cone along that plane a circle of cone_angle. + lock_cone_minor: bool index_a: int index_b: int linear_impulse: Vec3 @@ -996,6 +1001,11 @@ struct SphericalJoint { swing_mass: float twist_mass: float spring_softness: Softness + // The minor lock's row: its axis (B's z crossed with the cone's minor + // axis, world), impulse and effective mass; unused unless locked. + lock_axis: Vec3 + lock_impulse: float + lock_mass: float } struct WeldJoint { @@ -1072,7 +1082,7 @@ struct JointSim { constraint_softness: Softness force_threshold: float // the solve's joint event test torque_threshold: float - data: long[70] // JOINT_DATA_BYTES + data: long[76] // JOINT_DATA_BYTES joint_id: int body_id_a: int body_id_b: int @@ -2416,7 +2426,7 @@ struct RecordedGeometry { // which older players skip. const RECORDING_MAGIC = 0x43525041 // "APRC", little-endian const RECORDING_MAJOR_VERSION = 1 -const RECORDING_MINOR_VERSION = 2 +const RECORDING_MINOR_VERSION = 3 const RECORDING_HEADER_BYTES = 32 const OP_STATE_HASH = 2 @@ -2592,6 +2602,7 @@ const OP_WHEEL_JOINT_SET_STEERING_LIMITS = 174 const OP_WHEEL_JOINT_SET_TARGET_STEERING_ANGLE = 175 const OP_ENABLE_KINEMATIC_SPECULATION = 176 const OP_ENABLE_TRAVEL_SPECULATION = 177 +const OP_SPHERICAL_JOINT_LOCK_CONE_MINOR = 178 // Whether the world of this index is recording: the hooks' test, false // for an index that is not a world's. @@ -7064,14 +7075,7 @@ cone_reach(s: *SphericalJoint, b: Vec3) -> ConeReach { reach = ConeReach { theta: math.atan2_approx(sin_t, cos_t), sin_theta: sin_t, cos_theta: cos_t, across: math.vec3_zero(), along_major: 1.0, along_minor: 0.0 } if sin_t < math.EPSILON { return reach } d = math.mul_sv(1.0 / sin_t, lateral) - // The ellipse's axes across the centre: the major one from - // cone_major_axis (frame A's x when zero, its y when x lies along the - // centre), the minor one square to it. - major = math.widen3(s.cone_major_axis) - if math.length_squared(major) == 0.0 { major = math.vec3_axis_x() } - major = math.sub(major, math.mul_sv(math.dot(major, c), c)) - if math.length_squared(major) < 0.000001 { major = math.sub(math.vec3_axis_y(), math.mul_sv(c.y, c)) } - major = math.normalize(major) + major = spherical_cone_major_axis(s) minor = math.cross(c, major) reach.across = d reach.along_major = math.dot(d, major) @@ -7079,6 +7083,27 @@ cone_reach(s: *SphericalJoint, b: Vec3) -> ConeReach { return reach } +// The cone's axes across its centre, in frame A: the major one from +// cone_major_axis (frame A's x when zero, its y when x lies along the +// centre), unit, and the minor one square to it. +spherical_cone_major_axis(s: *SphericalJoint) -> Vec3 { + c = s.cone_axis + major = math.widen3(s.cone_major_axis) + if math.length_squared(major) == 0.0 { major = math.vec3_axis_x() } + major = math.sub(major, math.mul_sv(math.dot(major, c), c)) + if math.length_squared(major) < 0.000001 { major = math.sub(math.vec3_axis_y(), math.mul_sv(c.y, c)) } + return math.normalize(major) +} + +spherical_cone_minor_axis(s: *SphericalJoint) -> Vec3 { return math.cross(s.cone_axis, spherical_cone_major_axis(s)) } + +// The minor lock's error with frames A and B at `quat_a` and `quat_b` +// (world): B's z along the cone's minor axis, the sine of the angle it +// leans out of the lock's plane. +spherical_lock_error(s: *SphericalJoint, quat_a: Quat, quat_b: Quat) -> float { + return math.dot(math.rotate_vector(quat_b, math.vec3_axis_z()), math.rotate_vector(quat_a, spherical_cone_minor_axis(s))) +} + // The ellipse's half-angle in a direction: the polar radius of an ellipse // of half-angles a (along the major axis) and b (across it). cone_radius(a: float, b: float, along_major: float, along_minor: float) -> float { @@ -7514,6 +7539,11 @@ joint_get_angular_separation(id: JointId) -> float { sum = sum + math.max_float(0.0, s.lower_twist_angle - twist) sum = sum + math.max_float(0.0, twist - s.upper_twist_angle) } + // A locked minor: the angle B's z leans out of the lock's plane. + if s.lock_cone_minor { + lean = spherical_lock_error(s, q_a, q_b) + sum = sum + math.abs_float(math.atan2_approx(lean, math.sqrt_float(math.max_float(1.0 - lean * lean, 0.0)))) + } return sum } if kind == JOINT_WELD { diff --git a/aephysics/human/module.ae b/aephysics/human/module.ae index 7a483cd..dc0992b 100644 --- a/aephysics/human/module.ae +++ b/aephysics/human/module.ae @@ -15,6 +15,13 @@ // straight) unless human_tight_limits puts back the reference's, and its // soles grip the ground. // +// Past the reference's twelve, each optional as the ankles are (#152, +// #190): a head on its own body above the neck, clavicles between the +// chest and the upper arms, hands below the forearms, a spine_02 between +// the two spine bodies, and elbows and knees that twist about the bone +// as they bend. Each is a flag on Human, set before the figure is made; +// left off, the figure is built as before, bit for bit. +// // This is Box3D's shared/human.c (Erin Catto, MIT): CreateHuman is // create_human, Human_SetJointFrictionTorque is // set_joint_friction_torque. Beyond the reference's sample it is the @@ -35,9 +42,14 @@ exports ( BONE_PELVIS, BONE_SPINE_01, BONE_SPINE_03, BONE_NECK, BONE_THIGH_L, BONE_CALF_L, BONE_THIGH_R, BONE_CALF_R, BONE_UPPER_ARM_L, BONE_LOWER_ARM_L, BONE_UPPER_ARM_R, BONE_LOWER_ARM_R, BONE_COUNT, FILTER_JOINT_COUNT, BONE_FOOT_L, BONE_FOOT_R, MAX_BONE_COUNT, ANKLE_PLANTAR_DEGREES, ANKLE_DORSAL_DEGREES, ANKLE_SIDE_DEGREES, ANKLE_ROLL_DEGREES, + BONE_HEAD, BONE_CLAVICLE_L, BONE_CLAVICLE_R, BONE_HAND_L, BONE_HAND_R, BONE_SPINE_02, ANKLED_BONE_COUNT, MAX_FILTER_JOINT_COUNT, + HEAD_FLEXION_DEGREES, HEAD_EXTENSION_DEGREES, HEAD_TILT_DEGREES, HEAD_TURN_DEGREES, + CLAVICLE_ELEVATION_DEGREES, CLAVICLE_DEPRESSION_DEGREES, CLAVICLE_PROTRACTION_DEGREES, CLAVICLE_ROTATION_DEGREES, + WRIST_FLEXION_DEGREES, WRIST_EXTENSION_DEGREES, WRIST_DEVIATION_DEGREES, WRIST_TWIST_DEGREES, + ELBOW_TWIST_DEGREES, KNEE_TWIST_DEGREES, HumanBone, Human, BoneSpec, bone_at, filter_joint_at, spec_at, human_reference_shape, human_anatomical_limits, human_tight_limits, human_chain_child, human_shape_head, human_shape_tail, human_shape_toe, human_fit_shape, destroy_shape, create_human_shaped, - human_bone_far_end, + human_fit_bone, human_has_bone, human_bone_far_end, create_human, destroy_human, human_set_velocity, human_apply_angular_impulse, human_set_joint_friction_torque, human_set_joint_spring_hertz, human_set_joint_damping_ratio, human_align_spring, human_release_align_spring, human_create_motor_anchors, human_create_parallel_anchors, human_destroy_anchors, human_has_anchors, human_set_anchor_target, human_set_bullet @@ -65,7 +77,51 @@ const FILTER_JOINT_COUNT = 3 // its calf; without, the reference's twelve, the foot a shape on the calf. const BONE_FOOT_L = 12 const BONE_FOOT_R = 13 -const MAX_BONE_COUNT = 14 +const ANKLED_BONE_COUNT = 14 +// The bones past the reference's figure and its feet (#190), at fixed +// indices: a figure with any of them has MAX_BONE_COUNT bones, and those +// it was not given (the feet too, without ankles) have no body: see +// human_has_bone. MAX_BONE_COUNT is the most bones a figure has. +const BONE_HEAD = 14 +const BONE_CLAVICLE_L = 15 +const BONE_CLAVICLE_R = 16 +const BONE_HAND_L = 17 +const BONE_HAND_R = 18 +const BONE_SPINE_02 = 19 +const MAX_BONE_COUNT = 20 +// The reference's three filter joints, and room for the ones the extra +// bones bring: the head off the chest, the clavicles and the upper arms, +// the clavicles off each other and the neck, and each bone off the one +// it no longer hangs from or that it was set between (the upper arms off +// the chest, the chest off the lumbar, spine_02 off the pelvis). +const MAX_FILTER_JOINT_COUNT = 15 +// A person's ranges for the extra joints, in degrees. The head nods +// forward and back and tilts to the side on the neck, beyond what the +// neck's own joint gives, and turns on it (the atlas): with the neck's +// cone of 30 and twist of 17.5, the cervical spine's 55 / 65 / 50 / 77. +const HEAD_FLEXION_DEGREES = 25.0 +const HEAD_EXTENSION_DEGREES = 35.0 +const HEAD_TILT_DEGREES = 20.0 +const HEAD_TURN_DEGREES = 60.0 +// The shoulder girdle: the clavicle lifts 40 and drops 10, swings 20 +// forward and back (protraction, retraction) and turns 20 about itself. +const CLAVICLE_ELEVATION_DEGREES = 40.0 +const CLAVICLE_DEPRESSION_DEGREES = 10.0 +const CLAVICLE_PROTRACTION_DEGREES = 20.0 +const CLAVICLE_ROTATION_DEGREES = 20.0 +// A clavicle body's density against the default: the girdle's mass. +const CLAVICLE_DENSITY = 2.0 +// The wrist: 75 toward the palm, 65 back, 27.5 to either side between the +// radial 20 and ulnar 35, and little turn of its own (the forearm turns). +const WRIST_FLEXION_DEGREES = 75.0 +const WRIST_EXTENSION_DEGREES = 65.0 +const WRIST_DEVIATION_DEGREES = 27.5 +const WRIST_TWIST_DEGREES = 10.0 +// A twisting hinge (#190): the forearm turns 80 either way about itself +// (pronation, supination), the shin 10 (the knee's axial turn); either +// bends as its hinge did, and neither bends sideways. +const ELBOW_TWIST_DEGREES = 80.0 +const KNEE_TWIST_DEGREES = 10.0 // The ankle's range, a loaded one's: the toes down 50 degrees and up 35 (an // elliptical cone about a centre tipped toward the toes), 15 to either // side, and the sole rolled 15 either way about the foot's line. @@ -103,29 +159,59 @@ struct HumanBone { } // The ragdoll: a heap block of bones and filter joints (create allocates, -// destroy frees). `ankles`, set before it is made, gives the feet bones of -// their own (bone_count MAX_BONE_COUNT); left false it is the reference's -// figure of BONE_COUNT bones. +// destroy frees). The flags, set before it is made, add bones past the +// reference's figure of BONE_COUNT: `ankles` the feet (bone_count +// ANKLED_BONE_COUNT); `head`, `clavicles`, `hands` and `spine_02` those +// bones (bone_count MAX_BONE_COUNT, the ones not asked for left without a +// body); `twist` makes the elbows and knees twisting hinges. struct Human { bones: ptr // HumanBone[bone_count] - filter_joints: ptr // JointId[FILTER_JOINT_COUNT] + filter_joints: ptr // JointId[MAX_FILTER_JOINT_COUNT] filter_joint_count: int friction_torque: float is_spawned: bool ankles: bool bone_count: int + head: bool + clavicles: bool + hands: bool + spine_02: bool + twist: bool } bone_at(h: *Human, index: int) -> *HumanBone { return (h.bones + index * sizeof(HumanBone)) as *HumanBone } filter_joint_at(h: *Human, index: int) -> JointId { return (h.filter_joints as JointId[])[index] } +// Whether a figure was given a bone (the reference's twelve always; a +// foot with ankles; an extra bone with its flag), whatever it is spawned. +asks_for_bone(h: *Human, index: int) -> bool { + if index < 0 || index >= MAX_BONE_COUNT { return false } + if index < BONE_COUNT { return true } + if index == BONE_FOOT_L || index == BONE_FOOT_R { return h.ankles } + if index == BONE_HEAD { return h.head } + if index == BONE_CLAVICLE_L || index == BONE_CLAVICLE_R { return h.clavicles } + if index == BONE_HAND_L || index == BONE_HAND_R { return h.hands } + return h.spine_02 +} + +// Whether a spawned figure has the bone: a body, a joint to its parent. +// The slots past the reference's twelve that its flags did not ask for +// hold no body, so a loop to bone_count asks here first. +human_has_bone(h: *Human, index: int) -> bool { + if h.is_spawned == false || index < 0 || index >= h.bone_count { return false } + return asks_for_bone(h, index) +} + +// Whether any flag asks for a bone past the feet. +has_extras(h: *Human) -> bool { return h.head || h.clavicles || h.hands || h.spine_02 } + // A built bone's far end in its body's frame: the end of its own capsule // farther from its joint (a forearm's past the wrist, a foot's at the // toe), but a calf with ankles ends at its ankle, the foot's joint, which // its shin stops short of by a radius (#174). A figure not spawned, or an // index past its bones, has none: zero. human_bone_far_end(h: *Human, index: int) -> Vec3 { - if h.is_spawned == false || index < 0 || index >= h.bone_count { return math.vec3_zero() } + if human_has_bone(h, index) == false { return math.vec3_zero() } if h.ankles && (index == BONE_CALF_L || index == BONE_CALF_R) { foot = BONE_FOOT_L if index == BONE_CALF_R { foot = BONE_FOOT_R } @@ -171,10 +257,14 @@ struct BoneSpec { cone_major_axis: Vec3 } -// A figure's shape is BONE_COUNT BoneSpecs in one block from the engine's -// allocator: the reference's from human_reference_shape, a rig's once -// human_fit_shape has fitted it, and create_human_shaped builds either. -// The caller frees it with destroy_shape. +// A figure's shape is MAX_BONE_COUNT BoneSpecs in one block from the +// engine's allocator: the reference's from human_reference_shape, a rig's +// once human_fit_shape has fitted it, and create_human_shaped builds +// either. The first BONE_COUNT are the reference's bones; the feet's two +// slots are unused (a foot is built from its calf's spec); the extra +// bones' specs follow, made from the twelve (extra_specs) and fitted to a +// rig with them or bone by bone (human_fit_bone). The caller frees it +// with destroy_shape. spec_at(shape: @into ptr, index: int) -> *BoneSpec { return (shape + index * sizeof(BoneSpec)) as *BoneSpec } set_spec(shape: ptr, index: int, spec: BoneSpec) { (shape as BoneSpec[])[index] = spec } @@ -199,7 +289,7 @@ const FOOT_FRICTION = 0.9 // The reference's figure, bone by bone (its frames measured from the rig, // its limits the reference's: cones about the rest, hinges either side of it). reference_tight_shape() -> ptr { - shape = basics.alloc(BONE_COUNT * sizeof(BoneSpec)) + shape = basics.alloc(MAX_BONE_COUNT * sizeof(BoneSpec)) no_joint = math.transform_identity() on_axis = math.vec3_axis_z() set_spec(shape, BONE_PELVIS, BoneSpec { @@ -287,9 +377,155 @@ reference_tight_shape() -> ptr { frame_a: transform_of(0.0 - 0.297979, 0.0 - 0.000361, 0.0, 0.0 - 0.405444, 0.914059, 0.010541, 0.000852), frame_b: transform_of(0.0, 0.0, 0.0, 0.0 - 0.002794, 0.999936, 0.010042, 0.004364), swing_degrees: 0.0, lower_degrees: 0.0 - 35.0, upper_degrees: 35.0, joint_friction: 0.06, density: 1.0, has_foot: false, foot: no_foot(), foot_friction: 0.0, cone_axis: on_axis, swing_minor_degrees: 0.0, cone_major_axis: math.vec3_zero() }) + set_spec(shape, BONE_FOOT_L, unused_spec()) + set_spec(shape, BONE_FOOT_R, unused_spec()) + extra_specs(shape) return shape } +// The feet's slots: a foot is built from its calf's spec. +unused_spec() -> BoneSpec { + no_joint = math.transform_identity() + return BoneSpec { + parent: 0 - 1, reference: no_joint, group: 0, friction: 0.0, capsule: no_foot(), + joint_kind: dynamics.JOINT_PARALLEL, frame_a: no_joint, frame_b: no_joint, swing_degrees: 0.0, lower_degrees: 0.0, upper_degrees: 0.0, joint_friction: 0.0, + density: 0.0, has_foot: false, foot: no_foot(), foot_friction: 0.0, cone_axis: math.vec3_axis_z(), swing_minor_degrees: 0.0, cone_major_axis: math.vec3_zero() } +} + +// --- the extra bones' shapes (#190) ------------------------------------------------------------------------ + +// An extra bone's spec: its frame `reference` looks down the bone (its z +// from the joint along the bone, its x the way the joint's main range +// reaches), so the joint's frames are the bone's own, one at rest. Its +// cone is an ellipse about a centre tipped `tilt` degrees toward x, +// reaching `major` degrees either side of it along x and `minor` across +// (a circle when minor is 0), and it turns `twist` degrees either way. +extra_spec(parent: int, reference: Transform, capsule: Capsule, group: int, friction: float, joint_friction: float, + tilt: float, major: float, minor: float, twist: float) -> BoneSpec { + t = tilt * math.DEG_TO_RAD + return BoneSpec { + parent: parent, reference: reference, group: group, friction: friction, capsule: capsule, + joint_kind: dynamics.JOINT_SPHERICAL, frame_a: math.transform_identity(), frame_b: math.transform_identity(), + swing_degrees: major, lower_degrees: 0.0 - twist, upper_degrees: twist, joint_friction: joint_friction, + density: 1.0, has_foot: false, foot: no_foot(), foot_friction: 0.0, cone_axis: math.vec3(sin(t), 0.0, cos(t)), + swing_minor_degrees: minor, cone_major_axis: math.vec3_zero() } +} + +// A rotation looking down `line` (its z), its x the part of `hint` across it. +line_frame(line: Vec3, hint: Vec3) -> Quat { + z = math.normalize(line) + x = math.normalize(across(hint, z)) + return math.make_quat_from_matrix(Matrix3 { cx: x, cy: math.cross(z, x), cz: z }) +} + +// The figure's up (the pelvis's joint to the neck's) and forward (the +// way its toes point, across the up), measured on the shape as it stands. +figure_up(shape: ptr) -> Vec3 { return math.normalize(math.sub(human_shape_head(shape, BONE_NECK), human_shape_head(shape, BONE_PELVIS))) } + +figure_forward(shape: ptr, up: Vec3) -> Vec3 { + toes = math.add(math.sub(human_shape_toe(shape, BONE_CALF_L), human_shape_tail(shape, BONE_CALF_L)), + math.sub(human_shape_toe(shape, BONE_CALF_R), human_shape_tail(shape, BONE_CALF_R))) + return math.normalize(across(toes, up)) +} + +// Away from the figure's middle line at `point`, across its up and forward. +out_from_middle(shape: ptr, point: Vec3, up: Vec3, forward: Vec3) -> Vec3 { + return across(across(math.sub(point, human_shape_head(shape, BONE_PELVIS)), up), forward) +} + +// A capsule's end nearer its body's origin, and the farther. +near_end(c: Capsule) -> Vec3 { + if math.length(c.center2) < math.length(c.center1) { return c.center2 } + return c.center1 +} + +far_end(c: Capsule) -> Vec3 { + if math.length(c.center2) > math.length(c.center1) { return c.center2 } + return c.center1 +} + +// The extra bones made from the twelve, where a person has them: +// the head above the neck, its joint at the near end of the neck's +// capsule (which carries the head on the reference's figure), its own +// capsule the head's part of it; +// each clavicle from the sternoclavicular joint (in from the shoulder, +// a little below and ahead of it, in proportion to the shoulders' +// width) to the shoulder; +// each hand from the wrist, four fifths along the forearm's capsule +// line, as long as a hand is to a forearm of that length; +// spine_02 halfway between the spine bodies' joints, as wide as the +// lumbar body it splits with spine_01. +extra_specs(shape: ptr) { + up = figure_up(shape) + forward = figure_forward(shape, up) + neck = spec_at(shape, BONE_NECK) + near = near_end(neck.capsule) + far = far_end(neck.capsule) + length = math.length(math.sub(far, near)) + head_frame = Transform { p: math.transform_point(neck.reference, near), q: line_frame(math.rotate_vector(neck.reference.q, math.sub(far, near)), forward) } + set_spec(shape, BONE_HEAD, extra_spec(BONE_NECK, head_frame, + manifold.capsule(math.vec3(0.0, 0.0, 0.24 * length), math.vec3(0.0, 0.0, length), 0.91 * neck.capsule.radius), 0, neck.friction, 1.0, + 0.5 * (HEAD_FLEXION_DEGREES - HEAD_EXTENSION_DEGREES), 0.5 * (HEAD_FLEXION_DEGREES + HEAD_EXTENSION_DEGREES), HEAD_TILT_DEGREES, HEAD_TURN_DEGREES)) + clavicle_spec(shape, BONE_CLAVICLE_L, BONE_UPPER_ARM_L, up, forward) + clavicle_spec(shape, BONE_CLAVICLE_R, BONE_UPPER_ARM_R, up, forward) + hand_spec(shape, BONE_HAND_L, BONE_LOWER_ARM_L, up, forward) + hand_spec(shape, BONE_HAND_R, BONE_LOWER_ARM_R, up, forward) + lumbar = spec_at(shape, BONE_SPINE_01) + chest = spec_at(shape, BONE_SPINE_03) + line = math.sub(chest.reference.p, lumbar.reference.p) + span = math.length(line) + middle = Transform { p: math.mul_add(lumbar.reference.p, 0.5, line), q: line_frame(line, forward) } + set_spec(shape, BONE_SPINE_02, extra_spec(BONE_SPINE_01, middle, + manifold.capsule(math.vec3(0.0, 0.12 * span, 0.32 * span), math.vec3(0.0, 0.0 - 0.12 * span, 0.32 * span), 0.45 * span), lumbar.group, lumbar.friction, 1.0, + 0.0, 0.5 * chest.swing_degrees, 0.0, 0.5 * chest.upper_degrees)) + i = BONE_HEAD + while i < MAX_BONE_COUNT { + rest_frames(shape, i) + i = i + 1 + } +} + +clavicle_spec(shape: ptr, index: int, arm_index: int, up: Vec3, forward: Vec3) { + shoulder = human_shape_head(shape, arm_index) + out = out_from_middle(shape, shoulder, up, forward) + half_width = math.length(out) + side = math.mul_sv(1.0 / half_width, out) + joint = math.add(math.sub(math.sub(shoulder, math.mul_sv(0.84 * half_width, side)), math.mul_sv(0.21 * half_width, up)), math.mul_sv(0.27 * half_width, forward)) + line = math.sub(shoulder, joint) + length = math.length(line) + set_spec(shape, index, extra_spec(BONE_SPINE_03, Transform { p: joint, q: line_frame(line, up) }, + manifold.capsule(math.vec3(0.0, 0.0, 0.12 * length), math.vec3(0.0, 0.0, 0.82 * length), 0.3 * length), 0, spec_at(shape, arm_index).friction, 1.0, + 0.5 * (CLAVICLE_ELEVATION_DEGREES - CLAVICLE_DEPRESSION_DEGREES), 0.5 * (CLAVICLE_ELEVATION_DEGREES + CLAVICLE_DEPRESSION_DEGREES), + CLAVICLE_PROTRACTION_DEGREES, CLAVICLE_ROTATION_DEGREES)) + // The shoulder girdle it stands for (the clavicle, the scapula and the + // muscles over them), not the bone alone: a body light against the arm + // it carries leaves the solver too little to hold the shoulder by. + spec_at(shape, index).density = CLAVICLE_DENSITY +} + +hand_spec(shape: ptr, index: int, forearm_index: int, up: Vec3, forward: Vec3) { + forearm = spec_at(shape, forearm_index) + far = far_end(forearm.capsule) + reach = math.length(far) + wrist = math.transform_point(forearm.reference, math.mul_sv(0.8, far)) + palm = math.neg(out_from_middle(shape, wrist, up, forward)) + set_spec(shape, index, extra_spec(forearm_index, Transform { p: wrist, q: line_frame(math.rotate_vector(forearm.reference.q, far), palm) }, + manifold.capsule(math.vec3(0.0, 0.0, 0.096 * reach), math.vec3(0.0, 0.0, 0.35 * reach), 0.67 * forearm.capsule.radius), 0, forearm.friction, forearm.joint_friction, + 0.5 * (WRIST_FLEXION_DEGREES - WRIST_EXTENSION_DEGREES), 0.5 * (WRIST_FLEXION_DEGREES + WRIST_EXTENSION_DEGREES), WRIST_DEVIATION_DEGREES, WRIST_TWIST_DEGREES)) +} + +// An extra bone's joint frames from its frame and its parent's: the +// bone's own frame on both sides, so the joint is one at rest and its +// cone and twist read from there. +rest_frames(shape: ptr, index: int) { + spec = spec_at(shape, index) + spec.frame_b = math.transform_identity() + spec.frame_a = math.inv_mul_transforms(spec_at(shape, spec.parent).reference, spec.reference) +} + +// Whether a bone index is one of the extra bones. +is_extra(index: int) -> bool { return index >= BONE_HEAD && index < MAX_BONE_COUNT } + // --- a shape's landmarks, and a shape fitted to a rig's -------------------------------------------- // A bone's next along the chain the figure is measured by -- the spine up @@ -313,7 +549,18 @@ human_shape_head(shape: ptr, index: int) -> Vec3 { return spec_at(shape, index). // joint along the chain, or for a bone that ends one the end of its own // capsule farther from its joint: a calf's at the ankle, a forearm's // past the wrist (it carries the hand), the neck's at the head's middle. +// An extra bone's: a clavicle's at the shoulder, spine_02's at the +// chest's joint, the head's at its crown and a hand's at its tip (its +// capsule's far end and cap). human_shape_tail(shape: ptr, index: int) -> Vec3 { + if index == BONE_CLAVICLE_L { return spec_at(shape, BONE_UPPER_ARM_L).reference.p } + if index == BONE_CLAVICLE_R { return spec_at(shape, BONE_UPPER_ARM_R).reference.p } + if index == BONE_SPINE_02 { return spec_at(shape, BONE_SPINE_03).reference.p } + if index == BONE_HEAD || index == BONE_HAND_L || index == BONE_HAND_R { + extra = spec_at(shape, index) + end = far_end(extra.capsule) + return math.transform_point(extra.reference, math.mul_add(end, extra.capsule.radius, math.normalize(end))) + } child = human_chain_child(index) if child >= 0 { return spec_at(shape, child).reference.p } spec = spec_at(shape, index) @@ -362,7 +609,24 @@ const LINE_MIN = 0.005 // about the shin so its toe points the rig's way. Each joint's frame on // the parent turns with the bone, so the joint's rest angle -- what its // spring and its limits are taken from -- is what it was. +// +// The extra bones ride their parents: each keeps its place and turn on +// its parent, its offsets scaled as the parent's line was, its radius as +// the figure was; a clavicle then reaches to the fitted shoulder and +// spine_02 sits halfway up the fitted spine. A rig that has those bones +// puts each on its own with human_fit_bone, after this. human_fit_shape(shape: ptr, heads: ptr, tails: ptr, toes: ptr) { + on_parents = basics.alloc(MAX_BONE_COUNT * sizeof(Transform)) + on_parent = on_parents as Transform[] + extra_lengths = basics.alloc(MAX_BONE_COUNT * 8) + extra_length = extra_lengths as float[] + e = BONE_HEAD + while e < MAX_BONE_COUNT { + extra = spec_at(shape, e) + on_parent[e] = math.inv_mul_transforms(spec_at(shape, extra.parent).reference, extra.reference) + extra_length[e] = math.length(math.sub(human_shape_tail(shape, e), extra.reference.p)) + e = e + 1 + } head_at = heads as Vec3[] tail_at = tails as Vec3[] turns = basics.alloc(BONE_COUNT * sizeof(Quat)) @@ -449,6 +713,27 @@ human_fit_shape(shape: ptr, heads: ptr, tails: ptr, toes: ptr) { } i = i + 1 } + e = BONE_HEAD + while e < MAX_BONE_COUNT { + extra = spec_at(shape, e) + held = on_parent[e] + extra.reference = math.mul_transforms(spec_at(shape, extra.parent).reference, Transform { p: math.mul_sv(scale[extra.parent], held.p), q: held.q }) + extra.capsule = manifold.capsule(math.mul_sv(scale[extra.parent], extra.capsule.center1), math.mul_sv(scale[extra.parent], extra.capsule.center2), extra.capsule.radius * figure) + e = e + 1 + } + aim_extra(shape, BONE_CLAVICLE_L, human_shape_head(shape, BONE_CLAVICLE_L), human_shape_head(shape, BONE_UPPER_ARM_L), + extra_length[BONE_CLAVICLE_L] * scale[BONE_SPINE_03]) + aim_extra(shape, BONE_CLAVICLE_R, human_shape_head(shape, BONE_CLAVICLE_R), human_shape_head(shape, BONE_UPPER_ARM_R), + extra_length[BONE_CLAVICLE_R] * scale[BONE_SPINE_03]) + aim_extra(shape, BONE_SPINE_02, math.mul_sv(0.5, math.add(human_shape_head(shape, BONE_SPINE_01), human_shape_head(shape, BONE_SPINE_03))), + human_shape_head(shape, BONE_SPINE_03), extra_length[BONE_SPINE_02] * scale[BONE_SPINE_01]) + e = BONE_HEAD + while e < MAX_BONE_COUNT { + rest_frames(shape, e) + e = e + 1 + } + basics.free_bytes(on_parents, MAX_BONE_COUNT * sizeof(Transform)) + basics.free_bytes(extra_lengths, MAX_BONE_COUNT * 8) basics.free_bytes(turns, BONE_COUNT * sizeof(Quat)) basics.free_bytes(olds, BONE_COUNT * sizeof(Quat)) basics.free_bytes(units, BONE_COUNT * sizeof(Vec3)) @@ -456,8 +741,35 @@ human_fit_shape(shape: ptr, heads: ptr, tails: ptr, toes: ptr) { basics.free_bytes(bends, BONE_COUNT * 8) } +// An extra bone put on a rig's: its joint at `head` and its line turned to +// run to `tail` (in the figure's space, landmarks as human_shape_head and +// human_shape_tail name them: a clavicle's tail is the shoulder, a hand's +// its tip), its capsule stretched along the line to the rig's length. +// Its joint's frames follow; its limits are held against its rest, so +// they turn with it. Nothing happens for the reference's bones (fitted by +// human_fit_shape, which is called first: it moves the extras with their +// parents) or for lines too short to have a direction. +human_fit_bone(shape: ptr, index: int, head: Vec3, tail: Vec3) { + if is_extra(index) == false { return } + aim_extra(shape, index, head, tail, math.length(math.sub(human_shape_tail(shape, index), human_shape_head(shape, index)))) +} + +// An extra bone's line is its frame's z, `length` long as it stands (a +// clavicle's or spine_02's tail is another bone's joint, which a fit may +// have moved already, so the caller says how long the bone is). +aim_extra(shape: ptr, index: int, head: Vec3, tail: Vec3, length: float) { + spec = spec_at(shape, index) + fitted = math.sub(tail, head) + fitted_length = math.length(fitted) + if length <= LINE_MIN || fitted_length <= LINE_MIN { return } + turn = math.compute_quat_between_unit_vectors(math.rotate_vector(spec.reference.q, math.vec3_axis_z()), math.mul_sv(1.0 / fitted_length, fitted)) + spec.capsule = stretch(spec.capsule, math.vec3_axis_z(), fitted_length / length, 1.0) + spec.reference = Transform { p: head, q: math.normalize_quat(math.mul_quat(turn, spec.reference.q)) } + rest_frames(shape, index) +} + // A figure's shape given back (human_reference_shape's block). -destroy_shape(shape: ptr) { basics.free_bytes(shape, BONE_COUNT * sizeof(BoneSpec)) } +destroy_shape(shape: ptr) { basics.free_bytes(shape, MAX_BONE_COUNT * sizeof(BoneSpec)) } cone_off_axis(axis: Vec3) -> bool { return axis.x != 0.0 || axis.y != 0.0 || axis.z <= 0.0 } @@ -774,9 +1086,10 @@ create_human_shaped(h: *Human, world_id: WorldId, placement: Transform, shape: p // Ankles need a foot on each calf to stand on. if h.ankles && (spec_at(shape, BONE_CALF_L).has_foot == false || spec_at(shape, BONE_CALF_R).has_foot == false) { return } h.bone_count = BONE_COUNT - if h.ankles { h.bone_count = MAX_BONE_COUNT } + if h.ankles { h.bone_count = ANKLED_BONE_COUNT } + if has_extras(h) { h.bone_count = MAX_BONE_COUNT } h.bones = basics.alloc(h.bone_count * sizeof(HumanBone)) - h.filter_joints = basics.alloc(FILTER_JOINT_COUNT * sizeof(JointId)) + h.filter_joints = basics.alloc(MAX_FILTER_JOINT_COUNT * sizeof(JointId)) h.filter_joint_count = 0 i = 0 while i < h.bone_count { @@ -796,85 +1109,212 @@ create_human_shaped(h: *Human, world_id: WorldId, placement: Transform, shape: p } filters = h.filter_joints as JointId[] i = 0 - while i < FILTER_JOINT_COUNT { + while i < MAX_FILTER_JOINT_COUNT { filters[i] = dynamics.null_joint_id() i = i + 1 } h.friction_torque = friction_torque + specs = shape as BoneSpec[] i = 0 while i < BONE_COUNT { - place_bone(h, world_id, i, placement, spec_at(shape, i), user_data, group_index, h.ankles == false) + spec = specs[i] + spec.capsule = given_up_capsule(h, i, spec.capsule) + place_bone(h, world_id, i, placement, &spec, user_data, group_index, h.ankles == false) i = i + 1 } if h.ankles { place_foot(h, world_id, BONE_FOOT_L, BONE_CALF_L, placement, spec_at(shape, BONE_CALF_L), user_data, group_index) place_foot(h, world_id, BONE_FOOT_R, BONE_CALF_R, placement, spec_at(shape, BONE_CALF_R), user_data, group_index) } + i = BONE_HEAD + while i < MAX_BONE_COUNT { + if asks_for_bone(h, i) { place_bone(h, world_id, i, placement, spec_at(shape, i), user_data, group_index, true) } + i = i + 1 + } // A bone the world refused (a full world, a shape spec out of range) // would leave a figure missing a limb that every later call reaches // for: what was made is taken down and the figure stays unspawned. i = 0 while i < h.bone_count { - if dynamics.body_is_valid(bone_at(h, i).body_id) == false { + if asks_for_bone(h, i) && dynamics.body_is_valid(bone_at(h, i).body_id) == false { take_down(h) return } i = i + 1 } + // The extra bones between the reference's: the upper arms hang from + // the clavicles, the chest rides spine_02 (the two sharing the chest + // joint's range), and the twisting hinges take the elbows and knees. + if h.clavicles { + hang_from(h, shape, BONE_UPPER_ARM_L, BONE_CLAVICLE_L) + hang_from(h, shape, BONE_UPPER_ARM_R, BONE_CLAVICLE_R) + } + if h.spine_02 { + hang_from(h, shape, BONE_SPINE_03, BONE_SPINE_02) + chest = bone_at(h, BONE_SPINE_03) + chest.swing_limit = 0.5 * chest.swing_limit + chest.swing_minor_limit = 0.5 * chest.swing_minor_limit + chest.twist_limit = Vec2 { x: 0.5 * chest.twist_limit.x, y: 0.5 * chest.twist_limit.y } + } + if h.twist { + twisting_hinge(h, shape, BONE_LOWER_ARM_L, ELBOW_TWIST_DEGREES) + twisting_hinge(h, shape, BONE_LOWER_ARM_R, ELBOW_TWIST_DEGREES) + twisting_hinge(h, shape, BONE_CALF_L, KNEE_TWIST_DEGREES) + twisting_hinge(h, shape, BONE_CALF_R, KNEE_TWIST_DEGREES) + } // The joints: a spring toward the reference pose when asked, the // motor's torque limit as friction, the limits as the rig allows. i = 1 while i < h.bone_count { - bone = bone_at(h, i) - parent = bone_at(h, bone.parent_index) - bone.local_frame_a.q = math.normalize_quat(bone.local_frame_a.q) - bone.local_frame_b.q = math.normalize_quat(bone.local_frame_b.q) - if bone.joint_kind == dynamics.JOINT_REVOLUTE { - rdef = dynamics.default_revolute_joint_def() - rdef.base.body_id_a = parent.body_id - rdef.base.body_id_b = bone.body_id - rdef.base.local_frame_a = bone.local_frame_a - rdef.base.local_frame_b = bone.local_frame_b - rdef.enable_limit = true - rdef.lower_angle = bone.twist_limit.x - rdef.upper_angle = bone.twist_limit.y - rdef.enable_spring = hertz > 0.0 - rdef.hertz = hertz - rdef.damping_ratio = damping_ratio - rdef.enable_motor = true - rdef.max_motor_torque = bone.joint_friction * friction_torque - bone.joint_id = dynamics.create_revolute_joint(world_id, &rdef) - } else { - sdef = dynamics.default_spherical_joint_def() - sdef.base.body_id_a = parent.body_id - sdef.base.body_id_b = bone.body_id - sdef.base.local_frame_a = bone.local_frame_a - sdef.base.local_frame_b = bone.local_frame_b - sdef.enable_cone_limit = true - sdef.cone_angle = bone.swing_limit - sdef.cone_axis = bone.cone_axis - sdef.cone_minor_angle = bone.swing_minor_limit - sdef.cone_major_axis = bone.cone_major_axis - sdef.enable_twist_limit = true - sdef.lower_twist_angle = bone.twist_limit.x - sdef.upper_twist_angle = bone.twist_limit.y - sdef.enable_spring = hertz > 0.0 - sdef.hertz = hertz - sdef.damping_ratio = damping_ratio - sdef.enable_motor = true - sdef.max_motor_torque = bone.joint_friction * friction_torque - bone.joint_id = dynamics.create_spherical_joint(world_id, &sdef) - } + if asks_for_bone(h, i) { make_joint(h, world_id, i, hertz, damping_ratio, friction_torque) } i = i + 1 } - // The thighs pass each other and the head the shoulders. + // The thighs pass each other and the head the shoulders; with the + // extra bones, the head the chest and the shoulders, the clavicles + // each other and the neck, and each bone the one it no longer hangs + // from or was set between (hang_from). add_filter(h, world_id, BONE_THIGH_L, BONE_THIGH_R) add_filter(h, world_id, BONE_NECK, BONE_UPPER_ARM_L) add_filter(h, world_id, BONE_NECK, BONE_UPPER_ARM_R) + if h.head { + add_filter(h, world_id, BONE_HEAD, BONE_SPINE_03) + add_filter(h, world_id, BONE_HEAD, BONE_UPPER_ARM_L) + add_filter(h, world_id, BONE_HEAD, BONE_UPPER_ARM_R) + } + if h.clavicles { + add_filter(h, world_id, BONE_CLAVICLE_L, BONE_CLAVICLE_R) + add_filter(h, world_id, BONE_CLAVICLE_L, BONE_NECK) + add_filter(h, world_id, BONE_CLAVICLE_R, BONE_NECK) + add_filter(h, world_id, BONE_UPPER_ARM_L, BONE_SPINE_03) + add_filter(h, world_id, BONE_UPPER_ARM_R, BONE_SPINE_03) + if h.head { + add_filter(h, world_id, BONE_HEAD, BONE_CLAVICLE_L) + add_filter(h, world_id, BONE_HEAD, BONE_CLAVICLE_R) + } + } + if h.spine_02 { + add_filter(h, world_id, BONE_SPINE_01, BONE_SPINE_03) + add_filter(h, world_id, BONE_PELVIS, BONE_SPINE_02) + } h.is_spawned = true } +// A bone's capsule as built: the reference's, but the neck gives the head +// to a head bone (keeping a short neck below it, three quarters of the +// head's girth: a person's neck carries the head, so it is no twig), +// a forearm the hand to a hand bone (ending at the wrist's side of the +// hand), and spine_01 the upper lumbar to spine_02 (keeping the lower, +// of a girth that holds the trunk's mass). +given_up_capsule(h: *Human, index: int, c: Capsule) -> Capsule { + if h.head && index == BONE_NECK { + near = near_end(c) + return manifold.capsule(math.mul_sv(0.2, near), math.mul_sv(0.7, near), 0.75 * c.radius) + } + if h.hands && (index == BONE_LOWER_ARM_L || index == BONE_LOWER_ARM_R) { + if math.length(c.center2) > math.length(c.center1) { return manifold.capsule(c.center1, math.mul_sv(0.67, c.center2), c.radius) } + return manifold.capsule(math.mul_sv(0.67, c.center1), c.center2, c.radius) + } + if h.spine_02 && index == BONE_SPINE_01 { + return manifold.capsule(math.mul_sv(0.48, c.center1), math.mul_sv(0.48, c.center2), 0.79 * c.radius) + } + return c +} + +// A bone's joint moved to a new parent between it and its old one: its +// frame on the parent re-read on the new one, the same frame in the +// figure, so its rest, cone and twist are what they were. +hang_from(h: *Human, shape: ptr, index: int, parent_index: int) { + bone = bone_at(h, index) + in_figure = math.mul_transforms(spec_at(shape, bone.parent_index).reference, bone.local_frame_a) + bone.local_frame_a = math.inv_mul_transforms(spec_at(shape, parent_index).reference, in_figure) + bone.parent_index = parent_index +} + +// A hinge that twists (#190): the elbow or knee as a spherical joint whose +// cone is the hinge's range of bend -- a circle about the middle of the +// bend from its lower angle to its upper, its minor locked so the bone +// swings only along the bend's arc (joint_solver.spherical_joint_lock_cone_minor, +// as UE5's locked swing; an ellipse that narrow chatters) -- and whose +// twist is the bone turning about itself, `twist_degrees` either way. +// The joint's frames are one at rest: z the bone's line across the hinge's +// axis (so bending sweeps z along a great circle), y the hinge's axis. +twisting_hinge(h: *Human, shape: ptr, index: int, twist_degrees: float) { + spec = spec_at(shape, index) + parent = spec_at(shape, spec.parent) + axis = hinge_axis(shape, index) + z = math.normalize(across(math.sub(human_shape_tail(shape, index), human_shape_head(shape, index)), axis)) + frame = math.make_quat_from_matrix(Matrix3 { cx: math.cross(axis, z), cy: axis, cz: z }) + // The hinge's limits are joint angles, and the reference's frames do + // not meet at the rest: the range against the rest is the limits less + // the angle there. + at_rest = math.get_twist_angle(math.inv_mul_quat(math.mul_quat(parent.reference.q, math.normalize_quat(spec.frame_a.q)), + math.mul_quat(spec.reference.q, math.normalize_quat(spec.frame_b.q)))) + lower = spec.lower_degrees * math.DEG_TO_RAD + upper = spec.upper_degrees * math.DEG_TO_RAD + middle = 0.5 * (lower + upper) - at_rest + bone = bone_at(h, index) + bone.joint_kind = dynamics.JOINT_SPHERICAL + bone.local_frame_a = Transform { p: spec.frame_a.p, q: math.inv_mul_quat(parent.reference.q, frame) } + bone.local_frame_b = Transform { p: spec.frame_b.p, q: math.inv_mul_quat(spec.reference.q, frame) } + bone.cone_axis = math.vec3(sin(middle), 0.0, cos(middle)) + bone.cone_major_axis = math.vec3(cos(middle), 0.0, 0.0 - sin(middle)) + bone.swing_limit = 0.5 * (upper - lower) + bone.swing_minor_limit = 0.0 + bone.twist_limit = Vec2 { x: 0.0 - twist_degrees * math.DEG_TO_RAD, y: twist_degrees * math.DEG_TO_RAD } +} + +// A bone's joint to its parent: a spring toward the reference pose when +// asked, the motor's torque limit as friction, the limits as the rig allows. +make_joint(h: *Human, world_id: WorldId, i: int, hertz: float, damping_ratio: float, friction_torque: float) { + bone = bone_at(h, i) + parent = bone_at(h, bone.parent_index) + bone.local_frame_a.q = math.normalize_quat(bone.local_frame_a.q) + bone.local_frame_b.q = math.normalize_quat(bone.local_frame_b.q) + if bone.joint_kind == dynamics.JOINT_REVOLUTE { + rdef = dynamics.default_revolute_joint_def() + rdef.base.body_id_a = parent.body_id + rdef.base.body_id_b = bone.body_id + rdef.base.local_frame_a = bone.local_frame_a + rdef.base.local_frame_b = bone.local_frame_b + rdef.enable_limit = true + rdef.lower_angle = bone.twist_limit.x + rdef.upper_angle = bone.twist_limit.y + rdef.enable_spring = hertz > 0.0 + rdef.hertz = hertz + rdef.damping_ratio = damping_ratio + rdef.enable_motor = true + rdef.max_motor_torque = bone.joint_friction * friction_torque + bone.joint_id = dynamics.create_revolute_joint(world_id, &rdef) + } else { + sdef = dynamics.default_spherical_joint_def() + sdef.base.body_id_a = parent.body_id + sdef.base.body_id_b = bone.body_id + sdef.base.local_frame_a = bone.local_frame_a + sdef.base.local_frame_b = bone.local_frame_b + sdef.enable_cone_limit = true + sdef.cone_angle = bone.swing_limit + sdef.cone_axis = bone.cone_axis + sdef.cone_minor_angle = bone.swing_minor_limit + sdef.cone_major_axis = bone.cone_major_axis + sdef.enable_twist_limit = true + sdef.lower_twist_angle = bone.twist_limit.x + sdef.upper_twist_angle = bone.twist_limit.y + sdef.enable_spring = hertz > 0.0 + sdef.hertz = hertz + sdef.damping_ratio = damping_ratio + sdef.enable_motor = true + sdef.max_motor_torque = bone.joint_friction * friction_torque + bone.joint_id = dynamics.create_spherical_joint(world_id, &sdef) + if h.twist && is_hinge_bone(i) { joint_solver.spherical_joint_lock_cone_minor(bone.joint_id, true) } + } +} + +// The elbows and knees, the hinges a twisting figure turns into twisting hinges. +is_hinge_bone(index: int) -> bool { + return index == BONE_LOWER_ARM_L || index == BONE_LOWER_ARM_R || index == BONE_CALF_L || index == BONE_CALF_R +} + add_filter(h: *Human, world_id: WorldId, a: int, b: int) { fdef = dynamics.default_filter_joint_def() fdef.base.body_id_a = bone_at(h, a).body_id @@ -915,7 +1355,7 @@ take_down(h: *Human) { i = i + 1 } basics.free_bytes(h.bones, h.bone_count * sizeof(HumanBone)) - basics.free_bytes(h.filter_joints, FILTER_JOINT_COUNT * sizeof(JointId)) + basics.free_bytes(h.filter_joints, MAX_FILTER_JOINT_COUNT * sizeof(JointId)) h.bones = null h.filter_joints = null h.filter_joint_count = 0 @@ -944,9 +1384,9 @@ human_set_joint_friction_torque(h: *Human, torque: float) { i = 1 while i < h.bone_count { bone = bone_at(h, i) - if bone.joint_kind == dynamics.JOINT_REVOLUTE { + if human_has_bone(h, i) && bone.joint_kind == dynamics.JOINT_REVOLUTE { joint_solver.revolute_joint_set_max_motor_torque(bone.joint_id, bone.joint_friction * torque) - } else { + } else if human_has_bone(h, i) { joint_solver.spherical_joint_set_max_motor_torque(bone.joint_id, bone.joint_friction * torque) } i = i + 1 @@ -958,9 +1398,9 @@ human_set_joint_spring_hertz(h: *Human, hertz: float) { i = 1 while i < h.bone_count { bone = bone_at(h, i) - if bone.joint_kind == dynamics.JOINT_REVOLUTE { + if human_has_bone(h, i) && bone.joint_kind == dynamics.JOINT_REVOLUTE { joint_solver.revolute_joint_set_spring_hertz(bone.joint_id, hertz) - } else { + } else if human_has_bone(h, i) { joint_solver.spherical_joint_set_spring_hertz(bone.joint_id, hertz) } i = i + 1 @@ -972,9 +1412,9 @@ human_set_joint_damping_ratio(h: *Human, damping_ratio: float) { i = 1 while i < h.bone_count { bone = bone_at(h, i) - if bone.joint_kind == dynamics.JOINT_REVOLUTE { + if human_has_bone(h, i) && bone.joint_kind == dynamics.JOINT_REVOLUTE { joint_solver.revolute_joint_set_spring_damping_ratio(bone.joint_id, damping_ratio) - } else { + } else if human_has_bone(h, i) { joint_solver.spherical_joint_set_spring_damping_ratio(bone.joint_id, damping_ratio) } i = i + 1 @@ -1028,14 +1468,16 @@ human_create_motor_anchors(h: *Human, world_id: WorldId) { mdef.max_spring_torque = math.MAX_FLOAT i = 0 while i < h.bone_count { - bone = bone_at(h, i) - xf = dynamics.body_get_transform(bone.body_id) - adef.position = xf.p - adef.rotation = xf.q - bone.anchor_id = dynamics.create_body(world_id, &adef) - mdef.base.body_id_a = bone.anchor_id - mdef.base.body_id_b = bone.body_id - bone.anchor_joint_id = dynamics.create_motor_joint(world_id, &mdef) + if human_has_bone(h, i) { + bone = bone_at(h, i) + xf = dynamics.body_get_transform(bone.body_id) + adef.position = xf.p + adef.rotation = xf.q + bone.anchor_id = dynamics.create_body(world_id, &adef) + mdef.base.body_id_a = bone.anchor_id + mdef.base.body_id_b = bone.body_id + bone.anchor_joint_id = dynamics.create_motor_joint(world_id, &mdef) + } i = i + 1 } } @@ -1054,17 +1496,19 @@ human_create_parallel_anchors(h: *Human, world_id: WorldId) { pdef.max_torque = 800.0 i = 0 while i < h.bone_count { - bone = bone_at(h, i) - xf = dynamics.body_get_transform(bone.body_id) - adef.position = xf.p - adef.rotation = xf.q - bone.anchor_id = dynamics.create_body(world_id, &adef) - pdef.base.body_id_a = bone.anchor_id - pdef.base.body_id_b = bone.body_id - frame_q = math.inv_mul_quat(xf.q, q_frame_world) - pdef.base.local_frame_a.q = frame_q - pdef.base.local_frame_b.q = frame_q - bone.anchor_joint_id = dynamics.create_parallel_joint(world_id, &pdef) + if human_has_bone(h, i) { + bone = bone_at(h, i) + xf = dynamics.body_get_transform(bone.body_id) + adef.position = xf.p + adef.rotation = xf.q + bone.anchor_id = dynamics.create_body(world_id, &adef) + pdef.base.body_id_a = bone.anchor_id + pdef.base.body_id_b = bone.body_id + frame_q = math.inv_mul_quat(xf.q, q_frame_world) + pdef.base.local_frame_a.q = frame_q + pdef.base.local_frame_b.q = frame_q + bone.anchor_joint_id = dynamics.create_parallel_joint(world_id, &pdef) + } i = i + 1 } } @@ -1107,7 +1551,7 @@ human_set_bullet(h: *Human, flag: bool) { if h.is_spawned == false { return } i = 0 while i < h.bone_count { - dynamics.body_set_bullet(bone_at(h, i).body_id, flag) + if human_has_bone(h, i) { dynamics.body_set_bullet(bone_at(h, i).body_id, flag) } i = i + 1 } } diff --git a/aephysics/joint_solver/module.ae b/aephysics/joint_solver/module.ae index 1552ae3..eeb2f9a 100644 --- a/aephysics/joint_solver/module.ae +++ b/aephysics/joint_solver/module.ae @@ -64,7 +64,7 @@ exports ( revolute_joint_get_max_motor_torque, revolute_joint_get_motor_torque, spherical_joint_enable_cone_limit, spherical_joint_is_cone_limit_enabled, spherical_joint_get_cone_limit, spherical_joint_set_cone_limit, spherical_joint_get_cone_angle, spherical_joint_set_cone_axis, spherical_joint_get_cone_axis, - spherical_joint_set_cone_ellipse, spherical_joint_get_cone_minor_angle, spherical_joint_get_cone_major_axis, spherical_joint_enable_twist_limit, spherical_joint_is_twist_limit_enabled, + spherical_joint_set_cone_ellipse, spherical_joint_lock_cone_minor, spherical_joint_is_cone_minor_locked, spherical_joint_get_cone_minor_angle, spherical_joint_get_cone_major_axis, spherical_joint_enable_twist_limit, spherical_joint_is_twist_limit_enabled, spherical_joint_get_lower_twist_limit, spherical_joint_get_upper_twist_limit, spherical_joint_set_twist_limits, spherical_joint_get_twist_angle, spherical_joint_enable_spring, spherical_joint_is_spring_enabled, spherical_joint_set_target_rotation, spherical_joint_get_target_rotation, spherical_joint_set_spring_hertz, spherical_joint_get_spring_hertz, @@ -1668,6 +1668,15 @@ prepare_spherical_joint(base: *JointSim, ctx: *StepContext) { if k > 0.0 { j.twist_mass = 1.0 / k } j.twist_jacobian = jacobian } + // The minor lock: C = B's z along the cone's minor axis on A, whose + // rate is (w_b - w_a) along B's z crossed with that axis. + if j.lock_cone_minor { + lock_axis = math.cross(twist_axis, math.rotate_vector(j.frame_a.q, dynamics.spherical_cone_minor_axis(j))) + k = math.dot(lock_axis, math.mul_mv(inertia_sum, lock_axis)) + j.lock_mass = 0.0 + if k > 0.0 { j.lock_mass = 1.0 / k } + j.lock_axis = lock_axis + } if base.fixed_rotation == false { j.rotation_mass = math.invert_matrix(inertia_sum) } else { j.rotation_mass = math.mat3_zero() } j.spring_softness = dynamics.make_soft(j.hertz, j.damping_ratio, ctx.h) // A spring off or of no stiffness computes no impulse; its last one is @@ -1680,6 +1689,7 @@ prepare_spherical_joint(base: *JointSim, ctx: *StepContext) { j.swing_impulse = 0.0 j.lower_twist_impulse = 0.0 j.upper_twist_impulse = 0.0 + j.lock_impulse = 0.0 } } @@ -1692,7 +1702,7 @@ warm_start_spherical_joint(base: *JointSim, ctx: *StepContext) { // The angular impulses are each zeroed when their part is off, so a // plain point (no spring, motor or limit) has none to give back. angular = math.vec3_zero() - if j.enable_spring || j.enable_motor || j.enable_cone_limit || j.enable_twist_limit { angular = spherical_angular_impulse(j) } + if j.enable_spring || j.enable_motor || j.enable_cone_limit || j.enable_twist_limit || j.lock_cone_minor { angular = spherical_angular_impulse(j) } v_a = math.mul_sub(math.widen3(state_a.v), base.inv_mass_a, j.linear_impulse) w_a = math.sub(math.widen3(state_a.w), math.mul_mv(math.widen_mat3(base.inv_inertia_a), math.add(math.cross(r_a, j.linear_impulse), angular))) v_b = math.mul_add(math.widen3(state_b.v), base.inv_mass_b, j.linear_impulse) @@ -1774,6 +1784,23 @@ solve_spherical_joint(base: *JointSim, ctx: *StepContext, use_bias: bool) { w_a = math.mul_add(w_a, delta, math.mul_mv(i_a, swing_axis)) w_b = math.mul_sub(w_b, delta, math.mul_mv(i_b, swing_axis)) } + // The minor lock, an equality softened as the point is. + if j.lock_cone_minor && fixed_rotation == false { + lock_axis = j.lock_axis + cdot = math.dot(math.sub(w_b, w_a), lock_axis) + bias = 0.0 + mass_scale = 1.0 + impulse_scale = 0.0 + if use_bias { + bias = base.constraint_softness.bias_rate * dynamics.spherical_lock_error(j, quat_a, quat_b) + mass_scale = base.constraint_softness.mass_scale + impulse_scale = base.constraint_softness.impulse_scale + } + delta = 0.0 - mass_scale * j.lock_mass * (cdot + bias) - impulse_scale * j.lock_impulse + j.lock_impulse = j.lock_impulse + delta + w_a = math.mul_sub(w_a, delta, math.mul_mv(i_a, lock_axis)) + w_b = math.mul_add(w_b, delta, math.mul_mv(i_b, lock_axis)) + } // The point. r_a = math.rotate_vector(math.widen_quat(state_a.dq), j.frame_a.p) r_b = math.rotate_vector(math.widen_quat(state_b.dq), j.frame_b.p) @@ -1811,7 +1838,9 @@ get_spherical_joint_torque(w: *World, base: *JointSim) -> Vec3 { spherical_angular_impulse(j: *SphericalJoint) -> Vec3 { angular = math.add(j.spring_impulse, j.motor_impulse) angular = math.mul_sub(angular, j.swing_impulse, j.swing_axis) - return math.mul_add(angular, j.lower_twist_impulse - j.upper_twist_impulse, j.twist_jacobian) + angular = math.mul_add(angular, j.lower_twist_impulse - j.upper_twist_impulse, j.twist_jacobian) + if j.lock_cone_minor { angular = math.mul_add(angular, j.lock_impulse, j.lock_axis) } + return angular } // The relative rotation of the frames, B's polarity matched to A's. @@ -1889,6 +1918,29 @@ spherical_joint_set_cone_ellipse(id: JointId, major_angle: float, minor_angle: f j.cone_minor_angle = dynamics.spherical_cone_minor(j, minor_angle) j.cone_major_axis = math.narrow3(major_axis) } +// A hinge that twists (#190): the cone's minor half-angle locked at zero, +// so frame B's z stays in the plane of the cone's centre and its major +// axis (an equality, as a revolute holds its axis) and swings along it as +// far as the circle of cone_angle about the centre; the twist about B's z +// is still the twist limit's. An elbow is one: it bends along its arc, +// its forearm turns about itself, and it never bends sideways. Locking +// drops an ellipse's minor angle; unlocked, the cone is a circle until an +// ellipse is set again. +spherical_joint_lock_cone_minor(id: JointId, flag: bool) { + if dynamics.recording_on(id.world0) { recorder = dynamics.rec_begin(id.world0, dynamics.OP_SPHERICAL_JOINT_LOCK_CONE_MINOR); dynamics.rec_id(recorder, id.index1, id.generation); dynamics.rec_bool(recorder, flag); dynamics.rec_end(recorder) } + base = sim_of(id, dynamics.JOINT_SPHERICAL) + if base == null { return } + j = dynamics.spherical_joint_of(base) + if flag == j.lock_cone_minor { return } + j.lock_cone_minor = flag + j.cone_minor_angle = 0.0 + j.lock_impulse = 0.0 +} +spherical_joint_is_cone_minor_locked(id: JointId) -> bool { + base = sim_of(id, dynamics.JOINT_SPHERICAL) + if base == null { return false } + return dynamics.spherical_joint_of(base).lock_cone_minor +} spherical_joint_get_cone_minor_angle(id: JointId) -> float { base = sim_of(id, dynamics.JOINT_SPHERICAL) if base == null { return 0.0 } diff --git a/aephysics/recording/module.ae b/aephysics/recording/module.ae index e404992..710db5f 100644 --- a/aephysics/recording/module.ae +++ b/aephysics/recording/module.ae @@ -2890,6 +2890,12 @@ dispatch_call(p: *Player, op: int) -> bool { if p.ok { joint_solver.spherical_joint_enable_twist_limit(a_joint_id_id, a_bool_flag) } return true } + if op == dynamics.OP_SPHERICAL_JOINT_LOCK_CONE_MINOR { + a_joint_id_id = read_joint_id(p) + a_bool_flag = read_bool(p) + if p.ok { joint_solver.spherical_joint_lock_cone_minor(a_joint_id_id, a_bool_flag) } + return true + } if op == dynamics.OP_SPHERICAL_JOINT_SET_TWIST_LIMITS { a_joint_id_id = read_joint_id(p) a_float_lower = read_float(p) diff --git a/tests/test_human.ae b/tests/test_human.ae index 9a13c8f..6ebd90c 100644 --- a/tests/test_human.ae +++ b/tests/test_human.ae @@ -873,7 +873,7 @@ test_ankles() { h = Human { bones: null, filter_joints: null, filter_joint_count: 0, friction_torque: 0.0, is_spawned: false } h.ankles = true human.create_human(&h, world_id, math.vec3(0.0, 1.0, 0.0), 5.0, 1.0, 0.7, 1, null) - ensure("ankles: spawned with 14 bones", h.is_spawned && h.bone_count == human.MAX_BONE_COUNT) + ensure("ankles: spawned with 14 bones", h.is_spawned && h.bone_count == human.ANKLED_BONE_COUNT) counters = physics_world.get_counters(world_id) ensure("ankles: 15 bodies, 16 joints (${counters.body_count}, ${counters.joint_count})", counters.body_count == 15 && counters.joint_count == 16) shapes_block = calloc(4, sizeof(ShapeId)) @@ -1185,6 +1185,457 @@ test_ankle_shins() { ensure("shins: and the shins nothing (${ankled.x})", ankled.x < 0.001) } +// --- the extra bones (#190) ------------------------------------------------------------------------------ + +// The figures as main built them before #190, each hashed after a kicked +// fall: the reference's, with ankles, and fitted to a figure a tenth +// larger on tight limits with ankles. The hashes were taken from main's +// module (ad0cae2) before the extra bones were added: every flag off, the +// figure is built and steps as it did, bit for bit. +const UNCHANGED_REFERENCE = 0 - 1845097547785975894 +const UNCHANGED_ANKLES = 6625657960679933986 +const UNCHANGED_FITTED = 0 - 3497850239973562386 + +unchanged_hash(kind: int) -> long { + world_id = world_with_ground() + h = Human { bones: null, filter_joints: null, filter_joint_count: 0, friction_torque: 0.0, is_spawned: false } + shape = human.human_reference_shape() + if kind == 2 { + block = calloc(2 * human.BONE_COUNT + 2, sizeof(Vec3)) + v = block as Vec3[] + i = 0 + while i < human.BONE_COUNT { + v[i] = math.mul_sv(1.1, human.human_shape_head(shape, i)) + v[human.BONE_COUNT + i] = math.mul_sv(1.1, human.human_shape_tail(shape, i)) + i = i + 1 + } + v[2 * human.BONE_COUNT] = math.mul_sv(1.1, human.human_shape_toe(shape, human.BONE_CALF_L)) + v[2 * human.BONE_COUNT + 1] = math.mul_sv(1.1, human.human_shape_toe(shape, human.BONE_CALF_R)) + human.human_fit_shape(shape, block, block + human.BONE_COUNT * sizeof(Vec3), block + 2 * human.BONE_COUNT * sizeof(Vec3)) + human.human_tight_limits(shape) + free(block) + } + h.ankles = kind >= 1 + human.create_human_shaped(&h, world_id, Transform { p: math.vec3(0.0, 1.2, 0.0), q: math.quat_identity() }, shape, 5.0, 1.0, 0.7, 1, null) + human.destroy_shape(shape) + human.human_apply_angular_impulse(&h, math.vec3(1.0, 2.0, 3.0)) + steps(world_id, 300) + hash = dynamics.hash_world_state(dynamics.get_world(world_id.index1 - 1)) + human.destroy_human(&h) + dynamics.destroy_world(world_id) + return hash +} + +test_unchanged() { + ensure("unchanged: the reference's figure, bit for bit", unchanged_hash(0) == UNCHANGED_REFERENCE) + ensure("unchanged: with ankles, bit for bit", unchanged_hash(1) == UNCHANGED_ANKLES) + ensure("unchanged: fitted on tight limits with ankles, bit for bit", unchanged_hash(2) == UNCHANGED_FITTED) +} + +// The flags as bits, for building every option alone and together. +const WITH_HEAD = 1 +const WITH_CLAVICLES = 2 +const WITH_HANDS = 4 +const WITH_SPINE_02 = 8 +const WITH_TWIST = 16 +const WITH_ANKLES = 32 + +set_flags(h: *Human, flags: int) { + h.head = (flags & WITH_HEAD) != 0 + h.clavicles = (flags & WITH_CLAVICLES) != 0 + h.hands = (flags & WITH_HANDS) != 0 + h.spine_02 = (flags & WITH_SPINE_02) != 0 + h.twist = (flags & WITH_TWIST) != 0 + h.ankles = (flags & WITH_ANKLES) != 0 +} + +bones_for(flags: int) -> int { + count = human.BONE_COUNT + if (flags & WITH_HEAD) != 0 { count = count + 1 } + if (flags & WITH_CLAVICLES) != 0 { count = count + 2 } + if (flags & WITH_HANDS) != 0 { count = count + 2 } + if (flags & WITH_SPINE_02) != 0 { count = count + 1 } + if (flags & WITH_ANKLES) != 0 { count = count + 2 } + return count +} + +filters_for(flags: int) -> int { + count = human.FILTER_JOINT_COUNT + if (flags & WITH_HEAD) != 0 { count = count + 3 } + if (flags & WITH_CLAVICLES) != 0 { count = count + 5 } + if (flags & WITH_HEAD) != 0 && (flags & WITH_CLAVICLES) != 0 { count = count + 2 } + if (flags & WITH_SPINE_02) != 0 { count = count + 2 } + return count +} + +// The deepest two of the figure's own bones press into each other (a +// negative separation, metres; 0 when none touch): bones that may touch +// must not start inside each other, or contact and joint fight for good. +deepest_bone_overlap(h: *Human) -> float { + block = calloc(16, sizeof(ContactData)) + data = block as ContactData[] + deepest = 0.0 + i = 0 + while i < h.bone_count { + if human.human_has_bone(h, i) { + count = dynamics.body_get_contact_data(human.bone_at(h, i).body_id, data, 16) + c = 0 + while c < count { + other_a = dynamics.shape_get_body(data[c].shape_id_a) + other_b = dynamics.shape_get_body(data[c].shape_id_b) + if is_bone_body(h, other_a) && is_bone_body(h, other_b) { + m = 0 + while m < data[c].manifold_count { + manifold = (data[c].manifolds + m * sizeof(Manifold)) as *Manifold + if manifold.point_count > 0 { deepest = math.min_float(deepest, manifold.point0.separation as float) } + m = m + 1 + } + } + c = c + 1 + } + } + i = i + 1 + } + free(block) + return deepest +} + +is_bone_body(h: *Human, id: BodyId) -> bool { + i = 0 + while i < h.bone_count { + bone_id = human.bone_at(h, i).body_id + if human.human_has_bone(h, i) && bone_id.index1 == id.index1 && bone_id.world0 == id.world0 && bone_id.generation == id.generation { return true } + i = i + 1 + } + return false +} + +// The worst of every present joint's limit excursion and separation. +struct JointWorst { + angular: float + linear: float + settled_angular: float // the worst once settled, after the drop and after the throw + settled_linear: float +} + +joint_worst(h: *Human) -> JointWorst { + worst = JointWorst { angular: 0.0, linear: 0.0, settled_angular: 0.0, settled_linear: 0.0 } + i = 1 + while i < h.bone_count { + if human.human_has_bone(h, i) { + id = human.bone_at(h, i).joint_id + worst.angular = math.max_float(worst.angular, dynamics.joint_get_angular_separation(id)) + worst.linear = math.max_float(worst.linear, dynamics.joint_get_linear_separation(id)) + } + i = i + 1 + } + return worst +} + +// The figure moving as one body: every bone at `velocity` plus `spin` +// about the pelvis. A kick or a throw given so moves figures alike +// however their mass is split (an impulse on one bone would kick a +// lighter bone harder). +move_rigidly(h: *Human, velocity: Vec3, spin: Vec3) { + centre = dynamics.body_get_position(human.bone_at(h, human.BONE_PELVIS).body_id) + i = 0 + while i < h.bone_count { + if human.human_has_bone(h, i) { + id = human.bone_at(h, i).body_id + dynamics.body_set_linear_velocity(id, math.add(velocity, math.cross(spin, math.sub(dynamics.body_get_position(id), centre)))) + dynamics.body_set_angular_velocity(id, spin) + } + i = i + 1 + } +} + +// A figure with `flags` dropped from standing with a kick, then thrown +// (flung up and spun hard), stepping 600 steps in all: the worst limit +// excursion and joint separation over the whole of it. `variant` picks +// one of four kicks and throws: a fall is chaotic, so one throw's worst +// says little, and figures are compared by their worst over the four. +drop_and_throw(flags: int, variant: int) -> JointWorst { + world_id = world_with_ground() + h = Human { bones: null, filter_joints: null, filter_joint_count: 0, friction_torque: 0.0, is_spawned: false } + set_flags(&h, flags) + shape = human.human_reference_shape() + human.create_human_shaped(&h, world_id, Transform { p: math.vec3(0.0, 1.2, 0.0), q: math.quat_identity() }, shape, 5.0, 1.0, 0.7, 1, null) + human.destroy_shape(shape) + sign = 1.0 + if variant % 2 == 1 { sign = 0.0 - 1.0 } + lean = 1.0 + 0.5 * (variant / 2) + move_rigidly(&h, math.vec3_zero(), math.vec3(0.4 * sign * lean, 0.3, 0.6 * sign)) + worst = JointWorst { angular: 0.0, linear: 0.0, settled_angular: 0.0, settled_linear: 0.0 } + s = 0 + while s < 600 { + if s == 300 { move_rigidly(&h, math.vec3(2.0 * sign, 6.0, lean), math.vec3(0.0 - 3.0 * lean, 4.0 * sign, 2.0)) } + physics_world.step(world_id, 1.0 / 60.0, 4) + now = joint_worst(&h) + worst.angular = math.max_float(worst.angular, now.angular) + worst.linear = math.max_float(worst.linear, now.linear) + if s == 299 || s == 599 { + worst.settled_angular = math.max_float(worst.settled_angular, now.angular) + worst.settled_linear = math.max_float(worst.settled_linear, now.linear) + } + s = s + 1 + } + human.destroy_human(&h) + dynamics.destroy_world(world_id) + return worst +} + +// The worst of the four throws. +worst_of_throws(flags: int) -> JointWorst { + worst = JointWorst { angular: 0.0, linear: 0.0, settled_angular: 0.0, settled_linear: 0.0 } + variant = 0 + while variant < 4 { + one = drop_and_throw(flags, variant) + worst.angular = math.max_float(worst.angular, one.angular) + worst.linear = math.max_float(worst.linear, one.linear) + worst.settled_angular = math.max_float(worst.settled_angular, one.settled_angular) + worst.settled_linear = math.max_float(worst.settled_linear, one.settled_linear) + variant = variant + 1 + } + return worst +} + +// A soft joint gives on an impact and comes back: Box3D's joints are soft +// constraints at a quarter of the sub-step rate, and a light body struck +// between the ground and a heavy one (a hand under the figure, a head on +// its neck in a spin) overruns its limit for a few steps, more than the +// reference's heavier bones do (at four sub-steps; eight halve it). What +// holds is the limit itself: every joint back at it once the figure +// settles, and no joint ever turned through it. A soft joint is a spring +// whose stiffness goes with the mass it moves, so a light bone pressed to +// its limit by the figure's weight (a wrist under the body, a shoulder +// hanging from a clavicle) settles a little past it: within 3.5 degrees, +// where the reference's heavier bones settle within 0.3. +const SETTLED_ANGLE = 0.06 +const SETTLED_SEPARATION = 0.005 +const NEVER_THROUGH = 1.2 + +test_extras_build() { + baseline = worst_of_throws(0) + println("human: the reference's figure, dropped and thrown four ways: worst limit excursion ${baseline.angular} rad, worst separation ${baseline.linear} m, settled ${baseline.settled_angular} rad and ${baseline.settled_linear} m") + options = [WITH_HEAD, WITH_CLAVICLES, WITH_HANDS, WITH_SPINE_02, WITH_TWIST, WITH_HEAD | WITH_CLAVICLES | WITH_HANDS | WITH_SPINE_02 | WITH_TWIST, + WITH_HEAD | WITH_CLAVICLES | WITH_HANDS | WITH_SPINE_02 | WITH_TWIST | WITH_ANKLES] + k = 0 + while k < 7 { + flags = options[k] + world_id = world_with_ground() + h = Human { bones: null, filter_joints: null, filter_joint_count: 0, friction_torque: 0.0, is_spawned: false } + set_flags(&h, flags) + shape = human.human_reference_shape() + placement = Transform { p: math.vec3(0.5, 1.2, 0.0 - 0.25), q: math.quat_identity() } + human.create_human_shaped(&h, world_id, placement, shape, 5.0, 1.0, 0.7, 1, null) + bones = bones_for(flags) + filters = filters_for(flags) + counters = physics_world.get_counters(world_id) + ensure("extras ${flags}: spawned with ${bones} bodies and ${filters} filter joints (${counters.body_count - 1}, ${h.filter_joint_count})", + h.is_spawned && counters.body_count == bones + 1 && counters.joint_count == bones - 1 + filters && h.filter_joint_count == filters) + present = 0 + i = 0 + while i < human.MAX_BONE_COUNT { + if human.human_has_bone(&h, i) { + present = present + 1 + bone = human.bone_at(&h, i) + ok = dynamics.body_is_valid(bone.body_id) && (i == human.BONE_PELVIS || dynamics.joint_is_valid(bone.joint_id)) + ensure("extras ${flags}: bone ${i} has its body and joint", ok) + } else if i < h.bone_count { + ensure("extras ${flags}: bone ${i}, not asked for, has no body", dynamics.body_is_valid(human.bone_at(&h, i).body_id) == false) + } + i = i + 1 + } + ensure("extras ${flags}: human_has_bone names the ${bones} bones (${present})", present == bones) + // Each extra bone sits on its spec at rest, and its joint is closed + // and inside its limits; every other joint too. + e = human.BONE_HEAD + while e < human.MAX_BONE_COUNT { + if human.human_has_bone(&h, e) { + spec = human.spec_at(shape, e) + xf = dynamics.body_get_transform(human.bone_at(&h, e).body_id) + at = math.transform_point(placement, spec.reference.p) + ensure("extras ${flags}: bone ${e} sits on its spec (${math.length(math.sub(xf.p, at))} m)", near_vec(xf.p, at, 0.000000001) && same_rotation(xf.q, spec.reference.q)) + } + e = e + 1 + } + rest = joint_worst(&h) + ensure("extras ${flags}: every joint closed and inside its limits at rest (${rest.angular} rad, ${rest.linear} m)", rest.angular < 0.001 && rest.linear < 0.001) + physics_world.step(world_id, 1.0 / 60.0, 4) + overlap = deepest_bone_overlap(&h) + ensure("extras ${flags}: no two bones that may touch start inside each other (${overlap} m)", overlap > 0.0 - 0.005) + twisted = (flags & WITH_TWIST) != 0 + elbow = dynamics.joint_get_type(human.bone_at(&h, human.BONE_LOWER_ARM_L).joint_id) + knee = dynamics.joint_get_type(human.bone_at(&h, human.BONE_CALF_R).joint_id) + if twisted { + ensure("extras ${flags}: the elbows and knees are twisting hinges", elbow == dynamics.JOINT_SPHERICAL && knee == dynamics.JOINT_SPHERICAL) + } else { + ensure("extras ${flags}: the elbows and knees stay hinges", elbow == dynamics.JOINT_REVOLUTE && knee == dynamics.JOINT_REVOLUTE) + } + human.destroy_human(&h) + ensure("extras ${flags}: destroyed, nothing left but the ground", physics_world.get_counters(world_id).body_count == 1 && physics_world.get_counters(world_id).joint_count == 0) + human.destroy_shape(shape) + dynamics.destroy_world(world_id) + // Dropped from standing and thrown, its joints hold and stay within + // their limits as the reference's figure's do. + worst = worst_of_throws(flags) + println("human: extras ${flags}, dropped and thrown four ways: worst limit excursion ${worst.angular} rad, worst separation ${worst.linear} m, settled ${worst.settled_angular} rad and ${worst.settled_linear} m") + ensure("extras ${flags}: settled, every joint inside its limits after each drop and throw (${worst.settled_angular} rad)", worst.settled_angular < SETTLED_ANGLE) + ensure("extras ${flags}: settled, every joint closed (${worst.settled_linear} m)", worst.settled_linear < SETTLED_SEPARATION) + ensure("extras ${flags}: no joint turned through its limit on an impact (${worst.angular} rad)", worst.angular < NEVER_THROUGH) + ensure("extras ${flags}: the joints stretch no more than half again the reference's (${worst.linear} against ${baseline.linear} m)", worst.linear <= 1.5 * baseline.linear + 0.01) + // The twisting hinges alone are held to the reference's own figure: + // locked sideways, they give no more than its hinges do. + if flags == WITH_TWIST { + ensure("extras ${flags}: within the limits as the reference's figure is (${worst.angular} against ${baseline.angular} rad)", worst.angular <= 1.25 * baseline.angular) + } + k = k + 1 + } +} + +// The bodies' masses: the extra bones take a person's share, and the +// figure weighs about what it did. +figure_mass(h: *Human) -> float { + total = 0.0 + i = 0 + while i < h.bone_count { + if human.human_has_bone(h, i) { total = total + dynamics.body_get_mass(human.bone_at(h, i).body_id) } + i = i + 1 + } + return total +} + +test_extras_masses() { + world_id = world_with_ground() + plain = Human { bones: null, filter_joints: null, filter_joint_count: 0, friction_torque: 0.0, is_spawned: false } + human.create_human(&plain, world_id, math.vec3(0.0, 1.0, 0.0), 5.0, 1.0, 0.7, 1, null) + h = Human { bones: null, filter_joints: null, filter_joint_count: 0, friction_torque: 0.0, is_spawned: false } + set_flags(&h, WITH_HEAD | WITH_CLAVICLES | WITH_HANDS | WITH_SPINE_02) + shape = human.human_reference_shape() + human.create_human_shaped(&h, world_id, Transform { p: math.vec3(3.0, 1.0, 0.0), q: math.quat_identity() }, shape, 5.0, 1.0, 0.7, 2, null) + human.destroy_shape(shape) + before = figure_mass(&plain) + after = figure_mass(&h) + head = dynamics.body_get_mass(human.bone_at(&h, human.BONE_HEAD).body_id) / after + neck = dynamics.body_get_mass(human.bone_at(&h, human.BONE_NECK).body_id) / after + hand = dynamics.body_get_mass(human.bone_at(&h, human.BONE_HAND_L).body_id) / after + clavicle = dynamics.body_get_mass(human.bone_at(&h, human.BONE_CLAVICLE_L).body_id) / after + spine = dynamics.body_get_mass(human.bone_at(&h, human.BONE_SPINE_02).body_id) / after + println("human: masses: the figure ${before} kg, with the extra bones ${after} kg; shares: head ${head}, neck ${neck}, hand ${hand}, clavicle ${clavicle}, spine_02 ${spine}") + ensure("masses: the figure weighs within a tenth of the reference's (${after} against ${before} kg)", math.abs_float(after - before) < 0.1 * before) + ensure("masses: the head about a person's share, 5 to 9 percent (${head})", head > 0.05 && head < 0.09) + ensure("masses: a hand about a person's share, 0.4 to 1.2 percent (${hand})", hand > 0.004 && hand < 0.012) + human.destroy_human(&h) + human.destroy_human(&plain) + dynamics.destroy_world(world_id) +} + +// A rig's figure: the reference's bones fitted to landmarks a tenth larger, +// then each extra bone put on a rig bone of its own (moved off where the +// fit put it): each body's joint sits on the rig bone's head and its line +// runs to the rig bone's tail. +test_extras_fit() { + shape = human.human_reference_shape() + block = landmarks(shape) + v = block as Vec3[] + i = 0 + while i < 2 * human.BONE_COUNT + 2 { + v[i] = math.mul_sv(1.1, v[i]) + i = i + 1 + } + human.human_fit_shape(shape, block, block + human.BONE_COUNT * sizeof(Vec3), block + 2 * human.BONE_COUNT * sizeof(Vec3)) + heads = calloc(human.MAX_BONE_COUNT, sizeof(Vec3)) + tails = calloc(human.MAX_BONE_COUNT, sizeof(Vec3)) + head_at = heads as Vec3[] + tail_at = tails as Vec3[] + nudge = math.vec3(0.004, 0.0 - 0.006, 0.01) + e = human.BONE_HEAD + while e < human.MAX_BONE_COUNT { + head_at[e] = math.add(human.human_shape_head(shape, e), nudge) + tail_at[e] = math.add(human.human_shape_tail(shape, e), math.mul_sv(0.5, nudge)) + // A clavicle reaches the shoulder and spine_02 the chest's joint. + if e == human.BONE_CLAVICLE_L { tail_at[e] = human.human_shape_head(shape, human.BONE_UPPER_ARM_L) } + if e == human.BONE_CLAVICLE_R { tail_at[e] = human.human_shape_head(shape, human.BONE_UPPER_ARM_R) } + if e == human.BONE_SPINE_02 { tail_at[e] = human.human_shape_head(shape, human.BONE_SPINE_03) } + human.human_fit_bone(shape, e, head_at[e], tail_at[e]) + e = e + 1 + } + world_id = world_with_ground() + h = Human { bones: null, filter_joints: null, filter_joint_count: 0, friction_torque: 0.0, is_spawned: false } + set_flags(&h, WITH_HEAD | WITH_CLAVICLES | WITH_HANDS | WITH_SPINE_02 | WITH_TWIST) + placement = Transform { p: math.vec3(0.0, 1.3, 0.0), q: math.quat_identity() } + human.create_human_shaped(&h, world_id, placement, shape, 5.0, 1.0, 0.7, 1, null) + ensure("fit extras: spawned", h.is_spawned) + e = human.BONE_HEAD + while e < human.MAX_BONE_COUNT { + xf = dynamics.body_get_transform(human.bone_at(&h, e).body_id) + at = math.transform_point(placement, head_at[e]) + line = math.normalize(math.sub(tail_at[e], head_at[e])) + along = math.rotate_vector(xf.q, math.vec3_axis_z()) + ensure("fit extras: bone ${e}'s joint on its rig bone (${math.length(math.sub(xf.p, at))} m) and its line to the tail (${angle_between(along, line)} rad)", + near_vec(xf.p, at, 0.000001) && angle_between(along, line) < 0.000001) + e = e + 1 + } + rest = joint_worst(&h) + ensure("fit extras: every joint closed and inside its limits at rest (${rest.angular} rad, ${rest.linear} m)", rest.angular < 0.001 && rest.linear < 0.001) + human.destroy_human(&h) + dynamics.destroy_world(world_id) + free(heads) + free(tails) + free(block) + human.destroy_shape(shape) +} + +// How far a bone has turned about its own line against its parent since +// `rest` (the bone's rotation in its parent's frame then). +turn_about_line(h: *Human, index: int, rest: Quat) -> float { + bone = human.bone_at(h, index) + now = math.inv_mul_quat(dynamics.body_get_rotation(human.bone_at(h, bone.parent_index).body_id), dynamics.body_get_rotation(bone.body_id)) + delta = math.inv_mul_quat(rest, now) + line = math.normalize(human.human_bone_far_end(h, index)) + along = delta.x * line.x + delta.y * line.y + delta.z * line.z + return 2.0 * atan2(along, delta.w) +} + +// A forearm spun about itself, in a world without gravity: a twisting +// elbow lets it turn (pronation, supination) up to its range and no +// further; a hinge does not let it turn at all. +forearm_turn(twist: bool, speed: float) -> float { + world_id = world_with_ground() + dynamics.world_set_gravity(dynamics.get_world(world_id.index1 - 1), math.vec3_zero()) + h = Human { bones: null, filter_joints: null, filter_joint_count: 0, friction_torque: 0.0, is_spawned: false } + h.twist = twist + shape = human.human_reference_shape() + human.create_human_shaped(&h, world_id, Transform { p: math.vec3(0.0, 1.5, 0.0), q: math.quat_identity() }, shape, 0.0, 0.0, 0.0, 1, null) + human.destroy_shape(shape) + index = human.BONE_LOWER_ARM_L + bone = human.bone_at(&h, index) + rest = math.inv_mul_quat(dynamics.body_get_rotation(human.bone_at(&h, bone.parent_index).body_id), dynamics.body_get_rotation(bone.body_id)) + axis = math.rotate_vector(dynamics.body_get_rotation(bone.body_id), math.normalize(human.human_bone_far_end(&h, index))) + most = 0.0 + s = 0 + while s < 90 { + dynamics.body_set_angular_velocity(bone.body_id, math.mul_sv(speed, axis)) + physics_world.step(world_id, 1.0 / 60.0, 4) + turned = math.abs_float(turn_about_line(&h, index, rest)) + if turned > most { most = turned } + s = s + 1 + } + human.destroy_human(&h) + dynamics.destroy_world(world_id) + return most +} + +test_twist() { + hinge = forearm_turn(false, 3.0) + twisting = forearm_turn(true, 3.0) + hard = forearm_turn(true, 30.0) + println("human: a forearm spun about itself turns ${hinge} rad on a hinge, ${twisting} on a twisting elbow, ${hard} spun hard (range ${human.ELBOW_TWIST_DEGREES * math.DEG_TO_RAD})") + ensure("twist: a hinge does not let the forearm turn about itself (${hinge} rad)", hinge < 0.05) + ensure("twist: a twisting elbow does, as a person's forearm turns (${twisting} rad)", twisting > 0.5) + ensure("twist: and no further than its range (${hard} rad)", hard < (human.ELBOW_TWIST_DEGREES + 5.0) * math.DEG_TO_RAD) +} + main() { g_slab = hull.make_box_hull(20.0, 1.0, 20.0) as ptr test_build_and_fall() @@ -1201,6 +1652,11 @@ main() { test_ankles() test_ankle_crouch() test_ankle_shins() + test_unchanged() + test_extras_build() + test_extras_masses() + test_extras_fit() + test_twist() hull.destroy_hull(g_slab as *HullData) println("human: ${checks} checks") if failures == 0 { diff --git a/tests/test_joint_solver.ae b/tests/test_joint_solver.ae index e3e35e7..303b21d 100644 --- a/tests/test_joint_solver.ae +++ b/tests/test_joint_solver.ae @@ -920,6 +920,58 @@ test_point_mass_matrix() { ensure("the point mass matrix is the full skew product to the bit (${mismatches} of 2000 differ)", mismatches == 0) } +// A hinge that twists (#190): a spherical joint whose cone's minor is +// locked. Its cone a circle of 0.6 about frame A's z, its major axis x, so +// frame B's z stays in the x-z plane: bending about y is free inside the +// cone, twisting about z is free, and turning about x (out of the plane) +// is held as a revolute holds its axis. +test_spherical_lock() { + f = fixture() + sdef = dynamics.default_spherical_joint_def() + common_frames(&f, &sdef.base) + sdef.enable_cone_limit = true + sdef.cone_angle = 0.6 + sdef.cone_minor_angle = 0.3 + id = dynamics.create_spherical_joint(f.world_id, &sdef) + ensure("lock: off by default", joint_solver.spherical_joint_is_cone_minor_locked(id) == false) + joint_solver.spherical_joint_lock_cone_minor(id, true) + ensure("lock: on", joint_solver.spherical_joint_is_cone_minor_locked(id)) + ensure("lock: an ellipse's minor dropped for the circle", joint_solver.spherical_joint_get_cone_minor_angle(id) == 0.0) + state = cube_state(&f) + set_velocity(&f, math.vec3_zero(), math.vec3(1.0, 2.0, 3.0)) + solve_step(&f, id) + w = math.widen3(state.w) + small("lock: turning out of the plane removed", w.x, 0.000001) + small("lock: bending in the plane kept", w.y - 2.0, 0.000001) + small("lock: twisting kept", w.z - 3.0, 0.000001) + // Turned out of the plane by 0.2: the biased solve turns it back, the + // separation reads the lean, the torque reported is the lock's. + dynamics.body_set_transform(f.cube_id, math.vec3(0.0, 4.0, 0.0), math.make_quat_from_axis_angle(math.vec3_axis_x(), 0.2)) + small("lock: the lean read as separation (atan2_approx)", dynamics.joint_get_angular_separation(id) - 0.2, 0.005) + set_velocity(&f, math.vec3_zero(), math.vec3_zero()) + solve_biased(&f, id) + ensure("lock: turned back into the plane (${math.widen3(state.w).x})", math.widen3(state.w).x < 0.0 - 0.1) + j = dynamics.spherical_joint_of(sim(&f, id)) + ensure("lock: the torque reported turns it back (${joint_solver.joint_get_constraint_torque(id).x})", joint_solver.joint_get_constraint_torque(id).x < 0.0 - 1.0) + // Bent in the plane by 0.4 (inside the cone): no lean, nothing to do. + dynamics.body_set_transform(f.cube_id, math.vec3(0.0, 4.0, 0.0), math.make_quat_from_axis_angle(math.vec3_axis_y(), 0.4)) + small("lock: bent in the plane, no separation", dynamics.joint_get_angular_separation(id), 0.000001) + // Warm starting off drops the lock's impulse; unlocking drops it too. + j.lock_impulse = 1.0 + f.ctx.enable_warm_starting = false + joint_solver.prepare_joint(sim(&f, id), &f.ctx) + ensure("lock: no warm start when disabled", j.lock_impulse == 0.0) + f.ctx.enable_warm_starting = true + j.lock_impulse = 1.0 + joint_solver.spherical_joint_lock_cone_minor(id, false) + ensure("lock: off again, its impulse gone", joint_solver.spherical_joint_is_cone_minor_locked(id) == false && j.lock_impulse == 0.0) + set_velocity(&f, math.vec3_zero(), math.vec3(1.0, 0.0, 0.0)) + dynamics.body_set_transform(f.cube_id, math.vec3(0.0, 4.0, 0.0), math.quat_identity()) + solve_step(&f, id) + small("lock: off, turning about x is free", math.widen3(state.w).x - 1.0, 0.000001) + dynamics.destroy_world(f.world_id) +} + main() { dynamics.reserve_scratch() before = basics.alloc_count() @@ -930,6 +982,7 @@ main() { test_weld() test_prismatic() test_spherical() + test_spherical_lock() test_parallel_motor_wheel() test_point_mass_matrix() hull.destroy_hull(g_cube as *HullData) diff --git a/tests/test_recording.ae b/tests/test_recording.ae index 9bd4164..4f5198a 100644 --- a/tests/test_recording.ae +++ b/tests/test_recording.ae @@ -1209,6 +1209,10 @@ all_ops() { joint_solver.spherical_joint_set_cone_axis(j_spherical, math.vec3(0.1, 0.4, 0.2)) physics_world.step(world_id, 1.0 / 60.0, 4) joint_solver.spherical_joint_set_cone_ellipse(j_spherical, 0.5, 0.3, math.vec3(1.0, 0.0, 0.0)) + joint_solver.spherical_joint_lock_cone_minor(j_spherical, true) + physics_world.step(world_id, 1.0 / 60.0, 4) + joint_solver.spherical_joint_lock_cone_minor(j_spherical, false) + joint_solver.spherical_joint_set_cone_ellipse(j_spherical, 0.5, 0.3, math.vec3(1.0, 0.0, 0.0)) joint_solver.spherical_joint_enable_twist_limit(j_spherical, true) joint_solver.spherical_joint_set_twist_limits(j_spherical, 0.0 - 0.4, 0.5) joint_solver.spherical_joint_enable_spring(j_spherical, true) @@ -1255,7 +1259,7 @@ all_ops() { dynamics.destroy_world(world_id) // Every opcode a recording has. - all_op_count = 162 + all_op_count = 163 all_ops_list = [ dynamics.OP_CREATE_BODY, dynamics.OP_DESTROY_BODY, dynamics.OP_BODY_SET_TRANSFORM, dynamics.OP_BODY_SET_LINEAR_VELOCITY, dynamics.OP_BODY_SET_ANGULAR_VELOCITY, @@ -1326,7 +1330,7 @@ all_ops() { dynamics.OP_WHEEL_JOINT_SET_STEERING_HERTZ, dynamics.OP_WHEEL_JOINT_SET_STEERING_DAMPING_RATIO, dynamics.OP_WHEEL_JOINT_SET_MAX_STEERING_TORQUE, dynamics.OP_WHEEL_JOINT_ENABLE_STEERING_LIMIT, dynamics.OP_WHEEL_JOINT_SET_STEERING_LIMITS, dynamics.OP_WHEEL_JOINT_SET_TARGET_STEERING_ANGLE, dynamics.OP_ENABLE_KINEMATIC_SPECULATION, - dynamics.OP_ENABLE_TRAVEL_SPECULATION + dynamics.OP_ENABLE_TRAVEL_SPECULATION, dynamics.OP_SPHERICAL_JOINT_LOCK_CONE_MINOR ] seen = calloc(4096, 4) mark_ops(r, seen)