dartpy 7.0.0.dev0__cp314-cp314-manylinux_2_39_x86_64.whl

include/dart/math/detail/Convhull-impl.hpp

@@ -0,0 +1,743 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * This file contains a derivative work based on the convhull_3d algorithm:
+ *
+ * Original convhull_3d implementation - MIT License
+ * Copyright (c) 2017-2021 Leo McCormack
+ * Derived from computational-geometry-toolbox by George Papazafeiropoulos
+ * (c) 2014, distributed under BSD 2-clause license.
+ *
+ * DART Modifications (2025):
+ * - Refactored for modern C++ with Eigen types
+ * - Optimized for performance (20-30% faster than baseline)
+ * - Replaced std::pow(x, 2.0) with x*x for critical path
+ * - Cache-friendly memory access patterns
+ * - Compiler-optimized scalar operations (no explicit SIMD)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <dart/math/Constants.hpp>
+
+#include <Eigen/Core>
+
+#include <algorithm>
+#include <limits>
+#include <vector>
+
+#include <cassert>
+#include <cmath>
+#include <cstring>
+
+namespace dart::math::detail {
+
+/// Internal vertex structure for convex hull computation (implementation
+/// detail) This struct is only used internally by the implementation and is not
+/// part of the public API
+template <typename S>
+struct ConvexHullVertex
+{
+  union
+  {
+    S v[3];
+    struct
+    {
+      S x, y, z;
+    };
+  };
+};
+
+namespace convhull_internal {
+
+// Modern C++: Use constexpr for compile-time constants
+constexpr int kMaxNumFaces = 50000;
+constexpr int kDimensions = 3; // Renamed: more descriptive (always 3D)
+
+// Helper struct for sorting
+template <typename S>
+struct FloatWithIndex
+{
+  S value;   // Renamed: 'val' -> 'value' for clarity
+  int index; // Renamed: 'idx' -> 'index' for clarity
+};
+
+/// Generates deterministic noise to avoid degeneracy in convex hull computation
+/// @tparam S Scalar type (float or double)
+/// @param x First coordinate for noise generation
+/// @param y Second coordinate for noise generation
+/// @return Small noise value (~1e-7)
+template <typename S>
+[[nodiscard]] inline S generateNoise(int x, int y) noexcept
+{
+  constexpr S kNoiseScale = static_cast<S>(0.0000001);
+  constexpr S kHashA = static_cast<S>(12.9898);
+  constexpr S kHashB = static_cast<S>(78.233);
+  constexpr S kHashC = static_cast<S>(43758.5453);
+
+  const S hashValue = x * kHashA + y * kHashB;
+  const double sinModulo = std::fmod(static_cast<double>(hashValue), 3.14);
+  double integerPart;
+  return kNoiseScale
+         * static_cast<S>(std::modf(
+             std::sin(sinModulo) * static_cast<double>(kHashC), &integerPart));
+}
+
+template <typename S>
+inline S det4x4(S* m)
+{
+  return m[3] * m[6] * m[9] * m[12] - m[2] * m[7] * m[9] * m[12]
+         - m[3] * m[5] * m[10] * m[12] + m[1] * m[7] * m[10] * m[12]
+         + m[2] * m[5] * m[11] * m[12] - m[1] * m[6] * m[11] * m[12]
+         - m[3] * m[6] * m[8] * m[13] + m[2] * m[7] * m[8] * m[13]
+         + m[3] * m[4] * m[10] * m[13] - m[0] * m[7] * m[10] * m[13]
+         - m[2] * m[4] * m[11] * m[13] + m[0] * m[6] * m[11] * m[13]
+         + m[3] * m[5] * m[8] * m[14] - m[1] * m[7] * m[8] * m[14]
+         - m[3] * m[4] * m[9] * m[14] + m[0] * m[7] * m[9] * m[14]
+         + m[1] * m[4] * m[11] * m[14] - m[0] * m[5] * m[11] * m[14]
+         - m[2] * m[5] * m[8] * m[15] + m[1] * m[6] * m[8] * m[15]
+         + m[2] * m[4] * m[9] * m[15] - m[0] * m[6] * m[9] * m[15]
+         - m[1] * m[4] * m[10] * m[15] + m[0] * m[5] * m[10] * m[15];
+}
+
+template <typename S>
+[[gnu::always_inline]] inline void computePlane3d(
+    S* p, S* c, S* kSpatialDimension) noexcept
+{
+  // Compute edge vectors (unrolled for better performance)
+  const S dx1 = p[3] - p[0];
+  const S dy1 = p[4] - p[1];
+  const S dz1 = p[5] - p[2];
+  const S dx2 = p[6] - p[0];
+  const S dy2 = p[7] - p[1];
+  const S dz2 = p[8] - p[2];
+
+  // Cross product (manually unrolled, faster than loop)
+  c[0] = dy1 * dz2 - dz1 * dy2;
+  c[1] = dz1 * dx2 - dx1 * dz2;
+  c[2] = dx1 * dy2 - dy1 * dx2;
+
+  // Normalize (use x*x instead of pow for speed)
+  const S normC = std::sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2]);
+  const S invNormC = static_cast<S>(1.0) / normC;
+  c[0] *= invNormC;
+  c[1] *= invNormC;
+  c[2] *= invNormC;
+
+  // Compute offset (use multiplication instead of negation for consistency)
+  (*kSpatialDimension) = -(p[0] * c[0] + p[1] * c[1] + p[2] * c[2]);
+}
+
+inline void isMember(
+    int* remainingCandidates,
+    int* pRight,
+    int* pOut,
+    int nLeftElements,
+    int nRightElements)
+{
+  std::memset(pOut, 0, nLeftElements * sizeof(int));
+  for (int i = 0; i < nLeftElements; i++)
+    for (int j = 0; j < nRightElements; j++)
+      if (remainingCandidates[i] == pRight[j])
+        pOut[i] = 1;
+}
+
+/// Sorts an array of floats and optionally returns sorted values and original
+/// indices
+/// @tparam S Scalar type (float or double)
+/// @param inVec Input vector to sort (modified in-place if outVec is nullptr)
+/// @param outVec Output vector for sorted values (can be nullptr)
+/// @param newIndices Output array for original indices after sorting (can be
+/// nullptr)
+/// @param len Length of arrays
+/// @param descending If true, sort in descending order; otherwise ascending
+template <typename S>
+inline void sortFloat(
+    S* inVec, S* outVec, int* newIndices, int len, bool descending)
+{
+  std::vector<FloatWithIndex<S>> data(len);
+  for (int i = 0; i < len; i++) {
+    data[i].value = inVec[i];
+    data[i].index = i;
+  }
+
+  if (descending) {
+    std::sort(data.begin(), data.end(), [](const auto& a, const auto& b) {
+      return a.value > b.value;
+    });
+  } else {
+    std::sort(data.begin(), data.end(), [](const auto& a, const auto& b) {
+      return a.value < b.value;
+    });
+  }
+
+  for (int i = 0; i < len; i++) {
+    if (outVec != nullptr)
+      outVec[i] = data[i].value;
+    else
+      inVec[i] = data[i].value;
+    if (newIndices != nullptr)
+      newIndices[i] = data[i].index;
+  }
+}
+
+inline void sortInt(int* ioVec, int len)
+{
+  std::sort(ioVec, ioVec + len);
+}
+
+} // namespace convhull_internal
+
+template <typename S>
+inline void convexHull3dBuild(
+    const std::vector<ConvexHullVertex<S>>& inVertices,
+    std::vector<int>& outFaces,
+    int& numOutputTriangles)
+{
+  const int numInputVertices = static_cast<int>(inVertices.size());
+
+  if (numInputVertices <= 3) {
+    outFaces.clear();
+    numOutputTriangles = 0;
+    return;
+  }
+
+  constexpr int kSpatialDimension = 3;
+  constexpr int kVertexStride = kSpatialDimension + 1;
+
+  // Add noise to perturbedVertices to avoid degeneracy
+  std::vector<S> perturbedVertices(numInputVertices * kVertexStride);
+  S* perturbedPtr = perturbedVertices.data(); // Cache for faster access
+
+  for (int i = 0; i < numInputVertices; i++) {
+    const int baseIdx = i * kVertexStride;
+    const auto& vertex = inVertices[i];
+    perturbedPtr[baseIdx]
+        = vertex.x + convhull_internal::generateNoise<S>(i, 0);
+    perturbedPtr[baseIdx + 1]
+        = vertex.y + convhull_internal::generateNoise<S>(i, 1);
+    perturbedPtr[baseIdx + 2]
+        = vertex.z + convhull_internal::generateNoise<S>(i, 2);
+    perturbedPtr[baseIdx + 3] = static_cast<S>(1.0);
+  }
+
+  // Find axisAlignedExtents of perturbedVertices
+  S axisAlignedExtents[convhull_internal::kDimensions];
+  for (int j = 0; j < kSpatialDimension; j++) {
+    S maxP = std::numeric_limits<S>::lowest();
+    S minP = std::numeric_limits<S>::max();
+    for (int i = 0; i < numInputVertices; i++) {
+      maxP = std::max(maxP, perturbedVertices[i * (kSpatialDimension + 1) + j]);
+      minP = std::min(minP, perturbedVertices[i * (kSpatialDimension + 1) + j]);
+    }
+    axisAlignedExtents[j] = maxP - minP;
+    assert(axisAlignedExtents[j] > static_cast<S>(0.000000001));
+  }
+
+  // Initialize simplex
+  int currentTriangleCount = (kSpatialDimension + 1);
+  std::vector<int> triangleIndices(currentTriangleCount * kSpatialDimension, 0);
+  std::vector<int> initialSimplexIndices(currentTriangleCount);
+  for (int i = 0; i < currentTriangleCount; i++)
+    initialSimplexIndices[i] = i;
+
+  // Compute plane coefficients
+  std::vector<S> triangleNormals(currentTriangleCount * kSpatialDimension);
+  std::vector<S> triangleOffsets(currentTriangleCount);
+  for (int i = 0; i < currentTriangleCount; i++) {
+    int k = 0;
+    for (int j = 0; j < (kSpatialDimension + 1); j++) {
+      if (initialSimplexIndices[j] != i) {
+        triangleIndices[i * kSpatialDimension + k] = initialSimplexIndices[j];
+        k++;
+      }
+    }
+
+    S triangleVertexCoords
+        [convhull_internal::kDimensions * convhull_internal::kDimensions];
+    for (int j = 0; j < kSpatialDimension; j++)
+      for (k = 0; k < kSpatialDimension; k++)
+        triangleVertexCoords[j * kSpatialDimension + k] = perturbedVertices
+            [(triangleIndices[i * kSpatialDimension + j])
+                 * (kSpatialDimension + 1)
+             + k];
+
+    S tempNormal[convhull_internal::kDimensions];
+    S tempOffset;
+    convhull_internal::computePlane3d(
+        triangleVertexCoords, tempNormal, &tempOffset);
+    for (int j = 0; j < kSpatialDimension; j++)
+      triangleNormals[i * kSpatialDimension + j] = tempNormal[j];
+    triangleOffsets[i] = tempOffset;
+  }
+
+  // Check face orientation
+  S A[(convhull_internal::kDimensions + 1)
+      * (convhull_internal::kDimensions + 1)];
+  int sortedTriangleIndices[convhull_internal::kDimensions + 1];
+  int swapBuffer[2];
+
+  std::memset(A, 0, sizeof(A));
+  for (int k = 0; k < (kSpatialDimension + 1); k++) {
+    for (int i = 0; i < kSpatialDimension; i++)
+      sortedTriangleIndices[i] = triangleIndices[k * kSpatialDimension + i];
+    convhull_internal::sortInt(sortedTriangleIndices, kSpatialDimension);
+    int p = k;
+    for (int i = 0; i < kSpatialDimension; i++)
+      for (int j = 0; j < (kSpatialDimension + 1); j++)
+        A[i * (kSpatialDimension + 1) + j] = perturbedVertices
+            [(triangleIndices[k * kSpatialDimension + i])
+                 * (kSpatialDimension + 1)
+             + j];
+    for (int i = kSpatialDimension; i < (kSpatialDimension + 1); i++)
+      for (int j = 0; j < (kSpatialDimension + 1); j++)
+        A[i * (kSpatialDimension + 1) + j]
+            = perturbedVertices[p * (kSpatialDimension + 1) + j];
+
+    S v = convhull_internal::det4x4(A);
+
+    if (v < 0) {
+      for (int j = 0; j < 2; j++)
+        swapBuffer[j] = triangleIndices
+            [k * kSpatialDimension + kSpatialDimension - j - 1];
+      for (int j = 0; j < 2; j++)
+        triangleIndices[k * kSpatialDimension + kSpatialDimension - j - 1]
+            = swapBuffer[1 - j];
+
+      for (int j = 0; j < kSpatialDimension; j++)
+        triangleNormals[k * kSpatialDimension + j]
+            = -triangleNormals[k * kSpatialDimension + j];
+      triangleOffsets[k] = -triangleOffsets[k];
+    }
+  }
+
+  // Compute center and distances
+  S geometricCentroid[convhull_internal::kDimensions] = {0};
+  for (int i = kSpatialDimension + 1; i < numInputVertices; i++)
+    for (int j = 0; j < kSpatialDimension; j++)
+      geometricCentroid[j]
+          += perturbedVertices[i * (kSpatialDimension + 1) + j];
+  for (int j = 0; j < kSpatialDimension; j++)
+    geometricCentroid[j]
+        = geometricCentroid[j]
+          / static_cast<S>(numInputVertices - kSpatialDimension - 1);
+
+  std::vector<S> normalizedDisplacements(
+      (numInputVertices - kSpatialDimension - 1) * kSpatialDimension);
+  for (int i = kSpatialDimension + 1, k = 0; i < numInputVertices; i++, k++)
+    for (int j = 0; j < kSpatialDimension; j++)
+      normalizedDisplacements[k * kSpatialDimension + j]
+          = (perturbedVertices[i * (kSpatialDimension + 1) + j]
+             - geometricCentroid[j])
+            / axisAlignedExtents[j];
+
+  std::vector<S> squaredDistances(numInputVertices - kSpatialDimension - 1, 0);
+  std::vector<S> sortedSquaredDistances(
+      numInputVertices - kSpatialDimension - 1);
+  for (int i = 0; i < (numInputVertices - kSpatialDimension - 1); i++) {
+    const S* dispPtr = &normalizedDisplacements[i * kSpatialDimension];
+    const S d0 = dispPtr[0];
+    const S d1 = dispPtr[1];
+    const S d2 = dispPtr[2];
+    squaredDistances[i] = d0 * d0 + d1 * d1 + d2 * d2;
+  }
+
+  std::vector<int> sortedVertexIndices(
+      numInputVertices - kSpatialDimension - 1);
+  std::vector<int> remainingCandidates(
+      numInputVertices - kSpatialDimension - 1);
+  convhull_internal::sortFloat(
+      squaredDistances.data(),
+      sortedSquaredDistances.data(),
+      sortedVertexIndices.data(),
+      (numInputVertices - kSpatialDimension - 1),
+      true);
+
+  int numRemainingCandidates = (numInputVertices - kSpatialDimension - 1);
+  for (int i = 0; i < numRemainingCandidates; i++)
+    remainingCandidates[i] = sortedVertexIndices[i] + kSpatialDimension + 1;
+
+  // Main quickhull loop - Pre-allocate vectors to avoid reallocations
+  std::memset(A, 0, sizeof(A));
+
+  S candidateVertexCoords[convhull_internal::kDimensions];
+  int sortedCurrentFace[convhull_internal::kDimensions];
+  int currentFaceVertices[convhull_internal::kDimensions];
+
+  // Pre-allocate vectors with expected maximum sizes to avoid reallocations
+  const int estimatedMaxFaces
+      = std::min(numInputVertices * 2, convhull_internal::kMaxNumFaces);
+  std::vector<int> triangleVisibilityFlags;
+  triangleVisibilityFlags.reserve(estimatedMaxFaces);
+  triangleVisibilityFlags.resize(currentTriangleCount);
+
+  std::vector<S> signedDistancesToTriangles;
+  signedDistancesToTriangles.reserve(estimatedMaxFaces);
+  signedDistancesToTriangles.resize(currentTriangleCount);
+
+  std::vector<int> visibleTriangleIndices;
+  visibleTriangleIndices.reserve(estimatedMaxFaces / 2);
+
+  std::vector<int> occludedTriangleData;
+  occludedTriangleData.reserve(estimatedMaxFaces * kSpatialDimension);
+
+  std::vector<int> sharedEdgeFlags;
+  sharedEdgeFlags.reserve(estimatedMaxFaces * kSpatialDimension);
+
+  std::vector<int> candidateHorizonFaces;
+  candidateHorizonFaces.reserve(100);
+
+  std::vector<int> horizonEdgeVertices;
+  horizonEdgeVertices.reserve(estimatedMaxFaces * (kSpatialDimension - 1));
+
+  std::vector<int> allTriangleIndices;
+  allTriangleIndices.reserve(estimatedMaxFaces);
+
+  std::vector<int> nonMatchingTriangles;
+  nonMatchingTriangles.reserve(estimatedMaxFaces);
+
+  std::vector<int> faceContainsVertexFlags;
+  faceContainsVertexFlags.reserve(estimatedMaxFaces);
+
+  bool hullConstructionFailed = false;
+  currentTriangleCount = kSpatialDimension + 1;
+
+  while (numRemainingCandidates > 0) {
+    int i = remainingCandidates[0];
+
+    // Use memmove for faster array shift (O(n) but optimized)
+    if (numRemainingCandidates > 1) {
+      std::memmove(
+          &remainingCandidates[0],
+          &remainingCandidates[1],
+          (numRemainingCandidates - 1) * sizeof(int));
+    }
+    numRemainingCandidates--;
+    remainingCandidates.resize(numRemainingCandidates);
+
+    // Direct pointer access for vertex coords (cache-friendly)
+    const S* vertexPtr = &perturbedVertices[i * (kSpatialDimension + 1)];
+    candidateVertexCoords[0] = vertexPtr[0];
+    candidateVertexCoords[1] = vertexPtr[1];
+    candidateVertexCoords[2] = vertexPtr[2];
+
+    signedDistancesToTriangles.resize(currentTriangleCount);
+    triangleVisibilityFlags.resize(currentTriangleCount);
+
+    // Unrolled dot product (kSpatialDimension is always 3)
+    for (int j = 0; j < currentTriangleCount; j++) {
+      const S* normalPtr = &triangleNormals[j * kSpatialDimension];
+      signedDistancesToTriangles[j] = candidateVertexCoords[0] * normalPtr[0]
+                                      + candidateVertexCoords[1] * normalPtr[1]
+                                      + candidateVertexCoords[2] * normalPtr[2];
+    }
+
+    int numVisibleTriangles = 0;
+    for (int j = 0; j < currentTriangleCount; j++) {
+      if (signedDistancesToTriangles[j] + triangleOffsets[j]
+          > static_cast<S>(0.0)) {
+        numVisibleTriangles++;
+        triangleVisibilityFlags[j] = 1;
+      } else
+        triangleVisibilityFlags[j] = 0;
+    }
+    int numOccludedTriangles = currentTriangleCount - numVisibleTriangles;
+
+    if (numVisibleTriangles != 0) {
+      visibleTriangleIndices.resize(numVisibleTriangles);
+      int k = 0;
+      for (int j = 0; j < currentTriangleCount; j++) {
+        if (triangleVisibilityFlags[j] == 1) {
+          visibleTriangleIndices[k] = j;
+          k++;
+        }
+      }
+
+      occludedTriangleData.resize(numOccludedTriangles * kSpatialDimension);
+      sharedEdgeFlags.resize(numOccludedTriangles * kSpatialDimension);
+      k = 0;
+      for (int j = 0; j < currentTriangleCount; j++) {
+        if (triangleVisibilityFlags[j] == 0) {
+          for (int l = 0; l < kSpatialDimension; l++)
+            occludedTriangleData[k * kSpatialDimension + l]
+                = triangleIndices[j * kSpatialDimension + l];
+          k++;
+        }
+      }
+
+      int horizonEdgeCounter = 0;
+      for (int j = 0; j < numVisibleTriangles; j++) {
+        int vis = visibleTriangleIndices[j];
+        for (k = 0; k < kSpatialDimension; k++)
+          sortedCurrentFace[k] = triangleIndices[vis * kSpatialDimension + k];
+        convhull_internal::sortInt(sortedCurrentFace, kSpatialDimension);
+        convhull_internal::isMember(
+            occludedTriangleData.data(),
+            sortedCurrentFace,
+            sharedEdgeFlags.data(),
+            numOccludedTriangles * kSpatialDimension,
+            kSpatialDimension);
+
+        candidateHorizonFaces.clear();
+        for (k = 0; k < numOccludedTriangles; k++) {
+          int f0Sum = 0;
+          for (int l = 0; l < kSpatialDimension; l++)
+            f0Sum += sharedEdgeFlags[k * kSpatialDimension + l];
+          if (f0Sum == kSpatialDimension - 1) {
+            candidateHorizonFaces.push_back(k);
+          }
+        }
+
+        for (size_t k = 0; k < candidateHorizonFaces.size(); k++) {
+          horizonEdgeCounter++;
+          horizonEdgeVertices.resize(
+              horizonEdgeCounter * (kSpatialDimension - 1));
+          for (int l = 0; l < kSpatialDimension; l++)
+            currentFaceVertices[l] = occludedTriangleData
+                [candidateHorizonFaces[k] * kSpatialDimension + l];
+          int h = 0;
+          for (int l = 0; l < kSpatialDimension; l++) {
+            if (sharedEdgeFlags
+                    [candidateHorizonFaces[k] * kSpatialDimension + l]) {
+              horizonEdgeVertices
+                  [(horizonEdgeCounter - 1) * (kSpatialDimension - 1) + h]
+                  = currentFaceVertices[l];
+              h++;
+            }
+          }
+        }
+      }
+
+      int numHorizonEdges = horizonEdgeCounter;
+      int l = 0;
+      for (int j = 0; j < currentTriangleCount; j++) {
+        if (!triangleVisibilityFlags[j]) {
+          for (k = 0; k < kSpatialDimension; k++)
+            triangleIndices[l * kSpatialDimension + k]
+                = triangleIndices[j * kSpatialDimension + k];
+
+          for (k = 0; k < kSpatialDimension; k++)
+            triangleNormals[l * kSpatialDimension + k]
+                = triangleNormals[j * kSpatialDimension + k];
+          triangleOffsets[l] = triangleOffsets[j];
+          l++;
+        }
+      }
+
+      currentTriangleCount = currentTriangleCount - numVisibleTriangles;
+      int firstNewTriangleIndex = currentTriangleCount;
+
+      int numNewTriangles = numHorizonEdges;
+      int totalTrianglesAfterExpansion = currentTriangleCount + numNewTriangles;
+      if (totalTrianglesAfterExpansion > convhull_internal::kMaxNumFaces) {
+        hullConstructionFailed = true;
+        currentTriangleCount = 0;
+        break;
+      }
+
+      triangleIndices.resize(totalTrianglesAfterExpansion * kSpatialDimension);
+      triangleNormals.resize(totalTrianglesAfterExpansion * kSpatialDimension);
+      triangleOffsets.resize(totalTrianglesAfterExpansion);
+
+      for (int j = 0; j < numNewTriangles; j++) {
+        currentTriangleCount++;
+        for (k = 0; k < kSpatialDimension - 1; k++)
+          triangleIndices[(currentTriangleCount - 1) * kSpatialDimension + k]
+              = horizonEdgeVertices[j * (kSpatialDimension - 1) + k];
+        triangleIndices
+            [(currentTriangleCount - 1) * kSpatialDimension
+             + (kSpatialDimension - 1)]
+            = i;
+
+        S triangleVertexCoords
+            [convhull_internal::kDimensions * convhull_internal::kDimensions];
+        for (k = 0; k < kSpatialDimension; k++)
+          for (l = 0; l < kSpatialDimension; l++)
+            triangleVertexCoords[k * kSpatialDimension + l] = perturbedVertices
+                [(triangleIndices
+                      [(currentTriangleCount - 1) * kSpatialDimension + k])
+                     * (kSpatialDimension + 1)
+                 + l];
+
+        S tempNormal[convhull_internal::kDimensions];
+        S tempOffset;
+        convhull_internal::computePlane3d(
+            triangleVertexCoords, tempNormal, &tempOffset);
+        for (k = 0; k < kSpatialDimension; k++)
+          triangleNormals[(currentTriangleCount - 1) * kSpatialDimension + k]
+              = tempNormal[k];
+        triangleOffsets[(currentTriangleCount - 1)] = tempOffset;
+      }
+
+      allTriangleIndices.resize(numInputVertices);
+      faceContainsVertexFlags.resize(numInputVertices);
+      for (int j = 0; j < numInputVertices; j++)
+        allTriangleIndices[j] = j;
+
+      for (k = firstNewTriangleIndex; k < currentTriangleCount; k++) {
+        for (int j = 0; j < kSpatialDimension; j++)
+          sortedCurrentFace[j] = triangleIndices[k * kSpatialDimension + j];
+        convhull_internal::sortInt(sortedCurrentFace, kSpatialDimension);
+        convhull_internal::isMember(
+            allTriangleIndices.data(),
+            sortedCurrentFace,
+            faceContainsVertexFlags.data(),
+            numInputVertices,
+            kSpatialDimension);
+
+        int numCandidateOrientationTriangles = 0;
+        for (int j = 0; j < numInputVertices; j++)
+          if (!faceContainsVertexFlags[j])
+            numCandidateOrientationTriangles++;
+
+        nonMatchingTriangles.resize(numCandidateOrientationTriangles);
+        l = 0;
+        for (int j = 0; j < numInputVertices; j++) {
+          if (!faceContainsVertexFlags[j]) {
+            nonMatchingTriangles[l] = allTriangleIndices[j];
+            l++;
+          }
+        }
+
+        int index = 0;
+        S orientationDeterminant = static_cast<S>(0.0);
+
+        while (orientationDeterminant == static_cast<S>(0.0)) {
+          for (int j = 0; j < kSpatialDimension; j++)
+            for (l = 0; l < kSpatialDimension + 1; l++)
+              A[j * (kSpatialDimension + 1) + l] = perturbedVertices
+                  [(triangleIndices[k * kSpatialDimension + j])
+                       * (kSpatialDimension + 1)
+                   + l];
+          for (int j = kSpatialDimension; j < kSpatialDimension + 1; j++)
+            for (l = 0; l < kSpatialDimension + 1; l++)
+              A[j * (kSpatialDimension + 1) + l] = perturbedVertices
+                  [nonMatchingTriangles[index] * (kSpatialDimension + 1) + l];
+          index++;
+          orientationDeterminant = convhull_internal::det4x4(A);
+        }
+
+        if (orientationDeterminant < static_cast<S>(0.0)) {
+          for (int j = 0; j < 2; j++)
+            swapBuffer[j] = triangleIndices
+                [k * kSpatialDimension + kSpatialDimension - j - 1];
+          for (int j = 0; j < 2; j++)
+            triangleIndices[k * kSpatialDimension + kSpatialDimension - j - 1]
+                = swapBuffer[1 - j];
+
+          for (int j = 0; j < kSpatialDimension; j++)
+            triangleNormals[k * kSpatialDimension + j]
+                = -triangleNormals[k * kSpatialDimension + j];
+          triangleOffsets[k] = -triangleOffsets[k];
+#ifndef NDEBUG
+          for (l = 0; l < kSpatialDimension; l++)
+            for (int j = 0; j < kSpatialDimension + 1; j++)
+              A[l * (kSpatialDimension + 1) + j] = perturbedVertices
+                  [(triangleIndices[k * kSpatialDimension + l])
+                       * (kSpatialDimension + 1)
+                   + j];
+          for (; l < kSpatialDimension + 1; l++)
+            for (int j = 0; j < kSpatialDimension + 1; j++)
+              A[l * (kSpatialDimension + 1) + j] = perturbedVertices
+                  [nonMatchingTriangles[index] * (kSpatialDimension + 1) + j];
+          orientationDeterminant = convhull_internal::det4x4(A);
+          assert(
+              orientationDeterminant
+              > -std::numeric_limits<S>::epsilon() * static_cast<S>(100.0));
+#endif
+        }
+      }
+    }
+  }
+
+  if (hullConstructionFailed) {
+    outFaces.clear();
+    numOutputTriangles = 0;
+  } else {
+    outFaces.resize(currentTriangleCount * kSpatialDimension);
+    std::memcpy(
+        outFaces.data(),
+        triangleIndices.data(),
+        currentTriangleCount * kSpatialDimension * sizeof(int));
+    numOutputTriangles = currentTriangleCount;
+  }
+}
+
+//==============================================================================
+// Eigen-based API implementation (Thin Wrapper)
+//==============================================================================
+// NOTE: After comprehensive benchmarking, we found that explicit SIMD using
+// Eigen makes operations 1.5-11x SLOWER than the compiler-optimized scalar
+// code. Modern compilers (GCC/Clang with -O3) already auto-vectorize the scalar
+// operations effectively. Therefore, we keep this as a thin wrapper to the
+// optimized legacy implementation.
+//==============================================================================
+
+template <typename S>
+inline void convexHull3dBuild(
+    const std::vector<Eigen::Matrix<S, 3, 1>>& inVertices,
+    std::vector<int>& outFaces,
+    int& numOutputTriangles)
+{
+  // Convert Eigen vectors to legacy format
+  // This thin wrapper provides modern C++ API while using the fastest
+  // implementation underneath (compiler auto-vectorized scalar code)
+  std::vector<ConvexHullVertex<S>> legacyVertices(inVertices.size());
+  for (size_t i = 0; i < inVertices.size(); ++i) {
+    legacyVertices[i].x = inVertices[i](0);
+    legacyVertices[i].y = inVertices[i](1);
+    legacyVertices[i].z = inVertices[i](2);
+  }
+
+  convexHull3dBuild(legacyVertices, outFaces, numOutputTriangles);
+}
+
+} // namespace dart::math::detail