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

package/core/native/physics/abi.cpp

@@ -173,9 +173,10 @@ namespace {
 
 using namespace rundot::kinetix::physics;
 
-static std::array<SixDofC, kMaxSixDofCapacity> g_sixDofConstraints{};
+static std::array<SixDofConstraint, kMaxSixDofCapacity> g_sixDofConstraints{};
 static std::array<std::uint32_t, kMaxSixDofCapacity> g_sixDofOffsets{};
 static std::array<std::uint32_t, kMaxSixDofCapacity> g_sixDofLogicalIndices{};
+static std::array<SixDofConstraintOutput, kMaxSixDofCapacity> g_sixDofOutputs{};
 static std::array<double, kMaxStandardJointCapacity * kJointStride> g_standardJoints{};
 static std::array<std::uint32_t, kMaxStandardJointCapacity> g_standardLogicalIndices{};
 
@@ -186,29 +187,6 @@ struct StandardJointAccumulator {
 };
 static std::array<StandardJointAccumulator, kMaxStandardJointCapacity> g_standardAccumulators{};
 static std::array<Body, 4096> g_bodies{};
-static std::array<ConstraintBodyMobility, 4096> g_mobility{};
-
-struct SixDofAccumulator {
-  std::array<double, 6> limitImpulse{};
-  std::array<double, 6> motorImpulse{};
-  std::array<double, 6> springImpulse{};
-  std::array<bool, 6> limitEngaged{};
-  std::array<bool, 6> motorActive{};
-};
-static std::array<SixDofAccumulator, kMaxSixDofCapacity> g_accumulators{};
-
-// Must match kMaxSixDofRows in constraint_rows.hpp, which is what
-// buildSixDofRows can actually emit. This was 12 while the builder produced up to 18,
-// so a joint with a limit, a motor and a spring on all six axes lost its last rows in
-// silence and those axes moved unconstrained.
-constexpr std::uint32_t kMaxSixDofRows = 18;
-struct JointRange {
-  std::uint32_t start = 0;
-  std::uint32_t count = 0;
-};
-static std::array<JointRange, kMaxSixDofCapacity> g_ranges{};
-static std::array<ConstraintRow6, kMaxSixDofCapacity * kMaxSixDofRows> g_rows{};
-static std::array<SixDofMeta, kMaxSixDofCapacity * kMaxSixDofRows> g_meta{};
 
 struct Vec3 {
   double x = 0.0;
@@ -324,91 +302,8 @@ Body unpackBody(const double* bodyData, std::uint32_t offset) {
   return body;
 }
 
-void packBody(double* bodyData, std::uint32_t offset, const Body& body) {
-  bodyData[offset + 0] = body.x;
-  bodyData[offset + 1] = body.y;
-  bodyData[offset + 2] = body.z;
-  bodyData[offset + 3] = body.vx;
-  bodyData[offset + 4] = body.vy;
-  bodyData[offset + 5] = body.vz;
-  const std::uint32_t motionKind = body.dynamic ? 1u : (body.kinematic ? 2u : 0u);
-  const std::uint32_t motion = motionKind
-    | (body.trigger ? kMotionTrigger : 0u)
-    | (body.sensor ? kMotionSensor : 0u)
-    | (body.sleeping ? kMotionSleeping : 0u)
-    | (body.angularState ? kMotionAngularState : 0u);
-  bodyData[offset + 9] = static_cast<double>(motion);
-  bodyData[offset + 19] = body.angularVelX;
-  bodyData[offset + 20] = body.angularVelY;
-  bodyData[offset + 21] = body.angularVelZ;
-  bodyData[offset + 22] = body.orientationX;
-  bodyData[offset + 23] = body.orientationY;
-  bodyData[offset + 24] = body.orientationZ;
-  bodyData[offset + 25] = body.orientationW;
-  bodyData[offset + 40] = body.sleepThreshold;
-  bodyData[offset + 41] = body.sleepEnabled ? 1.0 : 0.0;
-  bodyData[offset + 42] = body.sleepTime;
-  bodyData[offset + 43] = body.rollingResistance;
-}
-
-void integrateOrientationStep(Body& body, double timeStep) {
-  const double halfStep = 0.5 * timeStep;
-  const double qx = body.orientationX;
-  const double qy = body.orientationY;
-  const double qz = body.orientationZ;
-  const double qw = body.orientationW;
-  const double wx = body.angularVelX;
-  const double wy = body.angularVelY;
-  const double wz = body.angularVelZ;
-  body.orientationX = qx + halfStep * (wx * qw + wy * qz - wz * qy);
-  body.orientationY = qy + halfStep * (-wx * qz + wy * qw + wz * qx);
-  body.orientationZ = qz + halfStep * (wx * qy - wy * qx + wz * qw);
-  body.orientationW = qw + halfStep * (-wx * qx - wy * qy - wz * qz);
-  const double lengthSquared = body.orientationX * body.orientationX
-    + body.orientationY * body.orientationY
-    + body.orientationZ * body.orientationZ
-    + body.orientationW * body.orientationW;
-  if (lengthSquared > 0.0) {
-    const double invLen = 1.0 / std::sqrt(lengthSquared);
-    body.orientationX *= invLen;
-    body.orientationY *= invLen;
-    body.orientationZ *= invLen;
-    body.orientationW *= invLen;
-  }
-}
-
-void finalizeBody(Body& body, const StepOptions& options) {
-  if ((!body.dynamic && !body.kinematic) || (body.dynamic && body.sleeping)) return;
-  if (body.dynamic) {
-    body.vx = quantizeDistance(body.vx * body.linearDamping);
-    body.vy = quantizeDistance(body.vy * body.linearDamping);
-    body.vz = quantizeDistance(body.vz * body.linearDamping);
-    body.angularVelX = quantizeDistance(body.angularVelX * body.angularDamping);
-    body.angularVelY = quantizeDistance(body.angularVelY * body.angularDamping);
-    body.angularVelZ = quantizeDistance(body.angularVelZ * body.angularDamping);
-  }
-  if ((body.motionLocks & (1u << 0)) != 0) body.vx = 0.0;
-  if ((body.motionLocks & (1u << 1)) != 0) body.vy = 0.0;
-  if ((body.motionLocks & (1u << 2)) != 0) body.vz = 0.0;
-  if ((body.motionLocks & (1u << 3)) != 0) body.angularVelX = 0.0;
-  if ((body.motionLocks & (1u << 4)) != 0) body.angularVelY = 0.0;
-  if ((body.motionLocks & (1u << 5)) != 0) body.angularVelZ = 0.0;
-  if (body.dynamic && options.maximumLinearSpeed > 0.0) {
-    const double speed = std::sqrt(body.vx * body.vx + body.vy * body.vy + body.vz * body.vz);
-    if (speed > options.maximumLinearSpeed) {
-      const double scale = options.maximumLinearSpeed / speed;
-      body.vx *= scale;
-      body.vy *= scale;
-      body.vz *= scale;
-    }
-  }
-  body.x = quantizeDistance(body.x);
-  body.y = quantizeDistance(body.y);
-  body.z = quantizeDistance(body.z);
-}
-
 void computeAxisStates(
-    const SixDofC& constraint,
+    const SixDofConstraint& constraint,
     const Body& bodyA,
     const Body& bodyB,
     std::array<double, 6>& positions,
@@ -474,65 +369,6 @@ void computeAxisStates(
   }
 }
 
-// Solves the six-DOF rows once per substep, at the substep timestep, from inside the
-// standard step's loop. Doing it here means the impulse reaches velocity before
-// integrateBodyPosition runs, so no position patch is needed afterwards.
-struct SixDofSubstepState {
-  std::uint32_t constraintCount = 0;
-  std::uint32_t velocityIterations = 1;
-};
-
-void solveSixDofSubstep(Body* bodies, std::uint32_t bodyCount, double substepDt, void* user) {
-  auto* state = static_cast<SixDofSubstepState*>(user);
-  if (state == nullptr || state->constraintCount == 0 || bodyCount == 0) return;
-
-  for (std::uint32_t i = 0; i < bodyCount; ++i) {
-    g_mobility[i] = constraintBodyMobility(bodies[i]);
-  }
-
-  std::span<Body> bodySpan(bodies, bodyCount);
-  std::uint32_t totalRows = 0;
-  for (std::uint32_t j = 0; j < state->constraintCount; ++j) {
-    g_ranges[j].start = totalRows;
-    std::span<ConstraintRow6> rSpan(g_rows.data() + totalRows, kMaxSixDofRows);
-    std::span<SixDofMeta> mSpan(g_meta.data() + totalRows, kMaxSixDofRows);
-    const std::uint32_t built = buildSixDofRows(
-        g_sixDofConstraints[j],
-        bodies[g_sixDofConstraints[j].bodyA],
-        bodies[g_sixDofConstraints[j].bodyB],
-        substepDt,
-        rSpan,
-        mSpan);
-    g_ranges[j].count = built;
-    totalRows += built;
-  }
-
-  for (std::uint32_t r = 0; r < totalRows; ++r) prepareConstraintRow(g_rows[r], g_mobility);
-  for (std::uint32_t iter = 0; iter < state->velocityIterations; ++iter) {
-    for (std::uint32_t r = 0; r < totalRows; ++r) solveConstraintRow(g_rows[r], bodySpan, g_mobility);
-  }
-
-  for (std::uint32_t j = 0; j < state->constraintCount; ++j) {
-    for (std::uint32_t r = g_ranges[j].start; r < g_ranges[j].start + g_ranges[j].count; ++r) {
-      const std::uint8_t a = g_meta[r].axisIndex;
-      const double imp = g_rows[r].scalar.accumulatedImpulse;
-      using K = SixDofRowKind;
-      const auto kind = g_meta[r].kind;
-      if (kind == K::LinearLock || kind == K::LinearLowerLimit || kind == K::LinearUpperLimit
-          || kind == K::LinearEqualLimit || kind == K::AngularLock || kind == K::AngularLowerLimit
-          || kind == K::AngularUpperLimit || kind == K::AngularEqualLimit) {
-        g_accumulators[j].limitEngaged[a] = true;
-        g_accumulators[j].limitImpulse[a] += imp;
-      } else if (kind == K::LinearMotor || kind == K::AngularMotor) {
-        g_accumulators[j].motorActive[a] = true;
-        g_accumulators[j].motorImpulse[a] += imp;
-      } else if (kind == K::LinearSpring || kind == K::AngularSpring) {
-        g_accumulators[j].springImpulse[a] += imp;
-      }
-    }
-  }
-}
-
 std::int32_t stepSoAWithSixDofJoints(
     double* body_data,
     std::uint32_t body_count,
@@ -558,8 +394,8 @@ std::int32_t stepSoAWithSixDofJoints(
       if (bAVal < 0.0 || bBVal < 0.0 || bAVal >= body_count || bBVal >= body_count || bAVal == bBVal) {
         return 3;
       }
-      SixDofC& c = g_sixDofConstraints[sixDofCount];
-      c = SixDofC{};
+      SixDofConstraint& c = g_sixDofConstraints[sixDofCount];
+      c = SixDofConstraint{};
       c.bodyA = static_cast<std::uint32_t>(bAVal);
       c.bodyB = static_cast<std::uint32_t>(bBVal);
       c.anchorA = {joint_data[offset + 3], joint_data[offset + 4], joint_data[offset + 5]};
@@ -580,6 +416,8 @@ std::int32_t stepSoAWithSixDofJoints(
         joint_data[offset + 14], joint_data[offset + 15],
         joint_data[offset + 16], joint_data[offset + 17]
       };
+      c.maximumLinearForce = joint_data[offset + 92];
+      c.maximumAngularTorque = joint_data[offset + 93];
       for (std::uint32_t a = 0; a < 6; ++a) {
         std::uint32_t m = static_cast<std::uint32_t>(joint_data[offset + 18 + a]);
         c.axes[a].limit.mode = m == 1 ? SixDofAxisMode::Locked
@@ -606,7 +444,7 @@ std::int32_t stepSoAWithSixDofJoints(
 
       g_sixDofOffsets[sixDofCount] = offset;
       g_sixDofLogicalIndices[sixDofCount] = logicalIndex;
-      g_accumulators[sixDofCount] = SixDofAccumulator{};
+      g_sixDofOutputs[sixDofCount] = SixDofConstraintOutput{};
       sixDofCount += 1;
       offset += slots * kJointStride;
     } else if (kindVal >= 1.0 && kindVal <= 9.0) {
@@ -622,30 +460,24 @@ std::int32_t stepSoAWithSixDofJoints(
     logicalIndex += 1;
   }
 
-  const std::uint32_t substeps = std::max(1u, options.substeps);
-  const std::uint32_t velocity_iterations = std::max(1u, options.velocityIterations);
-  const double substepDt = options.dtTicks / static_cast<double>(substeps);
-
-  // A six-DOF joint never reaches stepSoAWithJoints, so it takes no part in wake
-  // propagation or sleep islands. A sleeping body joined to an awake body by a locked
-  // axis therefore stayed still and stayed asleep. Wake the pair conservatively here.
+  std::array<std::array<std::uint32_t, 2>, kMaxSixDofCapacity> connections{};
+  std::array<std::array<std::uint32_t, 2>, kMaxSixDofCapacity> collisionExclusions{};
+  std::uint32_t collisionExclusionCount = 0;
   for (std::uint32_t j = 0; j < sixDofCount; ++j) {
-    const std::uint32_t a = g_sixDofConstraints[j].bodyA;
-    const std::uint32_t b = g_sixDofConstraints[j].bodyB;
-    if (a >= body_count || b >= body_count) continue;
-    double* rowA = body_data + a * kBodyStride;
-    double* rowB = body_data + b * kBodyStride;
-    const bool aSleeping = (static_cast<std::uint32_t>(rowA[9]) & kMotionSleeping) != 0;
-    const bool bSleeping = (static_cast<std::uint32_t>(rowB[9]) & kMotionSleeping) != 0;
-    if (aSleeping == bSleeping) continue;
-    double* wake = aSleeping ? rowA : rowB;
-    wake[9] = static_cast<double>(static_cast<std::uint32_t>(wake[9]) & ~kMotionSleeping);
-    wake[42] = 0.0;
+    connections[j] = {g_sixDofConstraints[j].bodyA, g_sixDofConstraints[j].bodyB};
+    if (joint_data[g_sixDofOffsets[j] + 98] == 1.0) {
+      collisionExclusions[collisionExclusionCount++] = connections[j];
+    }
   }
 
-  SixDofSubstepState sixDofState{sixDofCount, velocity_iterations};
+  SixDofSubstepParticipant sixDofParticipant{
+    {g_sixDofConstraints.data(), sixDofCount},
+    {g_sixDofOutputs.data(), sixDofCount},
+    true};
   const rundot::kinetix::physics::SubstepConstraintHook sixDofHook{
-    sixDofCount > 0 ? &solveSixDofSubstep : nullptr, &sixDofState};
+    sixDofCount > 0 ? &SixDofSubstepParticipant::invoke : nullptr, &sixDofParticipant,
+    {connections.data(), sixDofCount},
+    {collisionExclusions.data(), collisionExclusionCount}};
 
   std::int32_t status = standardCount > 0
     ? rundot::kinetix::physics::stepSoAWithJoints(
@@ -670,10 +502,6 @@ std::int32_t stepSoAWithSixDofJoints(
     }
   }
 
-  // The six-DOF rows were solved inside the substep loop by solveSixDofSubstep, so
-  // there is no full-step post-pass and no `dv * stepDt` position patch any more.
-  // Refresh the local body copies purely so the telemetry export below can read the
-  // final state.
   for (std::uint32_t i = 0; i < body_count; ++i) {
     g_bodies[i] = unpackBody(body_data, i * kBodyStride);
   }
@@ -693,24 +521,19 @@ std::int32_t stepSoAWithSixDofJoints(
     for (std::uint32_t a = 0; a < 6; ++a) {
       mutJointData[off2 + 0 + a] = pos[a];
       mutJointData[off2 + 6 + a] = vel[a];
-      mutJointData[off2 + 12 + a] = g_accumulators[j].limitEngaged[a] ? 1.0 : 0.0;
-      mutJointData[off2 + 18 + a] = g_accumulators[j].limitImpulse[a];
-      mutJointData[off2 + 24 + a] = g_accumulators[j].motorActive[a] ? 1.0 : 0.0;
-      mutJointData[off2 + 30 + a] = g_accumulators[j].motorImpulse[a];
-      mutJointData[off2 + 36 + a] = g_accumulators[j].springImpulse[a];
-      mutJointData[off2 + 42 + a] = g_accumulators[j].limitImpulse[a]
-        + g_accumulators[j].motorImpulse[a] + g_accumulators[j].springImpulse[a];
+      mutJointData[off2 + 12 + a] = g_sixDofOutputs[j].limitEngaged[a] ? 1.0 : 0.0;
+      mutJointData[off2 + 18 + a] = g_sixDofOutputs[j].limitImpulse[a];
+      mutJointData[off2 + 24 + a] = g_sixDofOutputs[j].motorActive[a] ? 1.0 : 0.0;
+      mutJointData[off2 + 30 + a] = g_sixDofOutputs[j].motorImpulse[a];
+      mutJointData[off2 + 36 + a] = g_sixDofOutputs[j].springImpulse[a];
+      mutJointData[off2 + 42 + a] = g_sixDofOutputs[j].limitImpulse[a]
+        + g_sixDofOutputs[j].motorImpulse[a] + g_sixDofOutputs[j].springImpulse[a];
     }
-    const double linLen = std::sqrt(
-        mutJointData[off2 + 42] * mutJointData[off2 + 42] +
-        mutJointData[off2 + 43] * mutJointData[off2 + 43] +
-        mutJointData[off2 + 44] * mutJointData[off2 + 44]);
-    const double angLen = std::sqrt(
-        mutJointData[off2 + 45] * mutJointData[off2 + 45] +
-        mutJointData[off2 + 46] * mutJointData[off2 + 46] +
-        mutJointData[off2 + 47] * mutJointData[off2 + 47]);
-    mutJointData[off2 + 48] = linLen;
-    mutJointData[off2 + 49] = angLen;
+    std::array<double, 2> magnitudes{};
+    if (sixDofReactionMagnitudes(g_sixDofOutputs[j], magnitudes)
+        != SixDofResponseStatus::Success) return 7;
+    mutJointData[off2 + 48] = magnitudes[0];
+    mutJointData[off2 + 49] = magnitudes[1];
   }
 
   g_customTelemetryCount = standardCount + sixDofCount;

package/core/native/physics/abi.hpp

@@ -5,12 +5,6 @@
 #include "physics.hpp"
 #include "constraint_rows.hpp"
 
-namespace rundot::kinetix::physics {
-using SixDofC = SixDofConstraint;
-using SixDofMeta = SixDofConstraintRowMetadata;
-constexpr auto buildSixDofRows = buildSixDofConstraintRows;
-}
-
 extern "C" {
 
 std::uint32_t rundot_kinetix_physics_abi_version();