Cloth3D.cpp

浏览该文件的文档.

#include "physics/cloth/Cloth3D.h"
#include "physics/cloth/ClothModel.h"
 
#include "common/Exception.h"
#include "graphics/Canvas.h"
#include "graphics/Graphics.h"
#include "graphics/Mesh.h"
#include "physics/Body3D.h"
#include "physics/World3D.h"
 
#include <algorithm>
#include <cmath>
#include <exception>
#include <limits>
#include <utility>
 
namespace eve::physics {
 
// Color lives in eve::graphics (see graphics/Canvas.h); keep the unqualified form.
using eve::graphics::Color;
using eve::graphics::Mesh;
 
namespace {
constexpr float kPi = 3.14159265358979323846f;
 
int64_t pairKey(int lo, int hi) { return (int64_t(lo) << 32) | int64_t(hi); }
 
// Minimal 3D vector helpers for the triangle-level self-collision pass.
struct V3 {
    float x = 0.f, y = 0.f, z = 0.f;
};
 
V3    sub(const V3& a, const V3& b) { return {a.x - b.x, a.y - b.y, a.z - b.z}; }
V3    add(const V3& a, const V3& b) { return {a.x + b.x, a.y + b.y, a.z + b.z}; }
V3    mul(float s, const V3& a) { return {s * a.x, s * a.y, s * a.z}; }
V3    fma(const V3& a, float s, const V3& b) { return {a.x * s + b.x, a.y * s + b.y, a.z * s + b.z}; }
float dot(const V3& a, const V3& b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
V3    cross(const V3& a, const V3& b) { return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x}; }
float vlen(const V3& a) { return std::sqrt(dot(a, a)); }
V3    vnorm(const V3& a) {
    const float l = vlen(a);
    return l > 1e-8f ? mul(1.f / l, a) : V3{0.f, 1.f, 0.f};
}
 
// Rotate vector v around the given unit axis by angle (right-hand rule).
V3 rotateAround(const V3& v, const V3& axis, float angle) {
    const float c  = std::cos(angle);
    const float sn = std::sin(angle);
    const V3    cv = cross(axis, v);
    const float d  = dot(axis, v);
    return add(add(mul(c, v), mul(sn, cv)), mul(d * (1.f - c), axis));
}
 
// Signed angle from a to b around the unit axis u (positive = CCW along +u).
float signedAngle(const V3& a, const V3& b, const V3& u) { return std::atan2(dot(cross(a, b), u), dot(a, b)); }
 
// Closest point on segment [a,b] to point p (Ericson, RTCD ch. 5).
V3 closestPointSegment(const V3& a, const V3& b, const V3& p) {
    const V3    ab   = sub(b, a);
    const float len2 = dot(ab, ab);
    if (len2 < 1e-12f) return a;
    const float t = std::clamp(dot(sub(p, a), ab) / len2, 0.f, 1.f);
    return fma(ab, t, a);
}
 
// Closest point on triangle (a,b,c) to point p (Ericson, RTCD ch. 5).
V3 closestPointTriangle(const V3& a, const V3& b, const V3& c, const V3& p) {
    const V3    ab = sub(b, a);
    const V3    ac = sub(c, a);
    const V3    ap = sub(p, a);
    const float d1 = dot(ab, ap);
    const float d2 = dot(ac, ap);
    if (d1 <= 0.f && d2 <= 0.f) return a;
 
    const V3    bp = sub(p, b);
    const float d3 = dot(ab, bp);
    const float d4 = dot(ac, bp);
    if (d3 >= 0.f && d4 <= d3) return b;
 
    const float vc = d1 * d4 - d3 * d2;
    if (vc <= 0.f && d1 >= 0.f && d3 <= 0.f) {
        const float t = d1 / (d1 - d3);
        return fma(ab, t, a);
    }
 
    const V3    cp = sub(p, c);
    const float d5 = dot(ab, cp);
    const float d6 = dot(ac, cp);
    if (d6 >= 0.f && d5 <= d6) return c;
 
    const float vb = d5 * d2 - d1 * d6;
    if (vb <= 0.f && d2 >= 0.f && d6 <= 0.f) {
        const float t = d2 / (d2 - d6);
        return fma(ac, t, a);
    }
 
    const float va = d3 * d6 - d5 * d4;
    if (va <= 0.f && (d4 - d3) >= 0.f && (d5 - d6) >= 0.f) {
        const float t = (d4 - d3) / ((d4 - d3) + (d5 - d6));
        return fma(sub(c, b), t, b);
    }
 
    const float denom = 1.f / (va + vb + vc);
    const float v     = vb * denom;
    const float w     = vc * denom;
    return add(fma(ab, v, a), mul(w, ac));
}
 
// Closest points between segments (p1,p2) and (q1,q2); returns distance.
float closestPointSegments(const V3& p1, const V3& p2, const V3& q1, const V3& q2, V3& cp1, V3& cp2) {
    const V3    d1 = sub(p2, p1);
    const V3    d2 = sub(q2, q1);
    const V3    r  = sub(p1, q1);
    const float a  = dot(d1, d1);
    const float e  = dot(d2, d2);
    const float f  = dot(d2, r);
    float       s  = 0.f;
    float       t  = 0.f;
    if (a <= 1e-12f && e <= 1e-12f) {
        s = t = 0.f;
    } else if (a <= 1e-12f) {
        s = 0.f;
        t = std::clamp(f / e, 0.f, 1.f);
    } else {
        const float c = dot(d1, r);
        if (e <= 1e-12f) {
            t = 0.f;
            s = std::clamp(-c / a, 0.f, 1.f);
        } else {
            const float b     = dot(d1, d2);
            const float denom = a * e - b * b;
            s                 = denom > 1e-12f ? std::clamp((b * f - c * e) / denom, 0.f, 1.f) : 0.f;
            t                 = (b * s + f) / e;
            if (t < 0.f) {
                t = 0.f;
                s = std::clamp(-c / a, 0.f, 1.f);
            } else if (t > 1.f) {
                t = 1.f;
                s = std::clamp((b - c) / a, 0.f, 1.f);
            }
        }
    }
    cp1 = fma(d1, s, p1);
    cp2 = fma(d2, t, q1);
    return vlen(sub(cp1, cp2));
}
 
bool pointInTriangle(const V3& p, const V3& a, const V3& b, const V3& c) {
    const V3 n  = cross(sub(b, a), sub(c, a));
    const V3 pa = cross(sub(b, a), sub(p, a));
    const V3 pb = cross(sub(c, b), sub(p, b));
    const V3 pc = cross(sub(a, c), sub(p, c));
    return dot(pa, n) >= -1e-8f && dot(pb, n) >= -1e-8f && dot(pc, n) >= -1e-8f;
}
 
// Closest distance between two triangles plus the indices of the vertices
// forming the closest feature on each side (1, 2 or 3 vertices).
float triangleDistance(const V3* A, const V3* B, int featA[3], int featB[3], int& countA, int& countB, V3& cpA,
                       V3& cpB) {
    float best = std::numeric_limits<float>::max();
    countA = countB = 0;
    for (int i = 0; i < 3; ++i) {
        const V3    cp = closestPointTriangle(B[0], B[1], B[2], A[i]);
        const float d  = vlen(sub(A[i], cp));
        if (d < best) {
            best     = d;
            cpA      = A[i];
            cpB      = cp;
            featA[0] = i;
            featB[0] = 0;
            featB[1] = 1;
            featB[2] = 2;
            countA   = 1;
            countB   = 3;
        }
    }
    for (int j = 0; j < 3; ++j) {
        const V3    cp = closestPointTriangle(A[0], A[1], A[2], B[j]);
        const float d  = vlen(sub(B[j], cp));
        if (d < best) {
            best     = d;
            cpA      = cp;
            cpB      = B[j];
            featA[0] = 0;
            featA[1] = 1;
            featA[2] = 2;
            featB[0] = j;
            countA   = 3;
            countB   = 1;
        }
    }
    const int aEdges[3][2] = {{0, 1}, {1, 2}, {2, 0}};
    const int bEdges[3][2] = {{0, 1}, {1, 2}, {2, 0}};
    for (const auto& ea : aEdges) {
        for (const auto& eb : bEdges) {
            V3          cp1;
            V3          cp2;
            const float d = closestPointSegments(A[ea[0]], A[ea[1]], B[eb[0]], B[eb[1]], cp1, cp2);
            if (d < best) {
                best     = d;
                cpA      = cp1;
                cpB      = cp2;
                featA[0] = ea[0];
                featA[1] = ea[1];
                featB[0] = eb[0];
                featB[1] = eb[1];
                countA   = 2;
                countB   = 2;
            }
        }
    }
    return best;
}
}  // namespace
 
Cloth3D::Cloth3D(int cols, int rows, float spacing, float originX, float originY, float originZ)
    : cols_(cols), rows_(rows), spacing_(spacing), originX_(originX), originY_(originY), originZ_(originZ) {
    auto model = ClothModel::grid(cols, rows, spacing, originX, originY, originZ);
    if (!model)
        throw Exception("%s", model.error() ? model.error()->message().c_str() : "Cloth3D grid model creation failed");
    initializeFromModel(model.value());
}
 
Cloth3D::Cloth3D(const ClothModel& model) { initializeFromModel(model); }
 
Cloth3D::~Cloth3D() { destroy(); }
 
void Cloth3D::destroy() { destroyed_ = true; }
 
void Cloth3D::reset() {
    particles_ = restParticles_;
    grabIndex_ = -1;
    forceX_ = forceY_ = forceZ_ = 0.f;
    interactStrength_           = 0.f;
}
 
void Cloth3D::rebuildLinks() {
    links_.clear();
    auto add = [&](int a, int b) {
        if (a < 0 || b < 0 || a >= getParticleCount() || b >= getParticleCount()) return;
        Link link;
        link.a             = a;
        link.b             = b;
        const Particle& pa = particles_[static_cast<size_t>(a)];
        const Particle& pb = particles_[static_cast<size_t>(b)];
        const float     dx = pb.x - pa.x;
        const float     dy = pb.y - pa.y;
        const float     dz = pb.z - pa.z;
        link.rest          = std::sqrt(dx * dx + dy * dy + dz * dz);
        if (link.rest > 1e-4f) links_.push_back(link);
    };
 
    for (int r = 0; r < rows_; ++r) {
        for (int c = 0; c < cols_; ++c) {
            const int i = r * cols_ + c;
            // Structural
            if (c + 1 < cols_) add(i, i + 1);
            if (r + 1 < rows_) add(i, i + cols_);
            // Shear
            if (c + 1 < cols_ && r + 1 < rows_) add(i, i + cols_ + 1);
            if (c > 0 && r + 1 < rows_) add(i, i + cols_ - 1);
            // Bend (every other)
            if (c + 2 < cols_) add(i, i + 2);
            if (r + 2 < rows_) add(i, i + cols_ * 2);
        }
    }
    buildLinkKeys();
}
 
void Cloth3D::buildLinkKeys() {
    linkKeys_.clear();
    for (const Link& link : links_) {
        const int lo = std::min(link.a, link.b);
        const int hi = std::max(link.a, link.b);
        linkKeys_.insert(pairKey(lo, hi));
    }
}
 
bool Cloth3D::areLinked(int a, int b) const {
    if (a == b) return true;
    const int lo = std::min(a, b);
    const int hi = std::max(a, b);
    return linkKeys_.find(pairKey(lo, hi)) != linkKeys_.end();
}
 
void Cloth3D::rebuildTriangles() {
    triangles_.clear();
    foldPairs_.clear();
 
    // Two triangles per quad, wound so the +Y side is the front face.
    std::unordered_map<int64_t, std::vector<int>> edgeTris;
    auto addTri = [&](int a, int b, int c, std::unordered_map<int64_t, std::vector<int>>& map) {
        Tri tri;
        tri.v[0]              = a;
        tri.v[1]              = b;
        tri.v[2]              = c;
        const int idx         = static_cast<int>(triangles_.size());
        const int edges[3][2] = {{a, b}, {b, c}, {c, a}};
        for (const auto& e : edges) {
            map[pairKey(std::min(e[0], e[1]), std::max(e[0], e[1]))].push_back(idx);
        }
        triangles_.push_back(tri);
    };
 
    for (int r = 0; r + 1 < rows_; ++r) {
        for (int c = 0; c + 1 < cols_; ++c) {
            const int a  = r * cols_ + c;
            const int b  = r * cols_ + c + 1;
            const int cc = (r + 1) * cols_ + c + 1;
            const int d  = (r + 1) * cols_ + c;
            addTri(a, d, b, edgeTris);
            addTri(d, cc, b, edgeTris);
        }
    }
 
    for (const auto& entry : edgeTris) {
        if (entry.second.size() != 2) continue;
        const Tri& t0 = triangles_[static_cast<size_t>(entry.second[0])];
        const Tri& t1 = triangles_[static_cast<size_t>(entry.second[1])];
        const int  e0 = int(entry.first >> 32);
        const int  e1 = static_cast<int>(static_cast<uint32_t>(entry.first));
        // Shared edge endpoints e0/e1; opposite vertices are the third of each tri.
        int k = -1;
        for (int v : t0.v) {
            if (v != e0 && v != e1) {
                k = v;
                break;
            }
        }
        int l = -1;
        for (int v : t1.v) {
            if (v != e0 && v != e1) {
                l = v;
                break;
            }
        }
        if (k < 0 || l < 0) continue;
        FoldPair pair;
        pair.a = e0;
        pair.b = e1;
        pair.k = k;
        pair.l = l;
        foldPairs_.push_back(pair);
    }
}
 
bool Cloth3D::validIndex(int index) const { return index >= 0 && index < getParticleCount(); }
 
void Cloth3D::setGravity(float gx, float gy, float gz) {
    gravityX_ = gx;
    gravityY_ = gy;
    gravityZ_ = gz;
}
 
void Cloth3D::setStiffness(float stiffness) { stiffness_ = std::clamp(stiffness, 0.f, 1.f); }
 
void Cloth3D::setStretchCompliance(float compliance) {
    stretchCompliance_ = std::isfinite(compliance) ? std::max(0.f, compliance) : 0.f;
}
 
void Cloth3D::setShearCompliance(float compliance) {
    shearCompliance_ = std::isfinite(compliance) ? std::max(0.f, compliance) : 0.f;
}
 
void Cloth3D::setBendCompliance(float compliance) {
    bendCompliance_ = std::isfinite(compliance) ? std::max(0.f, compliance) : 0.f;
}
 
void Cloth3D::setIterations(int iterations) { iterations_ = std::max(1, iterations); }
 
void Cloth3D::setDamping(float damping) { damping_ = std::clamp(damping, 0.f, 1.f); }
 
void Cloth3D::setParticleSize(float size) { particleSize_ = std::max(0.01f, size); }
 
void Cloth3D::setParticleMass(float mass) { particleMass_ = std::max(1e-4f, mass); }
 
void Cloth3D::initializeFromModel(const ClothModel& model) {
    cols_    = model.gridCols();
    rows_    = model.gridRows();
    spacing_ = model.gridSpacing();
    if (!model.particles().empty()) {
        originX_ = model.particles().front().x;
        originY_ = model.particles().front().y;
        originZ_ = model.particles().front().z;
    }
    particles_.clear();
    particles_.reserve(model.particles().size());
    for (const ClothModelParticle& source : model.particles()) {
        Particle particle;
        particle.x = particle.px = source.x;
        particle.y = particle.py = source.y;
        particle.z = particle.pz = source.z;
        particle.inverseMass      = source.inverseMass;
        particle.pinned          = source.inverseMass == 0.f;
        particles_.push_back(particle);
    }
    restParticles_ = particles_;
 
    links_.clear();
    links_.reserve(model.distanceConstraints().size());
    for (const ClothModelDistanceConstraint& source : model.distanceConstraints())
        links_.push_back({source.a, source.b, source.restLength, source.kind, 0.f});
 
    triangles_.clear();
    triangles_.reserve(model.triangles().size());
    for (const ClothModelTriangle& source : model.triangles())
        triangles_.push_back({{source.vertices[0], source.vertices[1], source.vertices[2]}});
 
    foldPairs_.clear();
    foldPairs_.reserve(model.foldConstraints().size());
    for (const ClothModelFoldConstraint& source : model.foldConstraints())
        foldPairs_.push_back({source.a, source.b, source.oppositeA, source.oppositeB});
    tethers_.clear();
    tethers_.reserve(model.tetherConstraints().size());
    for (const ClothModelTetherConstraint& source : model.tetherConstraints())
        tethers_.push_back({source.particle, source.anchor, source.maxLength, 0.f});
    restVolume_ = model.restVolume();
    buildLinkKeys();
}
 
void Cloth3D::setWindVelocity(float vx, float vy, float vz) {
    windVelocityX_ = vx;
    windVelocityY_ = vy;
    windVelocityZ_ = vz;
}
 
float Cloth3D::getWindVelocityX() const { return windVelocityX_; }
 
float Cloth3D::getWindVelocityY() const { return windVelocityY_; }
 
float Cloth3D::getWindVelocityZ() const { return windVelocityZ_; }
 
void Cloth3D::setAerodynamics(float airDensity, float dragCoefficient, float liftCoefficient) {
    airDensity_      = std::max(0.f, airDensity);
    dragCoefficient_ = std::max(0.f, dragCoefficient);
    liftCoefficient_ = std::max(0.f, liftCoefficient);
}
 
float Cloth3D::getAirDensity() const { return airDensity_; }
 
float Cloth3D::getDragCoefficient() const { return dragCoefficient_; }
 
float Cloth3D::getLiftCoefficient() const { return liftCoefficient_; }
 
void Cloth3D::setSelfCollision(bool on) { selfCollision_ = on; }
 
void Cloth3D::setFoldStiffness(float k) { foldStiffness_ = std::clamp(k, 0.f, 1.f); }
 
void Cloth3D::setMaxFoldAngle(float degrees) { maxFoldAngle_ = std::clamp(degrees, 0.f, 180.f) * kPi / 180.f; }
 
void Cloth3D::setBounds(float x, float y, float z, float w, float h, float d) {
    if (w <= 0.f || h <= 0.f || d <= 0.f) {
        clearBounds();
        return;
    }
    hasBounds_ = true;
    boundX_    = x;
    boundY_    = y;
    boundZ_    = z;
    boundW_    = w;
    boundH_    = h;
    boundD_    = d;
}
 
void Cloth3D::clearBounds() { hasBounds_ = false; }
 
void Cloth3D::setCollisionMaterial(float friction, float restitution) {
    collisionFriction_    = std::isfinite(friction) ? std::clamp(friction, 0.f, 1.f) : 0.f;
    collisionRestitution_ = std::isfinite(restitution) ? std::clamp(restitution, 0.f, 1.f) : 0.f;
}
 
void Cloth3D::setCollisionFilter(uint64_t categoryBits, uint64_t maskBits) {
    collisionCategoryBits_ = categoryBits;
    collisionMaskBits_     = maskBits;
}
 
void Cloth3D::pin(int index) {
    if (!validIndex(index)) throw Exception("Cloth3D.pin: index out of range");
    Particle& particle = particles_[static_cast<size_t>(index)];
    particle.pinned = true;
    particle.inverseMass = 0.f;
}
 
void Cloth3D::unpin(int index) {
    if (!validIndex(index)) throw Exception("Cloth3D.unpin: index out of range");
    Particle& particle = particles_[static_cast<size_t>(index)];
    particle.pinned = false;
    particle.inverseMass = 1.f;
}
 
void Cloth3D::pinTopRow() {
    for (int c = 0; c < cols_; ++c) pin(c);
}
 
bool Cloth3D::isPinned(int index) const {
    if (!validIndex(index)) return false;
    return particles_[static_cast<size_t>(index)].pinned;
}
 
void Cloth3D::setParticleInverseMass(int index, float inverseMass) {
    if (!validIndex(index)) throw Exception("Cloth3D.setParticleInverseMass: index out of range");
    Particle& particle = particles_[static_cast<size_t>(index)];
    particle.inverseMass = std::isfinite(inverseMass) ? std::max(0.f, inverseMass) : 0.f;
    particle.pinned      = particle.inverseMass == 0.f;
}
 
float Cloth3D::getParticleInverseMass(int index) const {
    return validIndex(index) ? particles_[static_cast<size_t>(index)].inverseMass : 0.f;
}
 
int Cloth3D::grabAt(float x, float y, float z, float radius) {
    grabIndex_ = -1;
    float best = radius * radius;
    for (int i = 0; i < getParticleCount(); ++i) {
        const Particle& p = particles_[static_cast<size_t>(i)];
        if (p.pinned) continue;
        const float dx = p.x - x;
        const float dy = p.y - y;
        const float dz = p.z - z;
        const float d2 = dx * dx + dy * dy + dz * dz;
        if (d2 <= best) {
            best       = d2;
            grabIndex_ = i;
        }
    }
    if (grabIndex_ >= 0) moveGrab(x, y, z);
    return grabIndex_;
}
 
void Cloth3D::moveGrab(float x, float y, float z) {
    grabX_ = x;
    grabY_ = y;
    grabZ_ = z;
    if (!validIndex(grabIndex_)) return;
    Particle& p = particles_[static_cast<size_t>(grabIndex_)];
    p.x         = x;
    p.y         = y;
    p.z         = z;
    p.px        = x;
    p.py        = y;
    p.pz        = z;
}
 
void Cloth3D::releaseGrab() { grabIndex_ = -1; }
 
void Cloth3D::applyForce(float fx, float fy, float fz) {
    forceX_ += fx;
    forceY_ += fy;
    forceZ_ += fz;
}
 
void Cloth3D::interactAt(float x, float y, float z, float radius, float strength) {
    interactX_        = x;
    interactY_        = y;
    interactZ_        = z;
    interactRadius_   = std::max(0.f, radius);
    interactStrength_ = strength;
}
 
void Cloth3D::setCollideWorld(World3D* world) { world_ = world; }
 
void Cloth3D::setColor(float r, float g, float b, float a) {
    colorR_ = r;
    colorG_ = g;
    colorB_ = b;
    colorA_ = a;
}
 
float Cloth3D::getParticleX(int index) const {
    if (!validIndex(index)) return 0.f;
    return particles_[static_cast<size_t>(index)].x;
}
 
float Cloth3D::getParticleY(int index) const {
    if (!validIndex(index)) return 0.f;
    return particles_[static_cast<size_t>(index)].y;
}
 
float Cloth3D::getParticleZ(int index) const {
    if (!validIndex(index)) return 0.f;
    return particles_[static_cast<size_t>(index)].z;
}
 
void Cloth3D::setParticlePosition(int index, float x, float y, float z) {
    if (!validIndex(index)) throw Exception("Cloth3D.setParticlePosition: index out of range");
    Particle& p = particles_[static_cast<size_t>(index)];
    p.x = p.px = x;
    p.y = p.py = y;
    p.z = p.pz = z;
}
 
int64_t Cloth3D::cellKey(int cx, int cy, int cz) const {
    // 21 bits per axis keeps the packed key inside a signed 64-bit int.
    return int64_t(uint32_t(cx) & 0x1fffffu) | (int64_t(uint32_t(cy) & 0x1fffffu) << 21) |
           (int64_t(uint32_t(cz) & 0x1fffffu) << 42);
}
 
void Cloth3D::rebuildHash() {
    hash_.clear();
    const float cell = std::max(1e-3f, particleSize_ * 2.f);
    const float inv  = 1.f / cell;
    for (int i = 0; i < getParticleCount(); ++i) {
        const Particle& p  = particles_[static_cast<size_t>(i)];
        const int       cx = int(std::floor(p.x * inv));
        const int       cy = int(std::floor(p.y * inv));
        const int       cz = int(std::floor(p.z * inv));
        hash_[cellKey(cx, cy, cz)].push_back(i);
    }
}
 
void Cloth3D::integrate(float dt) {
    if (dt <= 0.f) return;
    const float ax          = gravityX_ + forceX_;
    const float ay          = gravityY_ + forceY_;
    const float az          = gravityZ_ + forceZ_;
    const float damp        = 1.f - damping_;
    const bool  hasInteract = interactRadius_ > 0.f && interactStrength_ != 0.f;
 
    std::vector<float> aerodynamicAcceleration;
    if (airDensity_ > 0.f && (dragCoefficient_ > 0.f || liftCoefficient_ > 0.f)) {
        aerodynamicAcceleration.assign(particles_.size() * 3, 0.f);
        accumulateAerodynamicAcceleration(dt, aerodynamicAcceleration);
    }
 
    for (size_t particleIndex = 0; particleIndex < particles_.size(); ++particleIndex) {
        Particle& p = particles_[particleIndex];
        if (p.pinned) {
            p.px = p.x;
            p.py = p.y;
            p.pz = p.z;
            continue;
        }
        const float vx    = (p.x - p.px) * damp;
        const float vy    = (p.y - p.py) * damp;
        const float vz    = (p.z - p.pz) * damp;
        p.px              = p.x;
        p.py              = p.y;
        p.pz              = p.z;
        const float aeroX = aerodynamicAcceleration.empty() ? 0.f : aerodynamicAcceleration[particleIndex * 3];
        const float aeroY = aerodynamicAcceleration.empty() ? 0.f : aerodynamicAcceleration[particleIndex * 3 + 1];
        const float aeroZ = aerodynamicAcceleration.empty() ? 0.f : aerodynamicAcceleration[particleIndex * 3 + 2];
        p.x += vx + (ax + aeroX) * dt * dt;
        p.y += vy + (ay + aeroY) * dt * dt;
        p.z += vz + (az + aeroZ) * dt * dt;
        if (hasInteract) {
            const float dx = interactX_ - p.x;
            const float dy = interactY_ - p.y;
            const float dz = interactZ_ - p.z;
            const float r2 = dx * dx + dy * dy + dz * dz;
            const float R2 = interactRadius_ * interactRadius_;
            if (r2 < R2 && r2 > 1e-6f) {
                const float r = std::sqrt(r2);
                const float w = 1.f - r / interactRadius_;
                const float a = interactStrength_ * w * dt * dt;
                p.x += (dx / r) * a;
                p.y += (dy / r) * a;
                p.z += (dz / r) * a;
            }
        }
        constexpr float maxSpeed = 9.f;  // m/s
        const float     maxDisp  = maxSpeed * dt;
        const float     dvx      = p.x - p.px;
        const float     dvy      = p.y - p.py;
        const float     dvz      = p.z - p.pz;
        const float     v2       = dvx * dvx + dvy * dvy + dvz * dvz;
        if (v2 > maxDisp * maxDisp) {
            const float s = maxDisp / std::sqrt(v2);
            p.px          = p.x - dvx * s;
            p.py          = p.y - dvy * s;
            p.pz          = p.z - dvz * s;
        }
    }
}
 
void Cloth3D::accumulateAerodynamicAcceleration(float dt, std::vector<float>& accelerations) const {
    const float invDt   = dt > 1e-6f ? 1.f / dt : 0.f;
    const float invMass = 1.f / particleMass_;
    for (const Tri& tri : triangles_) {
        const Particle& a = particles_[static_cast<size_t>(tri.v[0])];
        const Particle& b = particles_[static_cast<size_t>(tri.v[1])];
        const Particle& c = particles_[static_cast<size_t>(tri.v[2])];
 
        const V3    ab{b.x - a.x, b.y - a.y, b.z - a.z};
        const V3    ac{c.x - a.x, c.y - a.y, c.z - a.z};
        const V3    areaNormal = cross(ab, ac);
        const float twiceArea  = vlen(areaNormal);
        if (twiceArea < 1e-8f) continue;
        V3 normal = mul(1.f / twiceArea, areaNormal);
 
        const V3    clothVelocity{((a.x - a.px) + (b.x - b.px) + (c.x - c.px)) * (invDt / 3.f),
                                  ((a.y - a.py) + (b.y - b.py) + (c.y - c.py)) * (invDt / 3.f),
                                  ((a.z - a.pz) + (b.z - b.pz) + (c.z - c.pz)) * (invDt / 3.f)};
        const V3    relativeWind{windVelocityX_ - clothVelocity.x, windVelocityY_ - clothVelocity.y,
                                 windVelocityZ_ - clothVelocity.z};
        const float speed = vlen(relativeWind);
        if (speed < 1e-5f) continue;
        const V3 windDirection = mul(1.f / speed, relativeWind);
        float    incidence     = dot(normal, windDirection);
        if (incidence < 0.f) {
            normal    = mul(-1.f, normal);
            incidence = -incidence;
        }
        if (incidence < 1e-5f) continue;
 
        const float dynamicPressureArea = 0.5f * airDensity_ * speed * speed * (twiceArea * 0.5f);
        const V3    drag                = mul(dragCoefficient_ * incidence * dynamicPressureArea, windDirection);
        V3          liftDirection       = sub(normal, mul(incidence, windDirection));
        const float liftLength          = vlen(liftDirection);
        if (liftLength > 1e-6f)
            liftDirection = mul(1.f / liftLength, liftDirection);
        else
            liftDirection = V3{0.f, 0.f, 0.f};
        const V3 lift         = mul(liftCoefficient_ * incidence * dynamicPressureArea, liftDirection);
        const V3 acceleration = mul(invMass / 3.f, add(drag, lift));
 
        for (int vertex : tri.v) {
            if (particles_[static_cast<size_t>(vertex)].pinned) continue;
            accelerations[static_cast<size_t>(vertex) * 3] += acceleration.x;
            accelerations[static_cast<size_t>(vertex) * 3 + 1] += acceleration.y;
            accelerations[static_cast<size_t>(vertex) * 3 + 2] += acceleration.z;
        }
    }
}
 
void Cloth3D::solveConstraints(float dt) {
    for (Link& link : links_) link.lambda = 0.f;
    for (Tether& tether : tethers_) tether.lambda = 0.f;
    for (SkinConstraint& skin : skinConstraints_) skin.lambda = 0.f;
    for (Attachment& attachment : attachments_) attachment.lambda = 0.f;
    volumeLambda_            = 0.f;
    const float invDtSquared = dt > 1e-8f ? 1.f / (dt * dt) : 0.f;
    for (int iter = 0; iter < iterations_; ++iter) {
        for (Link& link : links_) {
            Particle& a = particles_[static_cast<size_t>(link.a)];
            Particle& b = particles_[static_cast<size_t>(link.b)];
            if (a.pinned && b.pinned) continue;
            float       dx   = b.x - a.x;
            float       dy   = b.y - a.y;
            float       dz   = b.z - a.z;
            const float dist = std::sqrt(dx * dx + dy * dy + dz * dz);
            if (dist < 1e-5f) continue;
            float compliance = stretchCompliance_;
            if (link.kind == ClothConstraintKind::Shear)
                compliance = shearCompliance_;
            else if (link.kind == ClothConstraintKind::Bend)
                compliance = bendCompliance_;
            if (compliance > 0.f) {
                const float weightA     = a.pinned ? 0.f : a.inverseMass;
                const float weightB     = b.pinned ? 0.f : b.inverseMass;
                const float alpha       = compliance * invDtSquared;
                const float deltaLambda = (-(dist - link.rest) - alpha * link.lambda) / (weightA + weightB + alpha);
                link.lambda += deltaLambda;
                const float nx = dx / dist;
                const float ny = dy / dist;
                const float nz = dz / dist;
                a.x -= weightA * nx * deltaLambda;
                a.y -= weightA * ny * deltaLambda;
                a.z -= weightA * nz * deltaLambda;
                b.x += weightB * nx * deltaLambda;
                b.y += weightB * ny * deltaLambda;
                b.z += weightB * nz * deltaLambda;
                continue;
            }
            const float diff = (dist - link.rest) / dist * stiffness_;
            if (a.pinned) {
                b.x -= dx * diff;
                b.y -= dy * diff;
                b.z -= dz * diff;
            } else if (b.pinned) {
                a.x += dx * diff;
                a.y += dy * diff;
                a.z += dz * diff;
            } else {
                const float half = diff * 0.5f;
                a.x += dx * half;
                a.y += dy * half;
                a.z += dz * half;
                b.x -= dx * half;
                b.y -= dy * half;
                b.z -= dz * half;
            }
        }
        solveTetherConstraints(dt);
        solveVolumeConstraint(dt);
        solveSkinConstraints(dt);
        solveAttachments(dt);
        if (validIndex(grabIndex_)) {
            Particle& g = particles_[static_cast<size_t>(grabIndex_)];
            g.x         = grabX_;
            g.y         = grabY_;
            g.z         = grabZ_;
            g.px        = grabX_;
            g.py        = grabY_;
            g.pz        = grabZ_;
        }
    }
}
 
void Cloth3D::solveFoldConstraint() {
    if (foldStiffness_ <= 0.f || maxFoldAngle_ >= kPi || foldPairs_.empty()) return;
    // Fold pairs share edge (a,b) with opposite vertices k (tri 1) and l (tri 2).
    // For a flat sheet the in-plane directions m1,m2 point to opposite sides
    // (theta = pi); folding brings them toward the same side (theta -> 0). When
    // theta drops below pi - maxFoldAngle, rotate both triangles rigidly around
    // the shared edge to reopen the fold to the limit.
    const float thetaMin = kPi - maxFoldAngle_;
    const float s        = foldStiffness_;
    for (const FoldPair& pair : foldPairs_) {
        Particle& pa = particles_[static_cast<size_t>(pair.a)];
        Particle& pb = particles_[static_cast<size_t>(pair.b)];
        Particle& pk = particles_[static_cast<size_t>(pair.k)];
        Particle& pl = particles_[static_cast<size_t>(pair.l)];
        if (pa.pinned && pb.pinned && pk.pinned && pl.pinned) continue;
 
        const V3 paV{pa.x, pa.y, pa.z};
        const V3 pbV{pb.x, pb.y, pb.z};
        V3       pkV{pk.x, pk.y, pk.z};
        V3       plV{pl.x, pl.y, pl.z};
 
        const V3    u  = vnorm(sub(pbV, paV));
        const V3    vk = sub(pkV, paV);
        const V3    vl = sub(plV, paV);
        const V3    h1 = sub(vk, mul(dot(vk, u), u));
        const V3    h2 = sub(vl, mul(dot(vl, u), u));
        const float b1 = vlen(h1);
        const float b2 = vlen(h2);
        if (b1 < 1e-6f || b2 < 1e-6f) continue;
        const V3    m1    = mul(1.f / b1, h1);
        const V3    m2    = mul(1.f / b2, h2);
        const float theta = std::acos(std::clamp(dot(m1, m2), -1.f, 1.f));
        if (theta >= thetaMin) continue;
 
        // Reopen the fold along its own path: a pure fold moves m1/m2 from
        // opposite (flat, theta = pi) toward the same side (theta -> 0) with a
        // fixed rotation sign; rotate both triangles back by delta/2 each.
        const float phi   = signedAngle(m1, m2, u);
        const float sign  = phi >= 0.f ? 1.f : -1.f;
        const float delta = thetaMin - theta;
        const float a1    = -sign * delta * 0.5f * s;
        const float a2    = sign * delta * 0.5f * s;
        if (!pk.pinned) {
            pkV  = add(paV, rotateAround(vk, u, a1 * s));
            pk.x = pkV.x;
            pk.y = pkV.y;
            pk.z = pkV.z;
        }
        if (!pl.pinned) {
            plV  = add(paV, rotateAround(vl, u, a2 * s));
            pl.x = plV.x;
            pl.y = plV.y;
            pl.z = plV.z;
        }
    }
}
 
void Cloth3D::solveSelfCollision() {
    if (!selfCollision_ || getParticleCount() == 0) return;
    const float minDist = particleSize_ * 2.f;
    if (minDist <= 0.f) return;
    rebuildHash();
    const float cell = std::max(1e-3f, minDist);
    const float inv  = 1.f / cell;
    for (int i = 0; i < getParticleCount(); ++i) {
        Particle& pi = particles_[static_cast<size_t>(i)];
        const int cx = int(std::floor(pi.x * inv));
        const int cy = int(std::floor(pi.y * inv));
        const int cz = int(std::floor(pi.z * inv));
        for (int oz = -1; oz <= 1; ++oz) {
            for (int oy = -1; oy <= 1; ++oy) {
                for (int ox = -1; ox <= 1; ++ox) {
                    auto it = hash_.find(cellKey(cx + ox, cy + oy, cz + oz));
                    if (it == hash_.end()) continue;
                    for (int j : it->second) {
                        if (j <= i) continue;
                        if (areLinked(i, j)) continue;
                        Particle&   pj = particles_[static_cast<size_t>(j)];
                        float       dx = pj.x - pi.x;
                        float       dy = pj.y - pi.y;
                        float       dz = pj.z - pi.z;
                        const float d2 = dx * dx + dy * dy + dz * dz;
                        if (d2 >= minDist * minDist || d2 < 1e-8f) continue;
                        if (pi.pinned && pj.pinned) continue;
                        const float d    = std::sqrt(d2);
                        const float corr = std::min(0.5f, (minDist - d) / d);
                        float       wa   = 0.5f;
                        float       wb   = 0.5f;
                        if (pi.pinned && !pj.pinned) {
                            wa = 0.f;
                            wb = 1.f;
                        } else if (pj.pinned && !pi.pinned) {
                            wa = 1.f;
                            wb = 0.f;
                        }
                        pi.x -= dx * corr * wa;
                        pi.y -= dy * corr * wa;
                        pi.z -= dz * corr * wa;
                        pj.x += dx * corr * wb;
                        pj.y += dy * corr * wb;
                        pj.z += dz * corr * wb;
                    }
                }
            }
        }
    }
}
 
void Cloth3D::solveSelfCollisionTriangles() {
    if (!selfCollision_ || triangles_.size() < 2) return;
    const float thickness = std::max(1e-3f, particleSize_ * 2.f);
    const int   triCount  = static_cast<int>(triangles_.size());
 
    std::vector<V3> pos(particles_.size());
    for (size_t i = 0; i < particles_.size(); ++i) {
        pos[i] = {particles_[i].x, particles_[i].y, particles_[i].z};
    }
 
    const float                                   cell = thickness;
    const float                                   inv  = 1.f / cell;
    std::unordered_map<int64_t, std::vector<int>> triHash;
    auto                                          triCellKey = [](int cx, int cy, int cz) -> int64_t {
        return int64_t(uint32_t(cx) & 0x1fffffu) | (int64_t(uint32_t(cy) & 0x1fffffu) << 21) |
               (int64_t(uint32_t(cz) & 0x1fffffu) << 42);
    };
    const auto insertTri = [&](int ti) {
        const Tri& tri = triangles_[static_cast<size_t>(ti)];
        V3         mn  = pos[static_cast<size_t>(tri.v[0])];
        V3         mx  = mn;
        for (int k = 1; k < 3; ++k) {
            const V3& p = pos[static_cast<size_t>(tri.v[k])];
            mn.x        = std::min(mn.x, p.x);
            mn.y        = std::min(mn.y, p.y);
            mn.z        = std::min(mn.z, p.z);
            mx.x        = std::max(mx.x, p.x);
            mx.y        = std::max(mx.y, p.y);
            mx.z        = std::max(mx.z, p.z);
        }
        const V3 pad{thickness, thickness, thickness};
        mn           = sub(mn, pad);
        mx           = add(mx, pad);
        const int x0 = int(std::floor(mn.x * inv));
        const int y0 = int(std::floor(mn.y * inv));
        const int z0 = int(std::floor(mn.z * inv));
        const int x1 = int(std::floor(mx.x * inv));
        const int y1 = int(std::floor(mx.y * inv));
        const int z1 = int(std::floor(mx.z * inv));
        for (int cz = z0; cz <= z1; ++cz)
            for (int cy = y0; cy <= y1; ++cy)
                for (int cx = x0; cx <= x1; ++cx) triHash[triCellKey(cx, cy, cz)].push_back(ti);
    };
    for (int ti = 0; ti < triCount; ++ti) insertTri(ti);
 
    const auto sharesVertex = [&](const Tri& a, const Tri& b) {
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
                if (a.v[i] == b.v[j]) return true;
        return false;
    };
 
    std::unordered_set<int64_t> processed;
    std::vector<int>            cells;
    int                         featA[3];
    int                         featB[3];
 
    for (int i = 0; i < triCount; ++i) {
        const Tri& triI = triangles_[static_cast<size_t>(i)];
        const V3&  pa0  = pos[static_cast<size_t>(triI.v[0])];
        V3         mn   = pa0;
        V3         mx   = mn;
        for (int k = 1; k < 3; ++k) {
            const V3& p = pos[static_cast<size_t>(triI.v[k])];
            mn.x        = std::min(mn.x, p.x);
            mn.y        = std::min(mn.y, p.y);
            mn.z        = std::min(mn.z, p.z);
            mx.x        = std::max(mx.x, p.x);
            mx.y        = std::max(mx.y, p.y);
            mx.z        = std::max(mx.z, p.z);
        }
        const V3 pad{thickness, thickness, thickness};
        mn           = sub(mn, pad);
        mx           = add(mx, pad);
        const int x0 = int(std::floor(mn.x * inv));
        const int y0 = int(std::floor(mn.y * inv));
        const int z0 = int(std::floor(mn.z * inv));
        const int x1 = int(std::floor(mx.x * inv));
        const int y1 = int(std::floor(mx.y * inv));
        const int z1 = int(std::floor(mx.z * inv));
        cells.clear();
        for (int cz = z0; cz <= z1; ++cz)
            for (int cy = y0; cy <= y1; ++cy)
                for (int cx = x0; cx <= x1; ++cx) {
                    auto it = triHash.find(triCellKey(cx, cy, cz));
                    if (it != triHash.end()) cells.insert(cells.end(), it->second.begin(), it->second.end());
                }
        for (int j : cells) {
            if (j <= i) continue;
            const int64_t key = pairKey(i, j);
            if (!processed.insert(key).second) continue;
            const Tri& triJ = triangles_[static_cast<size_t>(j)];
            if (sharesVertex(triI, triJ)) continue;
 
            const V3    A[3] = {pos[static_cast<size_t>(triI.v[0])], pos[static_cast<size_t>(triI.v[1])],
                                pos[static_cast<size_t>(triI.v[2])]};
            const V3    B[3] = {pos[static_cast<size_t>(triJ.v[0])], pos[static_cast<size_t>(triJ.v[1])],
                                pos[static_cast<size_t>(triJ.v[2])]};
            V3          cpA;
            V3          cpB;
            int         countA = 0;
            int         countB = 0;
            const float d      = triangleDistance(A, B, featA, featB, countA, countB, cpA, cpB);
            if (d >= thickness) continue;
 
            V3 n = vnorm(sub(cpB, cpA));
            if (vlen(sub(cpB, cpA)) < 1e-6f) {
                n = vnorm(cross(sub(B[1], B[0]), sub(B[2], B[0])));
            }
            const float corr = (thickness - d) * stiffness_;
            if (corr <= 0.f) continue;
 
            const auto freeCount = [&](const int* feat, int count) {
                int free = 0;
                for (int k = 0; k < count; ++k)
                    if (!particles_[static_cast<size_t>(feat[k])].pinned) ++free;
                return free;
            };
            const int freeA = freeCount(featA, countA);
            const int freeB = freeCount(featB, countB);
            if (freeA == 0 && freeB == 0) continue;
            const float wA          = freeA > 0 ? (freeB > 0 ? 0.5f : 1.f) : 0.f;
            const float wB          = freeB > 0 ? (freeA > 0 ? 0.5f : 1.f) : 0.f;
            const auto  pushFeature = [&](const int* feat, int count, const V3& dir) {
                for (int k = 0; k < count; ++k) {
                    Particle& pv = particles_[static_cast<size_t>(feat[k])];
                    if (pv.pinned) continue;
                    pv.x += dir.x;
                    pv.y += dir.y;
                    pv.z += dir.z;
                }
            };
            if (freeA > 0) {
                const V3 dir = mul(-corr * wA / float(freeA), n);
                pushFeature(featA, countA, dir);
            }
            if (freeB > 0) {
                const V3 dir = mul(corr * wB / float(freeB), n);
                pushFeature(featB, countB, dir);
            }
        }
    }
 
    // Vertex-through-face: push any vertex that has crossed into the prism of a
    // non-adjacent triangle back out along the face normal.
    for (int i = 0; i < triCount; ++i) {
        const Tri& triI = triangles_[static_cast<size_t>(i)];
        const V3   A[3] = {pos[static_cast<size_t>(triI.v[0])], pos[static_cast<size_t>(triI.v[1])],
                           pos[static_cast<size_t>(triI.v[2])]};
        const V3   nI   = vnorm(cross(sub(A[1], A[0]), sub(A[2], A[0])));
        for (int j = i + 1; j < triCount; ++j) {
            const Tri& triJ = triangles_[static_cast<size_t>(j)];
            if (sharesVertex(triI, triJ)) continue;
            const V3 B[3] = {pos[static_cast<size_t>(triJ.v[0])], pos[static_cast<size_t>(triJ.v[1])],
                             pos[static_cast<size_t>(triJ.v[2])]};
            const V3 nJ   = vnorm(cross(sub(B[1], B[0]), sub(B[2], B[0])));
            // Vertices of tri i against the face of tri j.
            for (int k = 0; k < 3; ++k) {
                Particle& pv = particles_[static_cast<size_t>(triI.v[k])];
                if (pv.pinned) continue;
                const V3    va = A[k];
                const float d  = dot(sub(va, B[0]), nJ);
                if (std::fabs(d) >= thickness) continue;
                const V3 q = add(va, mul(-d, nJ));
                if (!pointInTriangle(q, B[0], B[1], B[2])) continue;
                const float push = (thickness - std::fabs(d)) * stiffness_ * 0.5f;
                const V3    dir  = mul(d >= 0.f ? push : -push, nJ);
                pv.x += dir.x;
                pv.y += dir.y;
                pv.z += dir.z;
            }
            // Vertices of tri j against the face of tri i.
            for (int k = 0; k < 3; ++k) {
                Particle& pv = particles_[static_cast<size_t>(triJ.v[k])];
                if (pv.pinned) continue;
                const V3    vb = B[k];
                const float d  = dot(sub(vb, A[0]), nI);
                if (std::fabs(d) >= thickness) continue;
                const V3 q = add(vb, mul(-d, nI));
                if (!pointInTriangle(q, A[0], A[1], A[2])) continue;
                const float push = (thickness - std::fabs(d)) * stiffness_ * 0.5f;
                const V3    dir  = mul(d >= 0.f ? push : -push, nI);
                pv.x += dir.x;
                pv.y += dir.y;
                pv.z += dir.z;
            }
        }
    }
}
 
void Cloth3D::collideWorld(float dt) {
    if (!world_ || !world_->isValid() || particleSize_ <= 0.f) return;
    const float    invDt = dt > 1e-6f ? 1.f / dt : 0.f;
    ClothContact3D contact;
    for (int i = 0; i < getParticleCount(); ++i) {
        Particle& p = particles_[static_cast<size_t>(i)];
        if (p.pinned) continue;
        if (world_->pointProbeFiltered(p.x, p.y, p.z, particleSize_, &contact, collisionCategoryBits_,
                                       collisionMaskBits_) != ClothProbeStatus::Hit ||
            !contact.hit)
            continue;
        const float vpx = (p.x - p.px) * invDt;
        const float vpy = (p.y - p.py) * invDt;
        const float vpz = (p.z - p.pz) * invDt;
        p.x += contact.nx * contact.depth;
        p.y += contact.ny * contact.depth;
        p.z += contact.nz * contact.depth;
 
        float      vbx      = 0.f;
        float      vby      = 0.f;
        float      vbz      = 0.f;
        float      bodyMass = 0.f;
        const bool dynamic  = contact.body != nullptr && contact.body->getType() == "dynamic";
        if (dynamic) {
            vbx      = contact.body->getLinearVelocityX();
            vby      = contact.body->getLinearVelocityY();
            vbz      = contact.body->getLinearVelocityZ();
            bodyMass = contact.body->getMass();
        }
        const float vn = (vpx - vbx) * contact.nx + (vpy - vby) * contact.ny + (vpz - vbz) * contact.nz;
        if (vn < 0.f) {
            const float restitution = std::max(collisionRestitution_, contact.restitution);
            const float friction = std::clamp(std::sqrt(collisionFriction_ * std::max(0.f, contact.friction)), 0.f, 1.f);
            const float relativeX = vpx - vbx;
            const float relativeY = vpy - vby;
            const float relativeZ = vpz - vbz;
            const float tangentX = relativeX - vn * contact.nx;
            const float tangentY = relativeY - vn * contact.ny;
            const float tangentZ = relativeZ - vn * contact.nz;
            const float nextNormal = -vn * restitution;
            const float nextVx = vbx + tangentX * (1.f - friction) + contact.nx * nextNormal;
            const float nextVy = vby + tangentY * (1.f - friction) + contact.ny * nextNormal;
            const float nextVz = vbz + tangentZ * (1.f - friction) + contact.nz * nextNormal;
            p.px = p.x - nextVx * dt;
            p.py = p.y - nextVy * dt;
            p.pz = p.z - nextVz * dt;
            const float     m           = particleMass_;
            const float     reduced     = bodyMass > 0.f ? (m * bodyMass) / (m + bodyMass) : m;
            float           j           = -(1.f + restitution) * vn * reduced;
            const float     maxKick     = 9.f;  // m/s
            j                           = std::min(j, maxKick * m);
            const float kick            = (j / m) * dt;
            p.x += contact.nx * kick;
            p.y += contact.ny * kick;
            p.z += contact.nz * kick;
            if (dynamic && bodyMass > 0.f) {
                contact.body->applyLinearImpulse(-contact.nx * j, -contact.ny * j, -contact.nz * j);
            }
        }
    }
}
 
void Cloth3D::collideBounds() {
    if (!hasBounds_) return;
    const float     minX   = boundX_;
    const float     minY   = boundY_;
    const float     minZ   = boundZ_;
    const float     maxX   = boundX_ + boundW_;
    const float     maxY   = boundY_ + boundH_;
    const float     maxZ   = boundZ_ + boundD_;
    const float bounce = collisionRestitution_;
 
    for (Particle& p : particles_) {
        if (p.pinned) continue;
        if (p.x < minX) {
            const float vx = p.x - p.px;
            p.x            = minX;
            p.px           = p.x + vx * bounce;
        } else if (p.x > maxX) {
            const float vx = p.x - p.px;
            p.x            = maxX;
            p.px           = p.x + vx * bounce;
        }
        if (p.y < minY) {
            const float vy = p.y - p.py;
            p.y            = minY;
            p.py           = p.y + vy * bounce;
        } else if (p.y > maxY) {
            const float vy = p.y - p.py;
            p.y            = maxY;
            p.py           = p.y + vy * bounce;
        }
        if (p.z < minZ) {
            const float vz = p.z - p.pz;
            p.z            = minZ;
            p.pz           = p.z + vz * bounce;
        } else if (p.z > maxZ) {
            const float vz = p.z - p.pz;
            p.z            = maxZ;
            p.pz           = p.z + vz * bounce;
        }
    }
}
 
void Cloth3D::update(float dt) {
    if (destroyed_) return;
    if (dt < 0.f) dt = 0.f;
    if (dt > 0.05f) dt = 0.05f;
 
    updateSubsteps(dt, 2);
}
 
void Cloth3D::updateSubsteps(float dt, int substeps) {
    if (destroyed_ || substeps < 1) return;
 
    const float h = dt / float(substeps);
    for (int s = 0; s < substeps; ++s) {
        integrate(h);
        applyAutomaticTearing();
        solveConstraints(h);
        solveFoldConstraint();
        solveSelfCollision();
        solveSelfCollisionTriangles();
        collideWorld(h);
        collideBounds();
        if (grabIndex_ >= 0) {
            Particle& p = particles_[static_cast<size_t>(grabIndex_)];
            p.px        = p.x;
            p.py        = p.y;
            p.pz        = p.z;
        }
    }
    forceX_           = 0.f;
    forceY_           = 0.f;
    forceZ_           = 0.f;
    interactStrength_ = 0.f;
}
 
eve::Result<void> Cloth3D::step(const eve::SimulationStep& stepValue, const SimulationSettings& settings) {
    if (destroyed_)
        return eve::Result<void>::failure(eve::Diagnostic::error(
            eve::DiagnosticCode::PreconditionViolation, "Cannot step a destroyed cloth", "physics.cloth3d.step"));
    auto valid = detail::validateSimulationStep(stepValue, settings, observation_);
    if (!valid) return valid;
    auto next = detail::advanceSimulationObservation(observation_, stepValue);
    if (!next) return eve::Result<void>::failure(next.status());
    try {
        updateSubsteps(static_cast<float>(stepValue.delta.seconds()), settings.subStepCount);
    } catch (const std::exception& error) {
        return eve::Result<void>::failure(eve::Diagnostic::error(
            eve::DiagnosticCode::Failed, std::string("Cloth3D step failed: ") + error.what(), "physics.cloth3d.step"));
    } catch (...) {
        return eve::Result<void>::failure(eve::Diagnostic::error(
            eve::DiagnosticCode::Failed, "Cloth3D step failed with an unknown exception", "physics.cloth3d.step"));
    }
    observation_ = std::move(next).takeValue();
    return eve::Result<void>::success(eve::Status::success(eve::StatusCode::Applied));
}
 
eve::Result<void> Cloth3D::restoreObservation(const SimulationObservation& observation) {
    auto valid = detail::validateSimulationObservation(observation, "physics.cloth3d.restoreObservation");
    if (!valid) return valid;
    if (destroyed_)
        return eve::Result<void>::failure(eve::Diagnostic::error(eve::DiagnosticCode::PreconditionViolation,
                                                                 "Cannot restore a destroyed cloth",
                                                                 "physics.cloth3d.restoreObservation"));
    observation_ = observation;
    return eve::Result<void>::success(eve::Status::success(eve::StatusCode::Applied));
}
 
void Cloth3D::draw(graphics::Graphics* gfx) {
    if (!gfx || destroyed_ || getParticleCount() < 4) return;
 
    // Double-sided mesh: front vertices carry the face normals, back vertices
    // duplicate positions with flipped normals and reversed winding.
    const int gridVertices = cols_ * rows_;
    const int vertexCount  = gridVertices * 2;
    const int quadCount    = (cols_ - 1) * (rows_ - 1);
    const int indexCount   = quadCount * 12;
 
    std::vector<float> pos(static_cast<size_t>(vertexCount) * 3);
    std::vector<float> nrm(static_cast<size_t>(vertexCount) * 3, 0.f);
    std::vector<float> uv(static_cast<size_t>(vertexCount) * 2);
    for (int r = 0; r < rows_; ++r) {
        for (int c = 0; c < cols_; ++c) {
            const size_t    i = static_cast<size_t>(r * cols_ + c);
            const Particle& p = particles_[i];
            const float     u = cols_ > 1 ? float(c) / float(cols_ - 1) : 0.f;
            const float     v = rows_ > 1 ? float(r) / float(rows_ - 1) : 0.f;
            for (int side = 0; side < 2; ++side) {
                const size_t vi = i + static_cast<size_t>(side) * gridVertices;
                pos[vi * 3 + 0] = p.x;
                pos[vi * 3 + 1] = p.y;
                pos[vi * 3 + 2] = p.z;
                uv[vi * 2 + 0]  = u;
                uv[vi * 2 + 1]  = v;
            }
        }
    }
 
    // Accumulate face normals.
    for (const Tri& tri : triangles_) {
        const float* pa  = &pos[static_cast<size_t>(tri.v[0]) * 3];
        const float* pb  = &pos[static_cast<size_t>(tri.v[1]) * 3];
        const float* pc  = &pos[static_cast<size_t>(tri.v[2]) * 3];
        float        e1x = pb[0] - pa[0], e1y = pb[1] - pa[1], e1z = pb[2] - pa[2];
        float        e2x = pc[0] - pa[0], e2y = pc[1] - pa[1], e2z = pc[2] - pa[2];
        float        nx  = e1y * e2z - e1z * e2y;
        float        ny  = e1z * e2x - e1x * e2z;
        float        nz  = e1x * e2y - e1y * e2x;
        const float  len = std::sqrt(nx * nx + ny * ny + nz * nz);
        if (len > 1e-8f) {
            nx /= len;
            ny /= len;
            nz /= len;
        } else {
            nx = 0.f;
            ny = 1.f;
            nz = 0.f;
        }
        for (int v : tri.v) {
            nrm[static_cast<size_t>(v) * 3 + 0] += nx;
            nrm[static_cast<size_t>(v) * 3 + 1] += ny;
            nrm[static_cast<size_t>(v) * 3 + 2] += nz;
            nrm[(static_cast<size_t>(v) + gridVertices) * 3 + 0] -= nx;
            nrm[(static_cast<size_t>(v) + gridVertices) * 3 + 1] -= ny;
            nrm[(static_cast<size_t>(v) + gridVertices) * 3 + 2] -= nz;
        }
    }
    for (int i = 0; i < gridVertices; ++i) {
        const float nx  = nrm[static_cast<size_t>(i) * 3 + 0];
        const float ny  = nrm[static_cast<size_t>(i) * 3 + 1];
        const float nz  = nrm[static_cast<size_t>(i) * 3 + 2];
        const float len = std::sqrt(nx * nx + ny * ny + nz * nz);
        for (int side = 0; side < 2; ++side) {
            const size_t vi   = static_cast<size_t>(i) + static_cast<size_t>(side) * gridVertices;
            const float  sign = side == 0 ? 1.f : -1.f;
            if (len > 1e-8f) {
                nrm[vi * 3 + 0] = sign * nx / len;
                nrm[vi * 3 + 1] = sign * ny / len;
                nrm[vi * 3 + 2] = sign * nz / len;
            } else {
                nrm[vi * 3 + 1] = sign;
            }
        }
    }
 
    if (!mesh_ || meshVertexCount_ != vertexCount || meshIndexCount_ != indexCount) {
        std::vector<uint32_t> indices(static_cast<size_t>(indexCount));
        int                   out = 0;
        for (int r = 0; r + 1 < rows_; ++r) {
            for (int c = 0; c + 1 < cols_; ++c) {
                const uint32_t a  = static_cast<uint32_t>(r * cols_ + c);
                const uint32_t b  = static_cast<uint32_t>(r * cols_ + c + 1);
                const uint32_t cc = static_cast<uint32_t>((r + 1) * cols_ + c + 1);
                const uint32_t d  = static_cast<uint32_t>((r + 1) * cols_ + c);
                // Front faces (CCW when viewed from +Y).
                indices[static_cast<size_t>(out) + 0] = a;
                indices[static_cast<size_t>(out) + 1] = d;
                indices[static_cast<size_t>(out) + 2] = b;
                indices[static_cast<size_t>(out) + 3] = d;
                indices[static_cast<size_t>(out) + 4] = cc;
                indices[static_cast<size_t>(out) + 5] = b;
                // Back faces (reversed winding on the duplicated vertex set).
                const uint32_t oa                      = a + static_cast<uint32_t>(gridVertices);
                const uint32_t ob                      = b + static_cast<uint32_t>(gridVertices);
                const uint32_t occ                     = cc + static_cast<uint32_t>(gridVertices);
                const uint32_t od                      = d + static_cast<uint32_t>(gridVertices);
                indices[static_cast<size_t>(out) + 6]  = ob;
                indices[static_cast<size_t>(out) + 7]  = od;
                indices[static_cast<size_t>(out) + 8]  = oa;
                indices[static_cast<size_t>(out) + 9]  = ob;
                indices[static_cast<size_t>(out) + 10] = occ;
                indices[static_cast<size_t>(out) + 11] = od;
                out += 12;
            }
        }
        mesh_ = gfx->newMeshFromArrays(pos.data(), nrm.data(), uv.data(), vertexCount, indices.data(), indexCount);
        meshVertexCount_ = vertexCount;
        meshIndexCount_  = indexCount;
    } else {
        gfx->updateMeshVertices(mesh_, pos.data(), nrm.data(), uv.data(), vertexCount, nullptr, 0);
    }
 
    if (mesh_) {
        gfx->drawMesh(mesh_, glm::mat4(1.f), nullptr, Color(colorR_, colorG_, colorB_, colorA_));
    }
}
 
}  // namespace eve::physics