@series-inc/rundot-syncplay 6.0.0-rc.3 → 6.0.0-rc.4

package/core/native/physics/buoyancy.cpp

@@ -4,6 +4,7 @@
 #include <array>
 #include <cmath>
 #include <cstddef>
+#include <vector>
 
 namespace rundot::kinetix::physics {
 namespace {
@@ -726,6 +727,66 @@ BuoyancyStatus calculateSubmergedGeometryWithPlane(
   return BuoyancyStatus::UnsupportedShape;
 }
 
+BuoyancyStatus finishFluidForces(
+    const FluidBodyInput3D& body,
+    const FluidVolume3D& volume,
+    const BuoyancyVector3& gravity,
+    double fullVolume,
+    FluidForcesResult3D& output) {
+  if (output.submerged.volume <= 0.0) {
+    output.submerged = {0.0, body.position};
+    return BuoyancyStatus::Success;
+  }
+  const double normSq = body.orientation.x * body.orientation.x
+    + body.orientation.y * body.orientation.y
+    + body.orientation.z * body.orientation.z
+    + body.orientation.w * body.orientation.w;
+  if (!(normSq > 0.0) || !finite(normSq)) return BuoyancyStatus::InvalidInput;
+  const BuoyancyVector3 centerOfMassWorld = add(
+    body.position,
+    rotate(body.orientation, 1.0 / normSq, body.centerOfMass));
+  const BuoyancyVector3 effectiveGravity = scale(gravity, body.gravityScale);
+  output.buoyantForce = scale(
+    effectiveGravity, -volume.density * output.submerged.volume);
+  const BuoyancyVector3 arm = subtract(
+    output.submerged.center, centerOfMassWorld);
+  const double submergedFraction = fullVolume > 0.0
+    ? std::min(1.0, output.submerged.volume / fullVolume) : 0.0;
+  if (submergedFraction >= 0.999999999
+    && dot(body.centerOfMass, body.centerOfMass) < 1e-12) {
+    output.buoyantTorque = {};
+  } else {
+    output.buoyantTorque = cross(arm, output.buoyantForce);
+  }
+  const BuoyancyVector3 pointVelocity = add(
+    body.linearVelocity, cross(body.angularVelocity, arm));
+  output.relativeVelocity = subtract(pointVelocity, volume.flowVelocity);
+  const double dragCoefficient = volume.density * output.submerged.volume
+    * volume.linearDrag;
+  output.dragForce = scale(output.relativeVelocity, -dragCoefficient);
+  const BuoyancyVector3 dragTorqueAtPoint = cross(arm, output.dragForce);
+  const BuoyancyVector3 angularDamping = scale(
+    body.angularVelocity, -submergedFraction * volume.angularDrag);
+  output.dragTorque = add(dragTorqueAtPoint, angularDamping);
+  output.totalForce = add(output.buoyantForce, output.dragForce);
+  output.totalTorque = add(output.buoyantTorque, output.dragTorque);
+  if (!finite(output.totalForce) || !finite(output.totalTorque)) {
+    return BuoyancyStatus::InvalidInput;
+  }
+  const BuoyancyVector3 gravitationalForce = scale(
+    effectiveGravity, body.mass);
+  const BuoyancyVector3 netForce = add(
+    output.buoyantForce, gravitationalForce);
+  const bool equilibrium = dot(netForce, netForce) < 1e-6
+    && dot(output.buoyantTorque, output.buoyantTorque) < 1e-6
+    && dot(output.relativeVelocity, output.relativeVelocity) < 1e-4
+    && dot(body.angularVelocity, body.angularVelocity) < 1e-4;
+  output.isInEquilibrium = equilibrium;
+  output.shouldWake = body.sleeping
+    && (!equilibrium || dot(volume.flowVelocity, volume.flowVelocity) > 1e-6);
+  return BuoyancyStatus::Success;
+}
+
 BuoyancyStatus calculateFluidForces(
     const FluidBodyInput3D& body,
     const FluidVolume3D& volume,
@@ -754,74 +815,216 @@ BuoyancyStatus calculateFluidForces(
     volume.bounds,
     output.submerged);
   if (geomStatus != BuoyancyStatus::Success) return geomStatus;
-  if (output.submerged.volume <= 0.0) {
-    output.submerged = {0.0, body.position};
-    return BuoyancyStatus::Success;
-  }
-
-  const double normSq = body.orientation.x * body.orientation.x
-    + body.orientation.y * body.orientation.y
-    + body.orientation.z * body.orientation.z
-    + body.orientation.w * body.orientation.w;
-  if (!(normSq > 0.0) || !finite(normSq)) return BuoyancyStatus::InvalidInput;
-  const BuoyancyVector3 centerOfMassWorld = add(
-    body.position,
-    rotate(body.orientation, 1.0 / normSq, body.centerOfMass));
-
-  const BuoyancyVector3 effectiveGravity = scale(gravity, body.gravityScale);
-
-  output.buoyantForce = scale(effectiveGravity, -volume.density * output.submerged.volume);
-
-  const BuoyancyVector3 arm = subtract(output.submerged.center, centerOfMassWorld);
-
   const double fullVol = (body.shape == ShapeKind::Box)
     ? (8.0 * body.size.x * body.size.y * body.size.z)
     : ((4.0 / 3.0) * kPi * body.size.x * body.size.x * body.size.x);
-  const double submergedFrac = (fullVol > 0.0)
-    ? std::min(1.0, output.submerged.volume / fullVol)
-    : 0.0;
+  return finishFluidForces(body, volume, gravity, fullVol, output);
+}
 
-  if (submergedFrac >= 0.999999999 && dot(body.centerOfMass, body.centerOfMass) < 1e-12) {
-    output.buoyantTorque = {0.0, 0.0, 0.0};
-  } else {
-    output.buoyantTorque = cross(arm, output.buoyantForce);
+BuoyancyStatus validateVoxelBoxes(std::span<const VoxelBoxShape3D> boxes) {
+  if (boxes.empty()) return BuoyancyStatus::InvalidInput;
+  for (std::size_t index = 0; index < boxes.size(); ++index) {
+    const VoxelBoxShape3D& box = boxes[index];
+    if (!finite(box.halfSize) || !finite(box.offset)
+      || !(box.halfSize.x > 0.0) || !(box.halfSize.y > 0.0)
+      || !(box.halfSize.z > 0.0)) return BuoyancyStatus::InvalidInput;
+    for (std::size_t otherIndex = 0; otherIndex < index; ++otherIndex) {
+      const VoxelBoxShape3D& other = boxes[otherIndex];
+      const bool overlap = box.offset.x - box.halfSize.x
+          < other.offset.x + other.halfSize.x
+        && other.offset.x - other.halfSize.x
+          < box.offset.x + box.halfSize.x
+        && box.offset.y - box.halfSize.y
+          < other.offset.y + other.halfSize.y
+        && other.offset.y - other.halfSize.y
+          < box.offset.y + box.halfSize.y
+        && box.offset.z - box.halfSize.z
+          < other.offset.z + other.halfSize.z
+        && other.offset.z - other.halfSize.z
+          < box.offset.z + box.halfSize.z;
+      if (overlap) return BuoyancyStatus::OverlapUnsupported;
+    }
   }
+  return BuoyancyStatus::Success;
+}
 
-  const BuoyancyVector3 pointVel = add(body.linearVelocity, cross(body.angularVelocity, arm));
-  output.relativeVelocity = subtract(pointVel, volume.flowVelocity);
+template <typename Calculate>
+BuoyancyStatus calculateVoxelGeometry(
+    const BuoyancyVector3& position,
+    const BuoyancyQuaternion& orientation,
+    std::span<const VoxelBoxShape3D> boxes,
+    bool validateBoxes,
+    Calculate calculate,
+    SubmergedGeometry& output,
+    double& fullVolume) {
+  output = {};
+  fullVolume = 0.0;
+  if (validateBoxes) {
+    const BuoyancyStatus validation = validateVoxelBoxes(boxes);
+    if (validation != BuoyancyStatus::Success) return validation;
+  }
+  const double normSquared = orientation.x * orientation.x
+    + orientation.y * orientation.y + orientation.z * orientation.z
+    + orientation.w * orientation.w;
+  if (!(normSquared > 0.0) || !finite(normSquared) || !finite(position)) {
+    return BuoyancyStatus::InvalidInput;
+  }
+  BuoyancyVector3 centerMoment{};
+  for (const VoxelBoxShape3D& box : boxes) {
+    const BuoyancyVector3 center = add(position,
+      rotate(orientation, 1.0 / normSquared, box.offset));
+    SubmergedGeometry child{};
+    const BuoyancyStatus status = calculate(center, box.halfSize, child);
+    if (status != BuoyancyStatus::Success) return status;
+    fullVolume += 8.0 * box.halfSize.x * box.halfSize.y * box.halfSize.z;
+    output.volume += child.volume;
+    centerMoment = add(centerMoment, scale(child.center, child.volume));
+  }
+  if (!finite(fullVolume) || !finite(output.volume)) {
+    return BuoyancyStatus::NumericRange;
+  }
+  output.center = output.volume > 0.0
+    ? scale(centerMoment, 1.0 / output.volume) : position;
+  return finite(output.center)
+    ? BuoyancyStatus::Success : BuoyancyStatus::NumericRange;
+}
 
-  const double dragCoeff = volume.density * output.submerged.volume * volume.linearDrag;
-  output.dragForce = scale(output.relativeVelocity, -dragCoeff);
+BuoyancyStatus calculateVoxelBuoyancy(
+    const BuoyancyInput& input,
+    std::span<const VoxelBoxShape3D> boxes,
+    BuoyancyResult& output) {
+  output = {};
+  if (!(input.density > 0.0) || !finite(input.density)
+    || !finite(input.gravity) || !finite(input.bodyCenter)
+    || !finite(input.waterY)) return BuoyancyStatus::InvalidInput;
+  double fullVolume = 0.0;
+  const BuoyancyStatus status = calculateVoxelGeometry(
+    input.shapeCenter, input.orientation, boxes, true,
+    [&](const BuoyancyVector3& center, const BuoyancyVector3& halfSize,
+        SubmergedGeometry& child) {
+      return calculateSubmergedGeometry(ShapeKind::Box, center,
+        input.orientation, halfSize, input.waterY, child);
+    }, output.submerged, fullVolume);
+  (void) fullVolume;
+  if (status != BuoyancyStatus::Success) return status;
+  output.force = scale(input.gravity,
+    -input.density * output.submerged.volume);
+  output.torque = cross(
+    subtract(output.submerged.center, input.bodyCenter), output.force);
+  return finite(output.force) && finite(output.torque)
+    ? BuoyancyStatus::Success : BuoyancyStatus::NumericRange;
+}
 
-  const BuoyancyVector3 dragTorquePoint = cross(arm, output.dragForce);
+BuoyancyStatus calculateVoxelFluidForcesInternal(
+    const FluidBodyInput3D& body,
+    std::span<const VoxelBoxShape3D> boxes,
+    const FluidVolume3D& volume,
+    const BuoyancyVector3& gravity,
+    FluidForcesResult3D& output,
+    bool validateBoxes) {
+  output = {};
+  if (!finite(gravity) || !finite(body.position)
+    || !finite(body.orientation) || !finite(body.linearVelocity)
+    || !finite(body.angularVelocity)) return BuoyancyStatus::InvalidInput;
+  const BuoyancyStatus volumeStatus = validateFluidVolume(volume);
+  if (volumeStatus != BuoyancyStatus::Success) return volumeStatus;
+  const bool filterEnabled = volume.layer != 0 || volume.mask != 0;
+  if (filterEnabled && ((volume.mask & body.layer) == 0
+    || (body.mask & volume.layer) == 0)) return BuoyancyStatus::Success;
+  double fullVolume = 0.0;
+  const BuoyancyStatus status = calculateVoxelGeometry(
+    body.position, body.orientation, boxes, validateBoxes,
+    [&](const BuoyancyVector3& center, const BuoyancyVector3& halfSize,
+        SubmergedGeometry& child) {
+      return calculateSubmergedGeometryWithPlane(ShapeKind::Box, center,
+        body.orientation, halfSize, volume.plane, volume.bounds, child);
+    }, output.submerged, fullVolume);
+  if (status != BuoyancyStatus::Success) return status;
+  return finishFluidForces(body, volume, gravity, fullVolume, output);
+}
 
-  const BuoyancyVector3 angularDamping = scale(body.angularVelocity, -submergedFrac * volume.angularDrag);
-  output.dragTorque = add(dragTorquePoint, angularDamping);
+BuoyancyStatus calculateVoxelFluidForces(
+    const FluidBodyInput3D& body,
+    std::span<const VoxelBoxShape3D> boxes,
+    const FluidVolume3D& volume,
+    const BuoyancyVector3& gravity,
+    FluidForcesResult3D& output) {
+  return calculateVoxelFluidForcesInternal(
+    body, boxes, volume, gravity, output, true);
+}
 
-  output.totalForce = add(output.buoyantForce, output.dragForce);
-  output.totalTorque = add(output.buoyantTorque, output.dragTorque);
+BuoyancyStatus calculateValidatedVoxelFluidForces(
+    const FluidBodyInput3D& body,
+    std::span<const VoxelBoxShape3D> boxes,
+    const FluidVolume3D& volume,
+    const BuoyancyVector3& gravity,
+    FluidForcesResult3D& output) {
+  return calculateVoxelFluidForcesInternal(
+    body, boxes, volume, gravity, output, false);
+}
 
-  if (!finite(output.totalForce) || !finite(output.totalTorque)) {
-    return BuoyancyStatus::InvalidInput;
+BuoyancyStatus calculateVoxelFluidStateForces(
+    const FluidBodyInput3D& body,
+    std::span<const VoxelBoxShape3D> boxes,
+    const FluidState3D& state,
+    const BuoyancyVector3& gravity,
+    FluidForcesResult3D& output) {
+  output = {};
+  const BuoyancyStatus stateStatus = validateFluidState(state);
+  if (stateStatus != BuoyancyStatus::Success) return stateStatus;
+  const BuoyancyStatus boxesStatus = validateVoxelBoxes(boxes);
+  if (boxesStatus != BuoyancyStatus::Success) return boxesStatus;
+  double maximumVolume = 0.0;
+  for (const VoxelBoxShape3D& box : boxes) {
+    maximumVolume += 8.0 * box.halfSize.x * box.halfSize.y * box.halfSize.z;
   }
-
-  const BuoyancyVector3 gravForce = scale(effectiveGravity, body.mass);
-  const BuoyancyVector3 netForce = add(output.buoyantForce, gravForce);
-  const double netForceSq = dot(netForce, netForce);
-  const double bTorqueSq = dot(output.buoyantTorque, output.buoyantTorque);
-  const double relVelSq = dot(output.relativeVelocity, output.relativeVelocity);
-  const double angVelSq = dot(body.angularVelocity, body.angularVelocity);
-
-  const bool inEquilibrium = (netForceSq < 1e-6 && bTorqueSq < 1e-6 && relVelSq < 1e-4 && angVelSq < 1e-4);
-  output.isInEquilibrium = inEquilibrium;
-
-  if (body.sleeping) {
-    const double flowSq = dot(volume.flowVelocity, volume.flowVelocity);
-    output.shouldWake = (!inEquilibrium || flowSq > 1e-6);
+  BuoyancyVector3 centerMoment{};
+  BuoyancyVector3 velocityMoment{};
+  bool equilibrium = true;
+  for (std::uint32_t index = 0; index < state.volumeCount; ++index) {
+    FluidForcesResult3D current{};
+    const BuoyancyStatus status = calculateVoxelFluidForces(
+      body, boxes, state.volumes[index], gravity, current);
+    if (status != BuoyancyStatus::Success) return status;
+    if (state.overlapPolicy == FluidOverlapPolicy::PriorityPartition
+      && output.submerged.volume < maximumVolume
+      && current.submerged.volume > maximumVolume - output.submerged.volume) {
+      const double remaining = maximumVolume - output.submerged.volume;
+      const double factor = remaining / current.submerged.volume;
+      current.submerged.volume = remaining;
+      current.buoyantForce = scale(current.buoyantForce, factor);
+      current.buoyantTorque = scale(current.buoyantTorque, factor);
+      current.dragForce = scale(current.dragForce, factor);
+      current.dragTorque = scale(current.dragTorque, factor);
+      current.totalForce = add(current.buoyantForce, current.dragForce);
+      current.totalTorque = add(current.buoyantTorque, current.dragTorque);
+    }
+    if (state.overlapPolicy == FluidOverlapPolicy::PriorityPartition
+      && output.submerged.volume >= maximumVolume) continue;
+    output.buoyantForce = add(output.buoyantForce, current.buoyantForce);
+    output.buoyantTorque = add(output.buoyantTorque, current.buoyantTorque);
+    output.dragForce = add(output.dragForce, current.dragForce);
+    output.dragTorque = add(output.dragTorque, current.dragTorque);
+    output.totalForce = add(output.totalForce, current.totalForce);
+    output.totalTorque = add(output.totalTorque, current.totalTorque);
+    centerMoment = add(centerMoment,
+      scale(current.submerged.center, current.submerged.volume));
+    velocityMoment = add(velocityMoment,
+      scale(current.relativeVelocity, current.submerged.volume));
+    output.submerged.volume += current.submerged.volume;
+    output.shouldWake = output.shouldWake || current.shouldWake;
+    if (current.submerged.volume > 0.0 && !current.isInEquilibrium) {
+      equilibrium = false;
+    }
+  }
+  if (output.submerged.volume > 0.0) {
+    const double inverseVolume = 1.0 / output.submerged.volume;
+    output.submerged.center = scale(centerMoment, inverseVolume);
+    output.relativeVelocity = scale(velocityMoment, inverseVolume);
+    output.isInEquilibrium = equilibrium;
   } else {
-    output.shouldWake = false;
+    output.submerged.center = body.position;
   }
-
   return BuoyancyStatus::Success;
 }
 
@@ -924,6 +1127,51 @@ BuoyancyStatus stepBodyInFluids(
 
 }  // namespace rundot::kinetix::physics
 
+namespace {
+
+bool parseVoxelBoxes(
+    const double* values,
+    std::uint32_t valueCount,
+    std::vector<rundot::kinetix::physics::VoxelBoxShape3D>& output) {
+  using rundot::kinetix::physics::VoxelBoxShape3D;
+  if (values == nullptr || valueCount == 0 || valueCount % 6 != 0) return false;
+  output.resize(valueCount / 6);
+  for (std::size_t index = 0; index < output.size(); ++index) {
+    const double* record = values + index * 6;
+    for (std::size_t field = 0; field < 6; ++field) {
+      if (!std::isfinite(record[field])) return false;
+    }
+    output[index] = VoxelBoxShape3D{
+      {record[0], record[1], record[2]},
+      {record[3], record[4], record[5]},
+    };
+  }
+  return true;
+}
+
+void writeFluidResult(
+    const rundot::kinetix::physics::FluidForcesResult3D& result,
+    double* output) {
+  const double values[rundot::kinetix::physics::kFluidForcesOutputStride] = {
+    result.submerged.volume,
+    result.submerged.center.x, result.submerged.center.y,
+    result.submerged.center.z,
+    result.buoyantForce.x, result.buoyantForce.y, result.buoyantForce.z,
+    result.buoyantTorque.x, result.buoyantTorque.y, result.buoyantTorque.z,
+    result.dragForce.x, result.dragForce.y, result.dragForce.z,
+    result.dragTorque.x, result.dragTorque.y, result.dragTorque.z,
+    result.totalForce.x, result.totalForce.y, result.totalForce.z,
+    result.totalTorque.x, result.totalTorque.y, result.totalTorque.z,
+    result.relativeVelocity.x, result.relativeVelocity.y,
+    result.relativeVelocity.z,
+    result.shouldWake ? 1.0 : 0.0,
+    result.isInEquilibrium ? 1.0 : 0.0,
+  };
+  std::copy(std::begin(values), std::end(values), output);
+}
+
+}  // namespace
+
 extern "C" std::int32_t rundot_kinetix_physics_buoyancy(
     const double* input,
     std::uint32_t input_values,
@@ -983,6 +1231,64 @@ extern "C" std::int32_t rundot_kinetix_physics_buoyancy(
   return static_cast<std::int32_t>(status);
 }
 
+extern "C" std::int32_t rundot_kinetix_physics_voxel_buoyancy(
+    const double* input,
+    std::uint32_t input_values,
+    const double* children,
+    std::uint32_t child_values,
+    double* output,
+    std::uint32_t output_values) {
+  using namespace rundot::kinetix::physics;
+  if (output == nullptr || output_values < kBuoyancyOutputStride) {
+    return static_cast<std::int32_t>(BuoyancyStatus::OutputTooSmall);
+  }
+  if (input == nullptr || input_values != kBuoyancyInputStride) {
+    return static_cast<std::int32_t>(BuoyancyStatus::InvalidInput);
+  }
+  for (std::uint32_t index = 0; index < input_values; ++index) {
+    if (!std::isfinite(input[index])) {
+      return static_cast<std::int32_t>(BuoyancyStatus::InvalidInput);
+    }
+  }
+  std::vector<VoxelBoxShape3D> boxes;
+  if (!parseVoxelBoxes(children, child_values, boxes)) {
+    return static_cast<std::int32_t>(BuoyancyStatus::InvalidInput);
+  }
+  BuoyancyInput parsed{
+    ShapeKind::Compound,
+    {input[1], input[2], input[3]},
+    {input[4], input[5], input[6], input[7]},
+    {},
+    input[11],
+    input[12],
+    {input[13], input[14], input[15]},
+    {input[16], input[17], input[18]},
+  };
+  const double normSquared = parsed.orientation.x * parsed.orientation.x
+    + parsed.orientation.y * parsed.orientation.y
+    + parsed.orientation.z * parsed.orientation.z
+    + parsed.orientation.w * parsed.orientation.w;
+  if (!(normSquared > 0.0) || !std::isfinite(normSquared)) {
+    return static_cast<std::int32_t>(BuoyancyStatus::InvalidInput);
+  }
+  parsed.bodyCenter = add(parsed.shapeCenter,
+    rotate(parsed.orientation, 1.0 / normSquared, parsed.bodyCenter));
+  BuoyancyResult result{};
+  const BuoyancyStatus status = calculateVoxelBuoyancy(parsed, boxes, result);
+  if (status != BuoyancyStatus::Success) {
+    return static_cast<std::int32_t>(status);
+  }
+  const double values[kBuoyancyOutputStride] = {
+    result.submerged.volume,
+    result.submerged.center.x, result.submerged.center.y,
+    result.submerged.center.z,
+    result.force.x, result.force.y, result.force.z,
+    result.torque.x, result.torque.y, result.torque.z,
+  };
+  std::copy(std::begin(values), std::end(values), output);
+  return static_cast<std::int32_t>(BuoyancyStatus::Success);
+}
+
 extern "C" std::uint32_t rundot_kinetix_physics_buoyancy_abi_version() {
   return 1;
 }
@@ -1091,6 +1397,77 @@ extern "C" std::int32_t rundot_kinetix_physics_fluid_forces(
   return static_cast<std::int32_t>(BuoyancyStatus::Success);
 }
 
+extern "C" std::int32_t rundot_kinetix_physics_voxel_fluid_forces(
+    const double* input,
+    std::uint32_t input_values,
+    const double* children,
+    std::uint32_t child_values,
+    double* output,
+    std::uint32_t output_values) {
+  using namespace rundot::kinetix::physics;
+  if (output == nullptr || output_values < kFluidForcesOutputStride) {
+    return static_cast<std::int32_t>(BuoyancyStatus::OutputTooSmall);
+  }
+  if (input == nullptr || input_values != kFluidForcesInputStride) {
+    return static_cast<std::int32_t>(BuoyancyStatus::InvalidInput);
+  }
+  for (std::uint32_t index = 0; index < input_values; ++index) {
+    if (!std::isfinite(input[index])) {
+      return static_cast<std::int32_t>(BuoyancyStatus::InvalidInput);
+    }
+  }
+  std::vector<VoxelBoxShape3D> boxes;
+  if (!parseVoxelBoxes(children, child_values, boxes)) {
+    return static_cast<std::int32_t>(BuoyancyStatus::InvalidInput);
+  }
+  const auto u32 = [](double value, std::uint32_t& result) {
+    if (value < 0.0 || value > 4294967295.0
+      || std::floor(value) != value) return false;
+    result = static_cast<std::uint32_t>(value);
+    return true;
+  };
+  std::uint32_t ignoredShape = 0;
+  std::uint32_t booleanValue = 0;
+  FluidBodyInput3D body{};
+  FluidVolume3D volume{};
+  if (!u32(input[0], ignoredShape)
+    || !u32(input[25], booleanValue) || booleanValue > 1
+    || !u32(input[26], body.layer) || !u32(input[27], body.mask)
+    || !u32(input[28], volume.id)
+    || !u32(input[35], booleanValue) || booleanValue > 1
+    || !u32(input[46], volume.layer) || !u32(input[47], volume.mask)) {
+    return static_cast<std::int32_t>(BuoyancyStatus::InvalidInput);
+  }
+  body.shape = ShapeKind::Compound;
+  body.position = {input[1], input[2], input[3]};
+  body.orientation = {input[4], input[5], input[6], input[7]};
+  body.centerOfMass = {input[11], input[12], input[13]};
+  body.linearVelocity = {input[14], input[15], input[16]};
+  body.angularVelocity = {input[17], input[18], input[19]};
+  body.mass = input[20];
+  body.inertiaDiag = {input[21], input[22], input[23]};
+  body.gravityScale = input[24];
+  body.sleeping = input[25] != 0.0;
+  volume.bounds = {
+    input[29], input[30], input[31], input[32], input[33], input[34]};
+  volume.plane.enabled = input[35] != 0.0;
+  volume.plane.normal = {input[36], input[37], input[38]};
+  volume.plane.offset = input[39];
+  volume.density = input[40];
+  volume.linearDrag = input[41];
+  volume.angularDrag = input[42];
+  volume.flowVelocity = {input[43], input[44], input[45]};
+  const BuoyancyVector3 gravity{input[48], input[49], input[50]};
+  FluidForcesResult3D result{};
+  const BuoyancyStatus status = calculateVoxelFluidForces(
+    body, boxes, volume, gravity, result);
+  if (status != BuoyancyStatus::Success) {
+    return static_cast<std::int32_t>(status);
+  }
+  writeFluidResult(result, output);
+  return static_cast<std::int32_t>(BuoyancyStatus::Success);
+}
+
 extern "C" std::int32_t rundot_kinetix_physics_fluid_state_forces(
     const double* input,
     std::uint32_t input_values,

package/core/native/physics/buoyancy.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cstdint>
+#include <span>
 
 #include "physics.hpp"
 
@@ -129,6 +130,11 @@ struct FluidBodyInput3D {
   std::uint32_t mask = 1;
 };
 
+struct VoxelBoxShape3D {
+  BuoyancyVector3 halfSize{};
+  BuoyancyVector3 offset{};
+};
+
 struct FluidForcesResult3D {
   SubmergedGeometry submerged{};
   BuoyancyVector3 buoyantForce{};
@@ -161,6 +167,34 @@ BuoyancyStatus calculateFluidForces(
   const BuoyancyVector3& gravity,
   FluidForcesResult3D& output);
 
+BuoyancyStatus validateVoxelBoxes(std::span<const VoxelBoxShape3D> boxes);
+
+BuoyancyStatus calculateVoxelBuoyancy(
+  const BuoyancyInput& input,
+  std::span<const VoxelBoxShape3D> boxes,
+  BuoyancyResult& output);
+
+BuoyancyStatus calculateVoxelFluidForces(
+  const FluidBodyInput3D& body,
+  std::span<const VoxelBoxShape3D> boxes,
+  const FluidVolume3D& volume,
+  const BuoyancyVector3& gravity,
+  FluidForcesResult3D& output);
+
+BuoyancyStatus calculateValidatedVoxelFluidForces(
+  const FluidBodyInput3D& body,
+  std::span<const VoxelBoxShape3D> boxes,
+  const FluidVolume3D& volume,
+  const BuoyancyVector3& gravity,
+  FluidForcesResult3D& output);
+
+BuoyancyStatus calculateVoxelFluidStateForces(
+  const FluidBodyInput3D& body,
+  std::span<const VoxelBoxShape3D> boxes,
+  const FluidState3D& state,
+  const BuoyancyVector3& gravity,
+  FluidForcesResult3D& output);
+
 BuoyancyStatus stepBodyInFluids(
   FluidBodyInput3D& body,
   const FluidState3D& state,
@@ -195,6 +229,20 @@ extern "C" std::int32_t rundot_kinetix_physics_fluid_state_forces(
   std::uint32_t input_values,
   double* output,
   std::uint32_t output_values);
+extern "C" std::int32_t rundot_kinetix_physics_voxel_buoyancy(
+  const double* input,
+  std::uint32_t input_values,
+  const double* children,
+  std::uint32_t child_values,
+  double* output,
+  std::uint32_t output_values);
+extern "C" std::int32_t rundot_kinetix_physics_voxel_fluid_forces(
+  const double* input,
+  std::uint32_t input_values,
+  const double* children,
+  std::uint32_t child_values,
+  double* output,
+  std::uint32_t output_values);
 extern "C" std::int32_t rundot_kinetix_physics_validate_fluid_volume(
   const rundot::kinetix::physics::FluidVolume3D* volume);
 extern "C" std::int32_t rundot_kinetix_physics_validate_fluid_state(

package/core/native/physics/geometry_store.cpp

@@ -103,7 +103,8 @@ bool validGeometry(std::span<const double> geometry, ShapeKind kind,
     if (primitive && !validPrimitive(childKind, record + 4)) return false;
     if (format == 4) continue;
     if (!nonNegativeInteger(record[13], count) || !nonNegativeInteger(record[14], count)
-      || !nonNegativeInteger(record[15], 5) || record[16] != 0 || record[17] != children) return false;
+      || !nonNegativeInteger(record[15], 5) || !nonNegativeInteger(record[16], 1)
+      || record[16] != values[16] || record[17] != children) return false;
     const auto childOffset = static_cast<std::uint64_t>(record[13]);
     const auto childCount = static_cast<std::uint64_t>(record[14]);
     if (primitive) {
@@ -115,6 +116,24 @@ bool validGeometry(std::span<const double> geometry, ShapeKind kind,
       static_cast<std::uint32_t>(record[15]), depth + 1)) return false;
     payloadEnd = childOffset + childCount;
   }
+  if (format == 5 && values[16] == 1) {
+    constexpr std::uint64_t recordStride = 18;
+    for (std::uint64_t child = 0; child < children; ++child) {
+      const double* first = values + child * recordStride;
+      if (first[0] != static_cast<double>(ShapeKind::Box)
+        || first[13] != 0 || first[14] != 0 || first[15] != 0) return false;
+      for (std::uint64_t other = 0; other < child; ++other) {
+        const double* second = values + other * recordStride;
+        const bool overlap = first[1] - first[4] < second[1] + second[4]
+          && second[1] - second[4] < first[1] + first[4]
+          && first[2] - first[5] < second[2] + second[5]
+          && second[2] - second[5] < first[2] + first[5]
+          && first[3] - first[6] < second[3] + second[6]
+          && second[3] - second[6] < first[3] + first[6];
+        if (overlap) return false;
+      }
+    }
+  }
   return true;
 }