okooooooo/EVEngine
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
658d19b88970c8f7eece28faea2a8b59fd5cf6f1
EVEngine/src/modules/physics/Cloth3D.cpp
T

1221 lines
44 KiB
C++
Raw Blame History

#include "physics/Cloth3D.h"
#include "common/Exception.h"
#include "graphics/Graphics.h"
#include "graphics/Canvas.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) {
if (cols_ < 2 || rows_ < 2)
throw Exception("Cloth3D: cols and rows must be >= 2");
if (spacing_ <= 0.f)
throw Exception("Cloth3D: spacing must be > 0");
particles_.resize(static_cast<size_t>(cols_ * rows_));
for (int r = 0; r < rows_; ++r) {
for (int c = 0; c < cols_; ++c) {
Particle &p = particles_[static_cast<size_t>(r * cols_ + c)];
p.x = p.px = originX_ + float(c) * spacing_;
p.y = p.py = originY_;
p.z = p.pz = originZ_ + float(r) * spacing_;
p.pinned = false;
}
}
rebuildLinks();
rebuildTriangles();
pinTopRow();
}
Cloth3D::~Cloth3D() { destroy(); }
void Cloth3D::destroy() { destroyed_ = true; }
void Cloth3D::reset() {
if (cols_ < 2 || rows_ < 2) return;
particles_.clear();
particles_.resize(static_cast<size_t>(cols_ * rows_));
for (int r = 0; r < rows_; ++r) {
for (int c = 0; c < cols_; ++c) {
Particle &p = particles_[static_cast<size_t>(r * cols_ + c)];
p.x = p.px = originX_ + float(c) * spacing_;
p.y = p.py = originY_;
p.z = p.pz = originZ_ + float(r) * spacing_;
p.pinned = false;
}
}
rebuildLinks();
rebuildTriangles();
pinTopRow();
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::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::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::pin(int index) {
if (!validIndex(index)) throw Exception("Cloth3D.pin: index out of range");
particles_[static_cast<size_t>(index)].pinned = true;
}
void Cloth3D::unpin(int index) {
if (!validIndex(index)) throw Exception("Cloth3D.unpin: index out of range");
particles_[static_cast<size_t>(index)].pinned = false;
}
void Cloth3D::pinTopRow() {
for (int c = 0; c < cols_; ++c)
particles_[static_cast<size_t>(c)].pinned = true;
}
bool Cloth3D::isPinned(int index) const {
if (!validIndex(index)) return false;
return particles_[static_cast<size_t>(index)].pinned;
}
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;
for (Particle &p : particles_) {
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;
p.x += vx + ax * dt * dt;
p.y += vy + ay * dt * dt;
p.z += vz + az * 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::solveConstraints() {
for (int iter = 0; iter < iterations_; ++iter) {
for (const 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;
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;
}
}
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_->pointProbe(p.x, p.y, p.z, particleSize_, &contact) || !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) {
constexpr float restitution = 0.15f;
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_;
constexpr float bounce = 0.35f;
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);
solveConstraints();
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
Reference in new issue View Git Blame Copy Permalink