From a67291a433d96208face295a9773dfdd1516de84 Mon Sep 17 00:00:00 2001 From: ShuliangLu Date: Fri, 14 Jul 2023 03:19:21 +0800 Subject: [PATCH 01/21] xpbd cloth constraint kernel --- .../constraint_function_kernel/constraint.cuh | 918 ++++++++++++++++-- .../generic_constraint.cuh | 8 + 2 files changed, 851 insertions(+), 75 deletions(-) create mode 100644 projects/CuLagrange/pbd/constraint_function_kernel/generic_constraint.cuh diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh index b0cfc69627..063c9141ea 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh @@ -1,90 +1,858 @@ #pragma once namespace zeno { namespace CONSTRAINT { +// FOR CLOTH SIMULATION + template + constexpr bool solve_DistanceConstraint( + const VECTOR3d &p0, SCALER invMass0, + const VECTOR3d &p1, SCALER invMass1, + const SCALER restLength, + const SCALER stiffness, + const SCALER dt, + SCALER& lambda, + VECTOR3d &corr0, VECTOR3d &corr1) + { + SCALER K = invMass0 + invMass1; + VECTOR3d n = p0 - p1; + SCALER d = n.norm(); + SCALER C = d - restLength; + if (d > static_cast(1e-6)) + n /= d; + else + { + corr0.setZero(); + corr1.setZero(); + return true; + } + + SCALER alpha = 0.0; + if (stiffness != 0.0) + { + alpha = static_cast(1.0) / (stiffness * dt * dt); + K += alpha; + } + + SCALER Kinv = 0.0; + if (fabs(K) > static_cast(1e-6)) + Kinv = static_cast(1.0) / K; + else + { + corr0.setZero(); + corr1.setZero(); + return true; + } + + const SCALER delta_lambda = -Kinv * (C + alpha * lambda); + lambda += delta_lambda; + const VECTOR3d pt = n * delta_lambda; + + corr0 = invMass0 * pt; + corr1 = -invMass1 * pt; + return true; + } + + // ---------------------------------------------------------------------------------------------- + template + constexpr bool XPBD::solve_VolumeConstraint( + const VECTOR3d& p0, SCALER invMass0, + const VECTOR3d& p1, SCALER invMass1, + const VECTOR3d& p2, SCALER invMass2, + const VECTOR3d& p3, SCALER invMass3, + const SCALER restVolume, + const SCALER stiffness, + const SCALER dt, + SCALER& lambda, + VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3) + { + SCALER volume = static_cast(1.0 / 6.0) * (p1 - p0).cross(p2 - p0).dot(p3 - p0); + + corr0.setZero(); corr1.setZero(); corr2.setZero(); corr3.setZero(); + + VECTOR3d grad0 = (p1 - p2).cross(p3 - p2); + VECTOR3d grad1 = (p2 - p0).cross(p3 - p0); + VECTOR3d grad2 = (p0 - p1).cross(p3 - p1); + VECTOR3d grad3 = (p1 - p0).cross(p2 - p0); + + SCALER K = + invMass0 * grad0.squaredNorm() + + invMass1 * grad1.squaredNorm() + + invMass2 * grad2.squaredNorm() + + invMass3 * grad3.squaredNorm(); + + SCALER alpha = 0.0; + if (stiffness != 0.0) + { + alpha = static_cast(1.0) / (stiffness * dt * dt); + K += alpha; + } + + if (fabs(K) < eps) + return false; + + const SCALER C = volume - restVolume; + const SCALER delta_lambda = -(C + alpha * lambda) / K; + lambda += delta_lambda; + + corr0 = delta_lambda * invMass0 * grad0; + corr1 = delta_lambda * invMass1 * grad1; + corr2 = delta_lambda * invMass2 * grad2; + corr3 = delta_lambda * invMass3 * grad3; + + return true; + } + +// ---------------------------------------------------------------------------------------------- + template + constexpr bool init_IsometricBendingConstraint( + const VECTOR3d& p0, + const VECTOR3d& p1, + const VECTOR3d& p2, + const VECTOR3d& p3, + MATRIX4d& Q) + { + // Compute matrix Q for quadratic bending + const VECTOR3d* x[4] = { &p2, &p3, &p0, &p1 }; + + const VECTOR3d e0 = *x[1] - *x[0]; + const VECTOR3d e1 = *x[2] - *x[0]; + const VECTOR3d e2 = *x[3] - *x[0]; + const VECTOR3d e3 = *x[2] - *x[1]; + const VECTOR3d e4 = *x[3] - *x[1]; + + const SCALER c01 = MathFunctions::cotTheta(e0, e1); + const SCALER c02 = MathFunctions::cotTheta(e0, e2); + const SCALER c03 = MathFunctions::cotTheta(-e0, e3); + const SCALER c04 = MathFunctions::cotTheta(-e0, e4); + + const SCALER A0 = static_cast(0.5) * (e0.cross(e1)).norm(); + const SCALER A1 = static_cast(0.5) * (e0.cross(e2)).norm(); + + const SCALER coef = -3.f / (2.f * (A0 + A1)); + const SCALER K[4] = { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 }; + const SCALER K2[4] = { coef * K[0], coef * K[1], coef * K[2], coef * K[3] }; + + for (unsigned char j = 0; j < 4; j++) + { + for (unsigned char k = 0; k < j; k++) + { + Q(j, k) = Q(k, j) = K[j] * K2[k]; + } + Q(j, j) = K[j] * K2[j]; + } + + return true; + } + +// ---------------------------------------------------------------------------------------------- + template + constexpr bool solve_IsometricBendingConstraint( + const VECTOR3d& p0, SCALER invMass0, + const VECTOR3d& p1, SCALER invMass1, + const VECTOR3d& p2, SCALER invMass2, + const VECTOR3d& p3, SCALER invMass3, + const MATRIX4d& Q, + const SCALER stiffness, + const SCALER dt, + SCALER& lambda, + VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3) + { + const VECTOR3d* x[4] = { &p2, &p3, &p0, &p1 }; + SCALER invMass[4] = { invMass2, invMass3, invMass0, invMass1 }; + + SCALER energy = 0.0; + for (unsigned char k = 0; k < 4; k++) + for (unsigned char j = 0; j < 4; j++) + energy += Q(j, k) * (x[k]->dot(*x[j])); + energy *= 0.5; + + VECTOR3d gradC[4]; + gradC[0].setZero(); + gradC[1].setZero(); + gradC[2].setZero(); + gradC[3].setZero(); + for (unsigned char k = 0; k < 4; k++) + for (unsigned char j = 0; j < 4; j++) + gradC[j] += Q(j, k) * *x[k]; + + + SCALER sum_normGradC = 0.0; + for (unsigned int j = 0; j < 4; j++) + { + // compute sum of squared gradient norms + if (invMass[j] != 0.0) + sum_normGradC += invMass[j] * gradC[j].squaredNorm(); + } + + SCALER alpha = 0.0; + if (stiffness != 0.0) + { + alpha = static_cast(1.0) / (stiffness * dt * dt); + sum_normGradC += alpha; + } + + // exit early if required + if (fabs(sum_normGradC) > eps) + { + // compute impulse-based scaling factor + const SCALER delta_lambda = -(energy + alpha * lambda) / sum_normGradC; + lambda += delta_lambda; + + corr0 = (delta_lambda * invMass[2]) * gradC[2]; + corr1 = (delta_lambda * invMass[3]) * gradC[3]; + corr2 = (delta_lambda * invMass[0]) * gradC[0]; + corr3 = (delta_lambda * invMass[1]) * gradC[1]; + + return true; + } + return false; + } + + + +// ---------------------------------------------------------------------------------------------- + template + constexpr bool solve_TrianglePointDistanceConstraint( + const VECTOR3d &p, SCALER invMass, + const VECTOR3d &p0, SCALER invMass0, + const VECTOR3d &p1, SCALER invMass1, + const VECTOR3d &p2, SCALER invMass2, + const SCALER restDist, + const SCALER compressionStiffness, + const SCALER stretchStiffness, + SCALER& lambda, + VECTOR3d &corr, VECTOR3d &corr0, VECTOR3d &corr1, VECTOR3d &corr2) + { + // find barycentric coordinates of closest point on triangle + + SCALER b0 = static_cast(1.0 / 3.0); // for singular case + SCALER b1 = b0; + SCALER b2 = b0; + + VECTOR3d d1 = p1 - p0; + VECTOR3d d2 = p2 - p0; + VECTOR3d pp0 = p - p0; + SCALER a = d1.dot(d1); + SCALER b = d2.dot(d1); + SCALER c = pp0.dot(d1); + SCALER d = b; + SCALER e = d2.dot(d2); + SCALER f = pp0.dot(d2); + SCALER det = a*e - b*d; + + if (det != 0.0) { + SCALER s = (c*e - b*f) / det; + SCALER t = (a*f - c*d) / det; + b0 = static_cast(1.0) - s - t; // inside triangle + b1 = s; + b2 = t; + if (b0 < 0.0) { // on edge 1-2 + VECTOR3d d = p2 - p1; + SCALER d2 = d.dot(d); + SCALER t = (d2 == static_cast(0.0)) ? static_cast(0.5) : d.dot(p - p1) / d2; + if (t < 0.0) t = 0.0; // on point 1 + if (t > 1.0) t = 1.0; // on point 2 + b0 = 0.0; + b1 = (static_cast(1.0) - t); + b2 = t; + } + else if (b1 < 0.0) { // on edge 2-0 + VECTOR3d d = p0 - p2; + SCALER d2 = d.dot(d); + SCALER t = (d2 == static_cast(0.0)) ? static_cast(0.5) : d.dot(p - p2) / d2; + if (t < 0.0) t = 0.0; // on point 2 + if (t > 1.0) t = 1.0; // on point 0 + b1 = 0.0; + b2 = (static_cast(1.0) - t); + b0 = t; + } + else if (b2 < 0.0) { // on edge 0-1 + VECTOR3d d = p1 - p0; + SCALER d2 = d.dot(d); + SCALER t = (d2 == static_cast(0.0)) ? static_cast(0.5) : d.dot(p - p0) / d2; + if (t < 0.0) t = 0.0; // on point 0 + if (t > 1.0) t = 1.0; // on point 1 + b2 = 0.0; + b0 = (static_cast(1.0) - t); + b1 = t; + } + } + VECTOR3d q = p0 * b0 + p1 * b1 + p2 * b2; + VECTOR3d n = p - q; + SCALER dist = n.norm(); + n.normalize(); + SCALER C = dist - restDist; + VECTOR3d grad = n; + VECTOR3d grad0 = -n * b0; + VECTOR3d grad1 = -n * b1; + VECTOR3d grad2 = -n * b2; + + SCALER s = invMass + invMass0 * b0*b0 + invMass1 * b1*b1 + invMass2 * b2*b2; + if (s == 0.0) + return false; + + s = C / s; + if (C < 0.0) + s *= compressionStiffness; + else + s *= stretchStiffness; + + if (s == 0.0) + return false; + + corr = -s * invMass * grad; + corr0 = -s * invMass0 * grad0; + corr1 = -s * invMass1 * grad1; + corr2 = -s * invMass2 * grad2; + return true; + } + + // ---------------------------------------------------------------------------------------------- + template + constexpr bool solve_EdgeEdgeDistanceConstraint( + const VECTOR3d &p0, SCALER invMass0, + const VECTOR3d &p1, SCALER invMass1, + const VECTOR3d &p2, SCALER invMass2, + const VECTOR3d &p3, SCALER invMass3, + const SCALER restDist, + const SCALER compressionStiffness, + const SCALER stretchStiffness, + SCALER& lambda, + VECTOR3d &corr0, VECTOR3d &corr1, VECTOR3d &corr2, VECTOR3d &corr3) + { + VECTOR3d d0 = p1 - p0; + VECTOR3d d1 = p3 - p2; + + SCALER a = d0.squaredNorm(); + SCALER b = -d0.dot(d1); + SCALER c = d0.dot(d1); + SCALER d = -d1.squaredNorm(); + SCALER e = (p2 - p0).dot(d0); + SCALER f = (p2 - p0).dot(d1); + SCALER det = a*d - b*c; + SCALER s, t; + if (det != 0.0) { + det = static_cast(1.0) / det; + s = (e*d - b*f) * det; + t = (a*f - e*c) * det; + } + else { // d0 and d1 parallel + SCALER s0 = p0.dot(d0); + SCALER s1 = p1.dot(d0); + SCALER t0 = p2.dot(d0); + SCALER t1 = p3.dot(d0); + bool flip0 = false; + bool flip1 = false; + + if (s0 > s1) { SCALER f = s0; s0 = s1; s1 = f; flip0 = true; } + if (t0 > t1) { SCALER f = t0; t0 = t1; t1 = f; flip1 = true; } + + if (s0 >= t1) { + s = !flip0 ? static_cast(0.0) : static_cast(1.0); + t = !flip1 ? static_cast(1.0) : static_cast(0.0); + } + else if (t0 >= s1) { + s = !flip0 ? static_cast(1.0) : static_cast(0.0); + t = !flip1 ? static_cast(0.0) : static_cast(1.0); + } + else { // overlap + SCALER mid = (s0 > t0) ? (s0 + t1) * static_cast(0.5) : (t0 + s1) * static_cast(0.5); + s = (s0 == s1) ? static_cast(0.5) : (mid - s0) / (s1 - s0); + t = (t0 == t1) ? static_cast(0.5) : (mid - t0) / (t1 - t0); + } + } + if (s < 0.0) s = 0.0; + if (s > 1.0) s = 1.0; + if (t < 0.0) t = 0.0; + if (t > 1.0) t = 1.0; + + SCALER b0 = static_cast(1.0) - s; + SCALER b1 = s; + SCALER b2 = static_cast(1.0) - t; + SCALER b3 = t; + + VECTOR3d q0 = p0 * b0 + p1 * b1; + VECTOR3d q1 = p2 * b2 + p3 * b3; + VECTOR3d n = q0 - q1; + SCALER dist = n.norm(); + n.normalize(); + SCALER C = dist - restDist; + VECTOR3d grad0 = n * b0; + VECTOR3d grad1 = n * b1; + VECTOR3d grad2 = -n * b2; + VECTOR3d grad3 = -n * b3; + + s = invMass0 * b0*b0 + invMass1 * b1*b1 + invMass2 * b2*b2 + invMass3 * b3*b3; + if (s == 0.0) + return false; + + s = C / s; + if (C < 0.0) + s *= compressionStiffness; + else + s *= stretchStiffness; + + if (s == 0.0) + return false; + + corr0 = -s * invMass0 * grad0; + corr1 = -s * invMass1 * grad1; + corr2 = -s * invMass2 * grad2; + corr3 = -s * invMass3 * grad3; + return true; + } + +// FOR ELASTIC RODS SIMULATION +// ---------------------------------------------------------------------------------------------- + template + constexpr bool PositionBasedCosseratRods::solve_StretchShearConstraint( + const VECTOR3d& p0, SCALER invMass0, + const VECTOR3d& p1, SCALER invMass1, + const QUATERNION& q0, SCALER invMassq0, + const VECTOR3d& stretchingAndShearingKs, + const SCALER restLength, + VECTOR3d& corr0, VECTOR3d& corr1, QUATERNION& corrq0) + { + VECTOR3d d3; //third director d3 = q0 * e_3 * q0_conjugate + d3[0] = static_cast(2.0) * (q0.x() * q0.z() + q0.w() * q0.y()); + d3[1] = static_cast(2.0) * (q0.y() * q0.z() - q0.w() * q0.x()); + d3[2] = q0.w() * q0.w() - q0.x() * q0.x() - q0.y() * q0.y() + q0.z() * q0.z(); + + VECTOR3d gamma = (p1 - p0) / restLength - d3; + gamma /= (invMass1 + invMass0) / restLength + invMassq0 * static_cast(4.0)*restLength + eps; + + if (std::abs(stretchingAndShearingKs[0] - stretchingAndShearingKs[1]) < eps && std::abs(stretchingAndShearingKs[0] - stretchingAndShearingKs[2]) < eps) //all Ks are approx. equal + for (int i = 0; i<3; i++) gamma[i] *= stretchingAndShearingKs[i]; + else //diffenent stretching and shearing Ks. Transform diag(Ks[0], Ks[1], Ks[2]) into world space using Ks_w = R(q0) * diag(Ks[0], Ks[1], Ks[2]) * R^T(q0) and multiply it with gamma + { + MATRIX3d R = q0.toRotationMatrix(); + gamma = (R.transpose() * gamma).eval(); + for (int i = 0; i<3; i++) gamma[i] *= stretchingAndShearingKs[i]; + gamma = (R * gamma).eval(); + } + + corr0 = invMass0 * gamma; + corr1 = -invMass1 * gamma; - // template - // constexpr bool edge_length_constraint_predicate(const VECTOR3d x[2], - // const SCALER& rl) - // { - // auto edge = x[0] - x[1]; - // auto en = edge.norm(); - // return en > rl; - // } + QUATERNION q_e_3_bar(q0.z(), -q0.y(), q0.x(), -q0.w()); //compute q*e_3.conjugate (cheaper than quaternion product) + corrq0 = QUATERNION(0.0, gamma.x(), gamma.y(), gamma.z()) * q_e_3_bar; + corrq0.coeffs() *= static_cast(2.0) * invMassq0 * restLength; + return true; + } + +// ---------------------------------------------------------------------------------------------- + template + constexpr bool PositionBasedCosseratRods::solve_BendTwistConstraint( + const QUATERNION& q0, SCALER invMassq0, + const QUATERNION& q1, SCALER invMassq1, + const VECTOR3d& bendingAndTwistingKs, + const QUATERNION& restDarbouxVector, + QUATERNION& corrq0, QUATERNION& corrq1) + { + QUATERNION omega = q0.conjugate() * q1; //darboux vector + + QUATERNION omega_plus; + omega_plus.coeffs() = omega.coeffs() + restDarbouxVector.coeffs(); //delta Omega with -Omega_0 + omega.coeffs() = omega.coeffs() - restDarbouxVector.coeffs(); //delta Omega with + omega_0 + if (omega.squaredNorm() > omega_plus.squaredNorm()) omega = omega_plus; + + for (int i = 0; i < 3; i++) omega.coeffs()[i] *= bendingAndTwistingKs[i] / (invMassq0 + invMassq1 + static_cast(1.0e-6)); + omega.w() = 0.0; //discrete Darboux vector does not have vanishing scalar part + + corrq0 = q1 * omega; + corrq1 = q0 * omega; + corrq0.coeffs() *= invMassq0; + corrq1.coeffs() *= -invMassq1; + return true; + } + + // ---------------------------------------------------------------------------------------------- template - constexpr void edge_length_constraint_projection(const VECTOR3d x[2], - const SCALER& rl, - const SCALER inv_m[2], - const SCALER& compliance, - VECTOR3d dx[2]) { - auto edge = x[0] - x[1]; - auto en = edge.norm(); - edge /= (en + (SCALER)1e-6); - auto C = en - rl; - auto w = inv_m[0] + inv_m[1]; - auto s = -C / (w + compliance); - - dx[0] = edge * s * inv_m[0]; - dx[1] = -edge * s * inv_m[1]; + constexpr bool solve_PerpendiculaBisectorConstraint( + const VECTOR3d &p0, SCALER invMass0, + const VECTOR3d &p1, SCALER invMass1, + const VECTOR3d &p2, SCALER invMass2, + const SCALER stiffness, + VECTOR3d &corr0, VECTOR3d &corr1, VECTOR3d &corr2) + { + const VECTOR3d pm = 0.5 * (p0 + p1); + const VECTOR3d p0p2 = p0 - p2; + const VECTOR3d p2p1 = p2 - p1; + const VECTOR3d p1p0 = p1 - p0; + const VECTOR3d p2pm = p2 - pm; + + SCALER wSum = invMass0 * p0p2.squaredNorm() + invMass1 * p2p1.squaredNorm() + invMass2 * p1p0.squaredNorm(); + if (wSum < eps) + return false; + + const SCALER lambda = stiffness * p2pm.dot(p1p0) / wSum; + + corr0 = -invMass0 * lambda * p0p2; + corr1 = -invMass1 * lambda * p2p1; + corr2 = -invMass2 * lambda * p1p0; + + return true; } + // ---------------------------------------------------------------------------------------------- template - constexpr void tet_volume_constraint_projection(const VECTOR3d x[4], - const SCALER& rv, - const SCALER inv_m[4], - const SCALER& compliance, - VECTOR3d dx[4]) { - dx[0] = (x[1] - x[2]).cross(x[3] - x[2]); - dx[1] = (x[2] - x[0]).cross(x[3] - x[0]); - dx[2] = (x[0] - x[1]).cross(x[3] - x[1]); - dx[3] = (x[1] - x[0]).cross(x[2] - x[0]); - - SCALER w = 0; - for(int i = 0;i != 4;++i) - w += dx[i].l2NormSqr() * inv_m[i]; - SCALER vol = zs::abs(x[1] - x[0]).cross(x[2] - x[0]).dot(x[3] - x[0]) / (SCALER)6; - SCALER C = (vol - rv) * (SCALER)6; - SCALER s = -C / (w + compliance); - - for(int i = 0;i != 4;++i) - dx[i] = dx[i] * s * inv_m[i]; + constexpr bool solve_GhostPointEdgeDistanceConstraint( + const VECTOR3d& p0, SCALER invMass0, + const VECTOR3d& p1, SCALER invMass1, + const VECTOR3d& p2, SCALER invMass2, + const SCALER stiffness, + const SCALER ghostEdgeRestLength, + VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2) + { + // Ghost-Edge constraint + VECTOR3d pm = 0.5 * (p0 + p1); + VECTOR3d p2pm = p2 - pm; + SCALER wSum = static_cast(0.25) * invMass0 + static_cast(0.25) * invMass1 + static_cast(1.0) * invMass2; + + if (wSum < eps) + return false; + + SCALER p2pm_mag = p2pm.norm(); + p2pm *= static_cast(1.0) / p2pm_mag; + + const SCALER lambda = stiffness * (p2pm_mag - ghostEdgeRestLength) / wSum; + + corr0 = 0.5 * invMass0 * lambda * p2pm; + corr1 = 0.5 * invMass1 * lambda * p2pm; + corr2 = -1.0 * invMass2 * lambda * p2pm; + + return true; } - // edge0 = [0,2,1] - // edge1 = [0,1,3] + // ---------------------------------------------------------------------------------------------- template - constexpr void tri_bending_constraint_projection(const VECTOR3d x[4], - const SCALER& rda, - const SCALER inv_m[4], - const SCALER& compliance, - VECTOR3d dx[4]) { - VECTOR3d p[4]; - for(int i = 0;i != 4;++i) - p[i] = x[i] - x[0]; - auto p12 = p[1].cross(p[2]); - auto p13 = p[1].cross(p[3]); - auto n0 = p12 / (p23.norm() + (SCALER)1e-6); - auto n1 = p13 / (p12.norm() + (SCALER)1e-6); - - auto d = n1.dot(n0); - auto p12n = p12.norm(); - auto p13n = p13.norm(); - - VECTOR3d q[4]; - - auto q[2] = (p[1].cross(n1) + n0.cross(p[1]) * d)/p12n; - auto q[3] = (p[1].cross(n0) + n1.cross(p[1]) * d)/p13n; - auto q[1] = -(p[2].cross(n1) + n0.cross(p[2]) *d)/p12n - (p[3].cross(n0) + n1.cross(p[3]) * d)/p13n; - auto q[0] = -q[1] - q[2] - q[3]; - - auto C = zs::acos(d) - rda; - SCALER w = 0; - for(int i = 0;i != 4;++i) - w += q[i].l2NormSqr() * inv_m[i]; - auto s = -C * zs::sqrt(1 - d * d) / (w + compliance); - for(int i = 0;i != 4;++i) - dx[i] = q[i] * s * inv_m[i]; + constexpr bool solve_DarbouxVectorConstraint( + const VECTOR3d& p0, SCALER invMass0, + const VECTOR3d& p1, SCALER invMass1, + const VECTOR3d& p2, SCALER invMass2, + const VECTOR3d& p3, SCALER invMass3, + const VECTOR3d& p4, SCALER invMass4, + const VECTOR3d& bendingAndTwistingKs, + const SCALER midEdgeLength, + const VECTOR3d& restDarbouxVector, + VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3, VECTOR3d& corr4) + { + // Single rod element: + // 3 4 //ghost points + // | | + // --0---1---2-- // rod points + + VECTOR3d darboux_vector; + MATRIX3d d0, d1; + + computeMaterialFrame(p0, p1, p3, d0); + computeMaterialFrame(p1, p2, p4, d1); + + computeDarbouxVector(d0, d1, midEdgeLength, darboux_vector); + + MATRIX3d dajpi[3][3]; + computeMaterialFrameDerivative(p0, p1, p3, d0, + dajpi[0][0], dajpi[0][1], dajpi[0][2], + dajpi[1][0], dajpi[1][1], dajpi[1][2], + dajpi[2][0], dajpi[2][1], dajpi[2][2]); + + MATRIX3d dbjpi[3][3]; + computeMaterialFrameDerivative(p1, p2, p4, d1, + dbjpi[0][0], dbjpi[0][1], dbjpi[0][2], + dbjpi[1][0], dbjpi[1][1], dbjpi[1][2], + dbjpi[2][0], dbjpi[2][1], dbjpi[2][2]); + + MATRIX3d constraint_jacobian[5]; + computeDarbouxGradient( + darboux_vector, midEdgeLength, d0, d1, + dajpi, dbjpi, + //bendingAndTwistingKs, + constraint_jacobian[0], + constraint_jacobian[1], + constraint_jacobian[2], + constraint_jacobian[3], + constraint_jacobian[4]); + + const VECTOR3d constraint_value(bendingAndTwistingKs[0] * (darboux_vector[0] - restDarbouxVector[0]), + bendingAndTwistingKs[1] * (darboux_vector[1] - restDarbouxVector[1]), + bendingAndTwistingKs[2] * (darboux_vector[2] - restDarbouxVector[2])); + + MATRIX3d factor_matrix; + factor_matrix.setZero(); + + MATRIX3d tmp_mat; + SCALER invMasses[]{ invMass0, invMass1, invMass2, invMass3, invMass4 }; + for (int i = 0; i < 5; ++i) + { + tmp_mat = constraint_jacobian[i].transpose() * constraint_jacobian[i]; + tmp_mat.col(0) *= invMasses[i]; + tmp_mat.col(1) *= invMasses[i]; + tmp_mat.col(2) *= invMasses[i]; + + factor_matrix += tmp_mat; + } + + VECTOR3d dp[5]; + tmp_mat = factor_matrix.inverse(); + + for (int i = 0; i < 5; ++i) + { + constraint_jacobian[i].col(0) *= invMasses[i]; + constraint_jacobian[i].col(1) *= invMasses[i]; + constraint_jacobian[i].col(2) *= invMasses[i]; + dp[i] = -(constraint_jacobian[i]) * (tmp_mat * constraint_value); + } + + corr0 = dp[0]; + corr1 = dp[1]; + corr2 = dp[2]; + corr3 = dp[3]; + corr4 = dp[4]; + + return true; } + // ---------------------------------------------------------------------------------------------- + template + constexpr bool computeMaterialFrame( + const VECTOR3d& p0, + const VECTOR3d& p1, + const VECTOR3d& p2, + MATRIX3d& frame) + { + frame.col(2) = (p1 - p0); + frame.col(2).normalize(); + + frame.col(1) = (frame.col(2).cross(p2 - p0)); + frame.col(1).normalize(); + + frame.col(0) = frame.col(1).cross(frame.col(2)); + return true; + } + + // ---------------------------------------------------------------------------------------------- + template + constexpr bool computeDarbouxVector(const MATRIX3d& dA, const MATRIX3d& dB, const SCALER mid_edge_length, VECTOR3d& darboux_vector) + { + SCALER factor = static_cast(1.0) + dA.col(0).dot(dB.col(0)) + dA.col(1).dot(dB.col(1)) + dA.col(2).dot(dB.col(2)); + + factor = static_cast(2.0) / (mid_edge_length * factor); + + for (int c = 0; c < 3; ++c) + { + const int i = permutation[c][0]; + const int j = permutation[c][1]; + const int k = permutation[c][2]; + darboux_vector[i] = dA.col(j).dot(dB.col(k)) - dA.col(k).dot(dB.col(j)); + } + darboux_vector *= factor; + return true; + } + + // ---------------------------------------------------------------------------------------------- + template + constexpr bool computeMaterialFrameDerivative( + const VECTOR3d& p0, const VECTOR3d& p1, const VECTOR3d& p2, const MATRIX3d& d, + MATRIX3d& d1p0, MATRIX3d& d1p1, MATRIX3d& d1p2, + MATRIX3d& d2p0, MATRIX3d& d2p1, MATRIX3d& d2p2, + MATRIX3d& d3p0, MATRIX3d& d3p1, MATRIX3d& d3p2) + { + ////////////////////////////////////////////////////////////////////////// + // d3pi + ////////////////////////////////////////////////////////////////////////// + const VECTOR3d p01 = p1 - p0; + SCALER length_p01 = p01.norm(); + + d3p0.col(0) = d.col(2)[0] * d.col(2); + d3p0.col(1) = d.col(2)[1] * d.col(2); + d3p0.col(2) = d.col(2)[2] * d.col(2); + + d3p0.col(0)[0] -= 1.0; + d3p0.col(1)[1] -= 1.0; + d3p0.col(2)[2] -= 1.0; + + d3p0.col(0) *= (static_cast(1.0) / length_p01); + d3p0.col(1) *= (static_cast(1.0) / length_p01); + d3p0.col(2) *= (static_cast(1.0) / length_p01); + + d3p1.col(0) = -d3p0.col(0); + d3p1.col(1) = -d3p0.col(1); + d3p1.col(2) = -d3p0.col(2); + + d3p2.col(0).setZero(); + d3p2.col(1).setZero(); + d3p2.col(2).setZero(); + + ////////////////////////////////////////////////////////////////////////// + // d2pi + ////////////////////////////////////////////////////////////////////////// + const VECTOR3d p02 = p2 - p0; + const VECTOR3d p01_cross_p02 = p01.cross(p02); + + const SCALER length_cross = p01_cross_p02.norm(); + + MATRIX3d mat; + mat.col(0) = d.col(1)[0] * d.col(1); + mat.col(1) = d.col(1)[1] * d.col(1); + mat.col(2) = d.col(1)[2] * d.col(1); + + mat.col(0)[0] -= 1.0; + mat.col(1)[1] -= 1.0; + mat.col(2)[2] -= 1.0; + + mat.col(0) *= (-static_cast(1.0) / length_cross); + mat.col(1) *= (-static_cast(1.0) / length_cross); + mat.col(2) *= (-static_cast(1.0) / length_cross); + + MATRIX3d product_matrix; + MathFunctions::crossProductMatrix(p2 - p1, product_matrix); + d2p0 = mat * product_matrix; + + MathFunctions::crossProductMatrix(p0 - p2, product_matrix); + d2p1 = mat * product_matrix; + + MathFunctions::crossProductMatrix(p1 - p0, product_matrix); + d2p2 = mat * product_matrix; + + ////////////////////////////////////////////////////////////////////////// + // d1pi + ////////////////////////////////////////////////////////////////////////// + MATRIX3d product_mat_d3; + MATRIX3d product_mat_d2; + MathFunctions::crossProductMatrix(d.col(2), product_mat_d3); + MathFunctions::crossProductMatrix(d.col(1), product_mat_d2); + + d1p0 = product_mat_d2 * d3p0 - product_mat_d3 * d2p0; + d1p1 = product_mat_d2 * d3p1 - product_mat_d3 * d2p1; + d1p2 = -product_mat_d3 * d2p2; + return true; + } + + // ---------------------------------------------------------------------------------------------- + template + constexpr bool computeDarbouxGradient( + const VECTOR3d& darboux_vector, const SCALER length, + const MATRIX3d& da, const MATRIX3d& db, + const MATRIX3d dajpi[3][3], const MATRIX3d dbjpi[3][3], + //const VECTOR3d& bendAndTwistKs, + MATRIX3d& omega_pa, MATRIX3d& omega_pb, MATRIX3d& omega_pc, MATRIX3d& omega_pd, MATRIX3d& omega_pe + ) + { + SCALER X = static_cast(1.0) + da.col(0).dot(db.col(0)) + da.col(1).dot(db.col(1)) + da.col(2).dot(db.col(2)); + X = static_cast(2.0) / (length * X); + + for (int c = 0; c < 3; ++c) + { + const int i = permutation[c][0]; + const int j = permutation[c][1]; + const int k = permutation[c][2]; + // pa + { + VECTOR3d term1(0,0,0); + VECTOR3d term2(0,0,0); + VECTOR3d tmp(0,0,0); + + // first term + term1 = dajpi[j][0].transpose() * db.col(k); + tmp = dajpi[k][0].transpose() * db.col(j); + term1 = term1 - tmp; + // second term + for (int n = 0; n < 3; ++n) + { + tmp = dajpi[n][0].transpose() * db.col(n); + term2 = term2 + tmp; + } + omega_pa.col(i) = X * (term1-(0.5 * darboux_vector[i] * length) * term2); + //omega_pa.col(i) *= bendAndTwistKs[i]; + } + // pb + { + VECTOR3d term1(0, 0, 0); + VECTOR3d term2(0, 0, 0); + VECTOR3d tmp(0, 0, 0); + // first term + term1 = dajpi[j][1].transpose() * db.col(k); + tmp = dajpi[k][1].transpose() * db.col(j); + term1 = term1 - tmp; + // third term + tmp = dbjpi[j][0].transpose() * da.col(k); + term1 = term1 - tmp; + + tmp = dbjpi[k][0].transpose() * da.col(j); + term1 = term1 + tmp; + + // second term + for (int n = 0; n < 3; ++n) + { + tmp = dajpi[n][1].transpose() * db.col(n); + term2 = term2 + tmp; + + tmp = dbjpi[n][0].transpose() * da.col(n); + term2 = term2 + tmp; + } + omega_pb.col(i) = X * (term1-(0.5 * darboux_vector[i] * length) * term2); + //omega_pb.col(i) *= bendAndTwistKs[i]; + } + // pc + { + VECTOR3d term1(0, 0, 0); + VECTOR3d term2(0, 0, 0); + VECTOR3d tmp(0, 0, 0); + + // first term + term1 = dbjpi[j][1].transpose() * da.col(k); + tmp = dbjpi[k][1].transpose() * da.col(j); + term1 = term1 - tmp; + + // second term + for (int n = 0; n < 3; ++n) + { + tmp = dbjpi[n][1].transpose() * da.col(n); + term2 = term2 + tmp; + } + omega_pc.col(i) = -X*(term1+(0.5 * darboux_vector[i] * length) * term2); + //omega_pc.col(i) *= bendAndTwistKs[i]; + } + // pd + { + VECTOR3d term1(0, 0, 0); + VECTOR3d term2(0, 0, 0); + VECTOR3d tmp(0, 0, 0); + // first term + term1 = dajpi[j][2].transpose() * db.col(k); + tmp = dajpi[k][2].transpose() * db.col(j); + term1 = term1 - tmp; + // second term + for (int n = 0; n < 3; ++n) { + tmp = dajpi[n][2].transpose() * db.col(n); + term2 = term2 + tmp; + } + omega_pd.col(i) = X*(term1-(0.5 * darboux_vector[i] * length) * term2); + //omega_pd.col(i) *= bendAndTwistKs[i]; + } + // pe + { + VECTOR3d term1(0, 0, 0); + VECTOR3d term2(0, 0, 0); + VECTOR3d tmp(0, 0, 0); + // first term + term1 = dbjpi[j][2].transpose() * da.col(k); + tmp = dbjpi[k][2].transpose() * da.col(j); + term1 -= tmp; + + // second term + for (int n = 0; n < 3; ++n) + { + tmp = dbjpi[n][2].transpose() * da.col(n); + term2 += tmp; + } + + omega_pe.col(i) = -X*(term1+(0.5 * darboux_vector[i] * length) * term2); + //omega_pe.col(i) *= bendAndTwistKs[i]; + } + } + return true; + } + +}; }; -}; \ No newline at end of file diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/generic_constraint.cuh b/projects/CuLagrange/pbd/constraint_function_kernel/generic_constraint.cuh new file mode 100644 index 0000000000..9a590aaab5 --- /dev/null +++ b/projects/CuLagrange/pbd/constraint_function_kernel/generic_constraint.cuh @@ -0,0 +1,8 @@ +#pragma once + +namespace zeno { namespace CONSTRAINT { + + + +}; +}; From 0aef4e3bd642a65cf9709bed872934bb3c959e51 Mon Sep 17 00:00:00 2001 From: ShuliangLu Date: Fri, 14 Jul 2023 18:14:57 +0800 Subject: [PATCH 02/21] XPBD interface --- projects/CuLagrange/pbd/Pipeline.cu | 89 +++++++++++++++++++++++++++++ 1 file changed, 89 insertions(+) create mode 100644 projects/CuLagrange/pbd/Pipeline.cu diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu new file mode 100644 index 0000000000..c96a9e71a6 --- /dev/null +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -0,0 +1,89 @@ +#include "Structures.hpp" +#include "zensim/Logger.hpp" +#include "zensim/cuda/execution/ExecutionPolicy.cuh" +#include "zensim/omp/execution/ExecutionPolicy.hpp" +#include "zensim/io/MeshIO.hpp" +#include "zensim/math/bit/Bits.h" +#include "zensim/types/Property.h" +#include +#include +#include +#include +#include +#include + +namespace zeno { + +struct AddGenericConstraints : zeno::INode { + using namespace zs; + virtual void apply() override { + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + + constexpr auto space = execspace_e::cuda; + + auto zsparticles = RETRIEVE_OBJECT_PTRS(ZenoParticles, "zsparticles"); + auto targets = get_input("target"); + auto constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "constraints"); + + set_output("zsparticles",zsparticles); + set_output("constraints",constraints); + }; +}; + +ZENDEFNODE(AddGenericConstraints, {{{"zsparticles"},{"constraints"},{"target"}}, + {{"zsparticles"},{"constraints"}}, + {}, + {"PBD"}}); + +struct UpdateConstraintTarget : zeno::INode { + using namespace zs; + virtual void apply() override { + auto constraints = get_input("constraints"); + auto targets = get_input("target"); + + set_output("constraints",constraints); + }; +}; + +ZENDEFNODE(UpdateConstraintTarget, {{{"constraints"},{"target"}}, + {{"constraints"}}, + {}, + {"PBD"}}); + +struct ZSMerge : zeno::INode { + using namespace zs; + virtual void apply() override { + constexpr auto space = execspace_e::cuda; + auto zsparticles0 = RETRIEVE_OBJECT_PTRS(ZenoParticles, "zsparticles0"); + auto zsparticles1 = RETRIEVE_OBJECT_PTRS(ZenoParticles, "zsparticles1"); + + set_output("zsparticles",zsparticles0); + }; +}; + +ZENDEFNODE(ZSMerge, {{{"zsparticles0"},{"zsparticles1"}}, + {{"zsparticles"}}, + {}, + {"PBD"}}); + + +struct XPBDSolve : zeno::INode { + using namespace zs; + virtual void apply() override { + constexpr auto space = execspace_e::cuda; + auto zsparticles = RETRIEVE_OBJECT_PTRS(ZenoParticles, "zsparticles"); + auto constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "constraints"); + + set_output("constraints",zsconstraints); + set_output("zsparticles",zsparticles); + }; +}; + +ZENDEFNODE(XPBDSolve, {{{"zsparticles"},{"constraints"}}, + {{"zsparticles"},{"constraints"}}, + {}, + {"PBD"}}); + +}; From db355a9d427d322907a4f8c1b0302d8d4e78c198 Mon Sep 17 00:00:00 2001 From: ShuliangLu Date: Fri, 14 Jul 2023 18:56:18 +0800 Subject: [PATCH 03/21] update XPBD interface --- projects/CuLagrange/pbd/Pipeline.cu | 59 +++++++++-------------------- 1 file changed, 17 insertions(+), 42 deletions(-) diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index c96a9e71a6..15d2e95ac4 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -14,7 +14,8 @@ namespace zeno { -struct AddGenericConstraints : zeno::INode { +// we only need to record the topo here +struct MakeGenericConstraintTopology : zeno::INode { using namespace zs; virtual void apply() override { using vec2i = zs::vec; @@ -23,60 +24,34 @@ struct AddGenericConstraints : zeno::INode { constexpr auto space = execspace_e::cuda; - auto zsparticles = RETRIEVE_OBJECT_PTRS(ZenoParticles, "zsparticles"); - auto targets = get_input("target"); - auto constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "constraints"); + auto source = get_input("source"); + auto constraint = std::make_shared(); - set_output("zsparticles",zsparticles); - set_output("constraints",constraints); - }; -}; + auto type = get_param("type"); + auto groupID = get_param("groupID"); -ZENDEFNODE(AddGenericConstraints, {{{"zsparticles"},{"constraints"},{"target"}}, - {{"zsparticles"},{"constraints"}}, - {}, - {"PBD"}}); - -struct UpdateConstraintTarget : zeno::INode { - using namespace zs; - virtual void apply() override { - auto constraints = get_input("constraints"); - auto targets = get_input("target"); - - set_output("constraints",constraints); + // set_output("target",target); + set_output("source",source); + set_output("constraint",constraint); }; }; -ZENDEFNODE(UpdateConstraintTarget, {{{"constraints"},{"target"}}, +ZENDEFNODE(MakeGenericConstraintTopology, {{{"source"}}, {{"constraints"}}, - {}, + { + {"enum points edges tris tets segment_angle dihedral_angle","topo_type","points"}, + {"string","groupID",""}, + }, {"PBD"}}); -struct ZSMerge : zeno::INode { - using namespace zs; - virtual void apply() override { - constexpr auto space = execspace_e::cuda; - auto zsparticles0 = RETRIEVE_OBJECT_PTRS(ZenoParticles, "zsparticles0"); - auto zsparticles1 = RETRIEVE_OBJECT_PTRS(ZenoParticles, "zsparticles1"); - - set_output("zsparticles",zsparticles0); - }; -}; - -ZENDEFNODE(ZSMerge, {{{"zsparticles0"},{"zsparticles1"}}, - {{"zsparticles"}}, - {}, - {"PBD"}}); - - struct XPBDSolve : zeno::INode { using namespace zs; virtual void apply() override { constexpr auto space = execspace_e::cuda; - auto zsparticles = RETRIEVE_OBJECT_PTRS(ZenoParticles, "zsparticles"); - auto constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "constraints"); + auto zsparticles = get_input("zsparticles"); + auto constraints = get_input("constraints"); - set_output("constraints",zsconstraints); + set_output("constraints",constraints); set_output("zsparticles",zsparticles); }; }; From b95d9a9528448468f5dff2075c82337362c3d45d Mon Sep 17 00:00:00 2001 From: ShuliangLu Date: Tue, 18 Jul 2023 00:45:51 +0800 Subject: [PATCH 04/21] constraint sort by color --- projects/CuLagrange/CMakeLists.txt | 1 + projects/CuLagrange/geometry/Topology.cu | 18 +- .../CuLagrange/geometry/kernel/geo_math.hpp | 7 + .../geometry/kernel/intersection.hpp | 4 +- .../CuLagrange/geometry/kernel/topology.hpp | 231 ++++++++++++++++-- projects/CuLagrange/pbd/Pipeline.cu | 220 ++++++++++++++++- 6 files changed, 449 insertions(+), 32 deletions(-) diff --git a/projects/CuLagrange/CMakeLists.txt b/projects/CuLagrange/CMakeLists.txt index f539068361..cf4371fb40 100644 --- a/projects/CuLagrange/CMakeLists.txt +++ b/projects/CuLagrange/CMakeLists.txt @@ -37,6 +37,7 @@ target_sources(zeno PRIVATE target_sources(zeno PRIVATE pbd/PBDInit.cu pbd/PBDPipeline.cu + pbd/Pipeline.cu ) # fem-cloth diff --git a/projects/CuLagrange/geometry/Topology.cu b/projects/CuLagrange/geometry/Topology.cu index 9e0bbe274f..ce8595ba6d 100644 --- a/projects/CuLagrange/geometry/Topology.cu +++ b/projects/CuLagrange/geometry/Topology.cu @@ -698,13 +698,15 @@ struct DoTopogicalColoring : zeno::INode { constexpr auto space = zs::execspace_e::cuda; auto zsparticles = get_input("zsparticles"); - const auto& verts = zsparticles->getParticles(); + // const auto& verts = zsparticles->getParticles(); auto& elms = zsparticles->getQuadraturePoints(); // auto& tris = (*zsparticles)[ZenoParticles::s_surfTriTag]; // const auto& tets = zsparticles->getQuadraturePoints(); auto cdim = elms.getPropertySize("inds"); auto color_tag = get_param("colorTag"); + auto do_sort_color = get_param("sort_color"); + if(!elms.hasProperty(color_tag)) elms.append_channels(cudaPol,{{color_tag,1}}); @@ -722,13 +724,22 @@ struct DoTopogicalColoring : zeno::INode { zs::Vector colors{elms.get_allocator(),elms.size()}; std::cout << "do topological coloring" << std::endl; topological_coloring(cudaPol,topos,colors); + zs::Vector reordered_map{elms.get_allocator(),elms.size()}; + cudaPol(zs::range(reordered_map.size()),[reordered_map = proxy(reordered_map)] ZS_LAMBDA(int ti) mutable {reordered_map[ti] = ti;}); + if(do_sort_color) + sort_topology_by_coloring_tag(cudaPol,reordered_map,colors); std::cout << "finish topological coloring" << std::endl; cudaPol(zs::range(elms.size()),[ elms = proxy({},elms), color_tag = zs::SmallString(color_tag), + topos = proxy(topos), + reordered_map = proxy(reordered_map), + cdim, colors = proxy(colors)] ZS_LAMBDA(int ei) mutable { - elms(color_tag,ei) = colors[ei]; + elms(color_tag,ei) = colors[reordered_map[ei]]; + for(int i = 0;i != cdim;++i) + elms("inds",i,ei) = zs::reinterpret_bits(topos[reordered_map[ei]][i]); }); set_output("zsparticles",zsparticles); @@ -740,7 +751,8 @@ ZENDEFNODE(DoTopogicalColoring, {{{"zsparticles"}}, {"zsparticles"} }, { - {"string","colorTag","colorTag"} + {"string","colorTag","colorTag"}, + {"bool","sort_color","1"} }, {"ZSGeometry"}}); diff --git a/projects/CuLagrange/geometry/kernel/geo_math.hpp b/projects/CuLagrange/geometry/kernel/geo_math.hpp index 6fc9fc6c0d..b3fd989788 100644 --- a/projects/CuLagrange/geometry/kernel/geo_math.hpp +++ b/projects/CuLagrange/geometry/kernel/geo_math.hpp @@ -43,6 +43,13 @@ namespace zeno { namespace LSL_GEO { return doublearea(a,b,c) / (typename VecT::value_type)2.0; } + template 1)> = 0> + constexpr auto cotTheta(const zs::VecInterface& e0,const zs::VecInterface& e1){ + auto costheta = e0.dot(e1); + auto sintheta = e0.cross(e1).norm(); + return (costheta / sintheta); + } + template 1)> = 0> constexpr int orient(const zs::VecInterface& p0, const zs::VecInterface& p1, diff --git a/projects/CuLagrange/geometry/kernel/intersection.hpp b/projects/CuLagrange/geometry/kernel/intersection.hpp index 0cb88d5ebb..2915fe0351 100644 --- a/projects/CuLagrange/geometry/kernel/intersection.hpp +++ b/projects/CuLagrange/geometry/kernel/intersection.hpp @@ -2079,8 +2079,8 @@ int do_global_self_intersection_analysis_on_surface_mesh_info(Pol& pol, tri_pairs[1] = tris.pack(dim_c<3>,topo_tag,tb,int_c); for(int t = 0;t != 2;++t) { - zs::vec out_edges[3] = {}; - elm_to_edges(tri_pairs[t],out_edges); + // zs::vec out_edges[3] = {}; + auto out_edges = elm_to_edges(tri_pairs[t]); for(int i = 0;i != 3;++i) { auto a = out_edges[i][0]; auto b = out_edges[i][1]; diff --git a/projects/CuLagrange/geometry/kernel/topology.hpp b/projects/CuLagrange/geometry/kernel/topology.hpp index c26c48a28f..98dbe271f0 100644 --- a/projects/CuLagrange/geometry/kernel/topology.hpp +++ b/projects/CuLagrange/geometry/kernel/topology.hpp @@ -383,25 +383,82 @@ namespace zeno { return true; } - constexpr void elm_to_edges(const zs::vec& single_edge,zs::vec out_edges[1]) { - out_edges[0] = single_edge; - } + // constexpr auto elm_to_edges(const zs::vec& single_edge) { + // zs::vec out_edges[1] = {}; + // out_edges[0] = single_edge; + // return out_edges; + // } - constexpr void elm_to_edges(const zs::vec& tri,zs::vec out_edges[3]) { - out_edges[0] = zs::vec{tri[0],tri[1]}; - out_edges[1] = zs::vec{tri[1],tri[2]}; - out_edges[2] = zs::vec{tri[2],tri[0]}; - } + // constexpr auto elm_to_edges(const zs::vec& tri) { + // zs::vec out_edges[3] {}; + // out_edges[0] = zs::vec{tri[0],tri[1]}; + // out_edges[1] = zs::vec{tri[1],tri[2]}; + // out_edges[2] = zs::vec{tri[2],tri[0]}; + // return out_edges; + // } + + template 1)> = 0> + constexpr auto elm_to_edges(const zs::VecInterface& elm) { + using Ti = typename VecTi::value_type; + constexpr auto CODIM = VecTi::extent; + constexpr auto NM_EDGES = (CODIM - 1) * (CODIM) / 2; - constexpr void elm_to_edges(const zs::vec& tet,zs::vec out_edges[6]) { - out_edges[0] = zs::vec{tet[0],tet[1]}; - out_edges[1] = zs::vec{tet[0],tet[2]}; - out_edges[2] = zs::vec{tet[0],tet[3]}; - out_edges[3] = zs::vec{tet[1],tet[2]}; - out_edges[4] = zs::vec{tet[1],tet[3]}; - out_edges[5] = zs::vec{tet[2],tet[3]}; + zs::vec out_edges[NM_EDGES] = {}; + int nm_out_edges = 0; + for(int i = 0;i != CODIM;++i) + for(int j = i + 1;j != CODIM;++j) + out_edges[nm_out_edges++] = zs::vec{elm[i],elm[j]}; + + return out_edges; } + // template + // constexpr auto elm_to_edges(const VecI& tet,zs::wrapv) { + // constexpr int NM_EDGES = codim * (codim - 1) / 2; + // zs::vec out_edges[NM_EDGES] = {}; + // if(codim == 4) { + + // out_edges[0] = zs::vec{tet[0],tet[1]}; + // out_edges[1] = zs::vec{tet[0],tet[2]}; + // out_edges[2] = zs::vec{tet[0],tet[3]}; + // out_edges[3] = zs::vec{tet[1],tet[2]}; + // out_edges[4] = zs::vec{tet[1],tet[3]}; + // out_edges[5] = zs::vec{tet[2],tet[3]}; + // return out_edges; + // } + // if(codim == 3) { + // zs::vec out_edges[3] {}; + // out_edges[0] = zs::vec{tri[0],tri[1]}; + // out_edges[1] = zs::vec{tri[1],tri[2]}; + // out_edges[2] = zs::vec{tri[2],tri[0]}; + // return out_edges; + // } + // } + + // constexpr auto elm_to_edges(const zs::vec& single_edge) { + // zs::vec out_edges[1] = {}; + // out_edges[0] = single_edge; + // return out_edges; + // } + + // constexpr auto elm_to_edges(const zs::vec& tri) { + // zs::vec out_edges[3] {}; + // out_edges[0] = zs::vec{tri[0],tri[1]}; + // out_edges[1] = zs::vec{tri[1],tri[2]}; + // out_edges[2] = zs::vec{tri[2],tri[0]}; + // return out_edges; + // } + + // constexpr auto elm_to_edges(const zs::vec& tet) { + // zs::vec out_edges[6] = {}; + // out_edges[0] = zs::vec{tet[0],tet[1]}; + // out_edges[1] = zs::vec{tet[0],tet[2]}; + // out_edges[2] = zs::vec{tet[0],tet[3]}; + // out_edges[3] = zs::vec{tet[1],tet[2]}; + // out_edges[4] = zs::vec{tet[1],tet[3]}; + // out_edges[5] = zs::vec{tet[2],tet[3]}; + // return out_edges; + // } // template // bool topo_to_incident_matrix(Pol& pol, // const TopoTileVec& topo, @@ -1226,6 +1283,150 @@ namespace zeno { std::cout << "nm_colors : " << iterRef << std::endl; } + template + void sort_topology_by_coloring_tag(Pol& pol, + // zs::Vector& topos, + zs::Vector& reordered_map, + const zs::Vector& colors) { + using namespace zs; + constexpr auto space = Pol::exec_tag::value; + constexpr auto exec_tag = wrapv{}; + + // zs::Vector reordered_map{colors.get_allocator(),colors.size()}; + reordered_map.resize(colors.size()); + zs::Vector max_color{colors.get_allocator(),1}; + max_color.setVal(0); + + pol(zs::range(colors.size()),[ + colors = proxy(colors),\ + exec_tag = exec_tag, + max_color = proxy(max_color)] ZS_LAMBDA(int ci) mutable { + auto color = (int)colors[ci]; + atomic_max(exec_tag,&max_color[0],color); + }); + + int nm_total_colors = max_color.getVal(0) + 1; + // zs::bht color_buffer{} + zs::Vector nm_colors{colors.get_allocator(),nm_total_colors}; + pol(zs::range(nm_colors),[] ZS_LAMBDA(auto& nclr) mutable {nclr = 0;}); + pol(zs::range(colors),[nm_colors = proxy(nm_colors),exec_tag] ZS_LAMBDA(const auto& clrf) mutable { + auto clr = (int)clrf; + atomic_add(exec_tag,&nm_colors[clr],1); + }); + + zs::Vector exclusive_offsets{colors.get_allocator(),nm_total_colors}; + pol(zs::range(exclusive_offsets),[] ZS_LAMBDA(auto& eoffset) {eoffset = 0;}); + exclusive_scan(pol,std::begin(nm_colors),std::end(nm_colors),std::begin(exclusive_offsets)); + pol(zs::range(nm_colors),[] ZS_LAMBDA(auto& nclr) {nclr = 0;}); + + pol(zs::range(colors.size()),[ + nm_colors = proxy(nm_colors), + colors = proxy(colors), + exec_tag, + exclusive_offsets = proxy(exclusive_offsets), + reordered_map = proxy(reordered_map)] ZS_LAMBDA(auto ci) mutable { + auto clr = (int)colors[ci]; + auto offset = atomic_add(exec_tag,&nm_colors[clr],1); + auto eoffset = exclusive_offsets[clr]; + + reordered_map[eoffset + offset] = ci; + }); + + // zs::Vector topos_copy{topos.get_allocator(),topos.size()}; + // pol(zip(zs::range(topos.size()),topos),[topos_copy = proxy(topos_copy)] ZS_LAMBDA(auto ti,const auto& topo) mutable {topos_copy[ti] = topo;}); + + // pol(zip(zs::range(topos.size()),topos),[ + // topos_copy = proxy(topos_copy), + // reordered_map = proxy(reordered_map)] ZS_LAMBDA(auto ti,auto& topo) mutable {topo = topos_copy[reordered_map[ti]];}); + } + + template> + zs::Vector tilevec_topo_to_zsvec_topo(Pol& pol,const TopoTileVec& source,zs::wrapv) { + zs::Vector out_topo{source.get_allocator(),source.size()}; + auto sr = zs::range(source, "inds", zs::dim_c, zs::int_c); + pol(zip(sr, out_topo), []ZS_LAMBDA(auto id, VecTi& dst) mutable { + if constexpr (std::is_integral_v) + dst[0] = id; + else + dst = id; + }); + return out_topo; + } + + template + void retrieve_tri_bending_topology(Pol& pol, + const TriTileVec& tris, + const HalfEdgeTileVec& halfedges, + zs::Vector>& tb_topos) { + using namespace zs; + constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; + constexpr auto exec_tag = wrapv{}; + + // zs::Vector nm_interior_edges{halfedges.get_allocator(),1}; + // nm_interior_edges.setVal(0); + + zs::bht interior_edges{halfedges.get_allocator(),halfedges.size()}; + interior_edges.reset(pol,true); + + pol(zs::range(halfedges.size()),[ + halfedges = proxy({},halfedges), + exec_tag, + interior_edges = proxy(interior_edges)] ZS_LAMBDA(int hi) mutable { + auto ohi = zs::reinterpret_bits(halfedges("opposite_he",hi)); + // the boundary halfedge will return -1 for opposite_he here, so it is automatically neglected + if(ohi < hi) + return; + interior_edges.insert(hi); + }); + + tb_topos.resize(interior_edges.size()); + pol(zs::zip(zs::range(interior_edges.size()),interior_edges._activeKeys),[ + tb_topos = proxy(tb_topos), + halfedges = proxy({},halfedges), + tris = proxy({},tris)] ZS_LAMBDA(auto id,auto hi_vec) mutable { + auto hi = hi_vec[0]; + auto ti = zs::reinterpret_bits(halfedges("to_face",hi)); + auto vid = zs::reinterpret_bits(halfedges("local_vertex_id",hi)); + auto ohi = zs::reinterpret_bits(halfedges("opposite_he",hi)); + auto oti = zs::reinterpret_bits(halfedges("to_face",ohi)); + auto ovid = zs::reinterpret_bits(halfedges("local_vertex_id",ohi)); + + auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); + auto otri = tris.pack(dim_c<3>,"inds",oti,int_c); + + tb_topos[id] = zs::vec(tri[(vid + 0) % 3],tri[(vid + 1) % 3],tri[(vid + 2) % 3],otri[(ovid + 2) % 3]); + }); + } + + template + void retrieve_edges_topology(Pol& pol, + const zs::Vector& src_topos, + zs::Vector>& edges_topos) { + using namespace zs; + constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; + // constexpr auto CDIM = VecTi::extent; + // constexpr auto NM_EDGES = CDIM * (CDIM - 1) / 2; + + zs::bht edges_tab{src_topos.get_allocator(),src_topos.size() * 6}; + edges_tab.reset(pol,true); + pol(zs::range(src_topos.size()),[ + src_topos = proxy(src_topos), + edges_tab = proxy(edges_tab)] ZS_LAMBDA(int ei) mutable { + auto elm_edges = elm_to_edges(src_topos[ei]); + for(int i = 0;i != NM_EDGES;++i) { + auto edge = elm_edges[i]; + if(edge[0] < edge[1]) + edges_tab.insert(zs::vec{edge[0],edge[1]}); + else + edges_tab.insert(zs::vec{edge[1],edge[0]}); + } + }); + + edges_topos.resize(edges_tab.size()); + pol(zip(zs::range(edges_tab.size()),zs::range(edges_tab._activeKeys)),[ + edges_topos = proxy(edges_topos)] ZS_LAMBDA(auto ei,const auto& edge){edges_topos[ei] = edge;}); + } + template void reorder_topology(Pol& pol, const TopoTileVec& reorder_map, diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index 15d2e95ac4..ffe9da29f5 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -12,44 +12,240 @@ #include #include +#include "../geometry/kernel/topology.hpp" +#include "../geometry/kernel/geo_math.hpp" + namespace zeno { // we only need to record the topo here -struct MakeGenericConstraintTopology : zeno::INode { - using namespace zs; +// serve triangulate mesh or strands only currently +struct MakeSurfaceConstraintTopology : INode { + virtual void apply() override { + using namespace zs; + using vec3 = zs::vec; using vec2i = zs::vec; using vec3i = zs::vec; using vec4i = zs::vec; + using mat4 = zs::vec; constexpr auto space = execspace_e::cuda; + auto cudaPol = zs::cuda_exec(); auto source = get_input("source"); auto constraint = std::make_shared(); - auto type = get_param("type"); - auto groupID = get_param("groupID"); + auto type = get_param("topo_type"); + + if(source->category != ZenoParticles::surface) + throw std::runtime_error("Try adding Constraint topology to non-surface ZenoParticles"); + + const auto& verts = source->getParticles(); + const auto& quads = source->getQuadraturePoints(); + + auto uniform_stiffness = get_input2("stiffness"); + + + if(type == "stretch") { + auto quads_vec = tilevec_topo_to_zsvec_topo(cudaPol,quads,wrapv<3>{}); + zs::Vector> edge_topos{quads.get_allocator(),0}; + retrieve_edges_topology(cudaPol,quads_vec,edge_topos); + // std::cout << "quads.size() = " << quads.size() << "\t" << "edge_topos.size() = " << edge_topos.size() << std::endl; + + constraint->category = ZenoParticles::curve; + constraint->sprayedOffset = 0; + constraint->elements = typename ZenoParticles::particles_t({{"inds",2},{"r",1},{"stiffness",1},{"lambda",1}}, edge_topos.size(), zs::memsrc_e::device,0); + auto &eles = constraint->getQuadraturePoints(); + cudaPol(zs::range(eles.size()),[ + verts = proxy({},verts), + eles = proxy({},eles), + uniform_stiffness = uniform_stiffness, + edge_topos = proxy(edge_topos)] ZS_LAMBDA(int ei) mutable { + eles.tuple(dim_c<2>,"inds",ei) = edge_topos[ei].reinterpret_bits(float_c); + vec3 x[2] = {}; + for(int i = 0;i != 2;++i) + x[i] = verts.pack(dim_c<3>,"x",edge_topos[ei][i]); + eles("r",ei) = (x[0] - x[1]).norm(); + eles("stiffness",1) = uniform_stiffness; + eles("lambda",1) = .0f; + }); + } + + if(type == "iso_bending") { + constraint->category = ZenoParticles::tri_bending_spring; + constraint->sprayedOffset = 0; + + const auto& halfedges = (*source)[ZenoParticles::s_surfHalfEdgeTag]; + + zs::Vector> bd_topos{quads.get_allocator(),0}; + retrieve_tri_bending_topology(cudaPol,quads,halfedges,bd_topos); + + // std::cout << "halfedges.size() = " << halfedges.size() << "\t" << "bd_topos.size() = " << bd_topos.size() << std::endl; + + constraint->elements = typename ZenoParticles::particles_t({{"inds",4},{"Q", 4 * 4},{"stiffness",1},{"lambda",1}},bd_topos.size(),zs::memsrc_e::device,0); + auto& eles = constraint->getQuadraturePoints(); + + cudaPol(zs::range(eles.size()),[ + eles = proxy({},eles), + uniform_stiffness = uniform_stiffness, + bd_topos = proxy(bd_topos), + verts = proxy({},verts)] ZS_LAMBDA(int ei) mutable { + eles.tuple(dim_c<4>,"inds",ei) = bd_topos[ei].reinterpret_bits(float_c); + vec3 x[4] = {}; + for(int i = 0;i != 4;++i) + x[i] = verts.pack(dim_c<3>,"x",bd_topos[ei][i]); + + auto e0 = x[1] - x[0]; + auto e1 = x[2] - x[0]; + auto e2 = x[3] - x[0]; + auto e3 = x[2] - x[1]; + auto e4 = x[3] - x[1]; + + auto c01 = LSL_GEO::cotTheta(e0, e1); + auto c02 = LSL_GEO::cotTheta(e0, e2); + auto c03 = LSL_GEO::cotTheta(-e0, e3); + auto c04 = LSL_GEO::cotTheta(-e0, e4); + + auto A0 = 0.5f * (e0.cross(e1)).norm(); + auto A1 = 0.5f * (e0.cross(e2)).norm(); + + auto coef = -3.f / (2.f * (A0 + A1)); + float K[4] = { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 }; + float K2[4] = { coef * K[0], coef * K[1], coef * K[2], coef * K[3] }; + + mat4 Q = mat4::uniform(0); + + for (unsigned char j = 0; j < 4; j++) + { + for (unsigned char k = 0; k < j; k++) + { + Q(j, k) = Q(k, j) = K[j] * K2[k]; + } + Q(j, j) = K[j] * K2[j]; + } + + eles.tuple(dim_c<16>,"Q",ei) = Q; + eles("stiffness",1) = uniform_stiffness; + eles("lambda",1) = .0f; + }); + } - // set_output("target",target); set_output("source",source); set_output("constraint",constraint); }; }; -ZENDEFNODE(MakeGenericConstraintTopology, {{{"source"}}, - {{"constraints"}}, +ZENDEFNODE(MakeSurfaceConstraintTopology, {{{"source"},{"float","stiffness","0.5"}}, + {{"constraint"}}, { - {"enum points edges tris tets segment_angle dihedral_angle","topo_type","points"}, - {"string","groupID",""}, + {"enum stretch iso_bending","topo_type","stretch"}, + // {"string","groupID",""}, }, {"PBD"}}); -struct XPBDSolve : zeno::INode { - using namespace zs; + +struct VisualizePBDConstraint : INode { + using T = float; + using vec3 = zs::vec; + // using tiles_t = typename ZenoParticles::particles_t; + // using dtiles_t = zs::TileVector; + + virtual void apply() override { + using namespace zs; + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + + auto zsparticles = get_input("zsparticles"); + auto constraints_ptr = get_input("constraints"); + + const auto& geo_verts = zsparticles->getParticles(); + const auto& constraints = constraints_ptr->getQuadraturePoints(); + + auto tclr_tag = get_param("tclrTag"); + + zs::Vector cvis{geo_verts.get_allocator(),constraints.getChannelSize("inds") * constraints.size()}; + zs::Vector cclrs{constraints.get_allocator(),constraints.size()}; + int cdim = constraints.getChannelSize("inds"); + cudaPol(zs::range(constraints.size()),[ + constraints = proxy({},constraints), + geo_verts = proxy({},geo_verts), + cclrs = proxy(cclrs), + tclr_tag = zs::SmallString(tclr_tag), + cdim = cdim, + cvis = proxy(cvis)] ZS_LAMBDA(int ci) mutable { + // auto cdim = constraints.propertySize("inds"); + for(int i = 0;i != cdim;++i) { + auto vi = zs::reinterpret_bits(constraints("inds",i,ci)); + cvis[ci * cdim + i] = geo_verts.pack(dim_c<3>,"x",vi); + } + cclrs[ci] = (int)constraints(tclr_tag,ci); + }); + + constexpr auto omp_space = execspace_e::openmp; + auto ompPol = omp_exec(); + + cvis = cvis.clone({zs::memsrc_e::host}); + cclrs = cclrs.clone({zs::memsrc_e::host}); + auto prim = std::make_shared(); + auto& pverts = prim->verts; + + if(constraints_ptr->category == ZenoParticles::curve) { + pverts.resize(constraints.size() * 2); + auto& plines = prim->lines; + plines.resize(constraints.size()); + auto& tclrs = pverts.add_attr(tclr_tag); + auto& ltclrs = plines.add_attr(tclr_tag); + + ompPol(zs::range(constraints.size()),[ + <clrs,&pverts,&plines,&tclrs,cvis = proxy(cvis),cclrs = proxy(cclrs)] (int ci) mutable { + pverts[ci * 2 + 0] = cvis[ci * 2 + 0].to_array(); + pverts[ci * 2 + 1] = cvis[ci * 2 + 1].to_array(); + tclrs[ci * 2 + 0] = cclrs[ci]; + tclrs[ci * 2 + 1] = cclrs[ci]; + plines[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + ltclrs[ci] = cclrs[ci]; + }); + }else if(constraints_ptr->category == ZenoParticles::tri_bending_spring) { + pverts.resize(constraints.size() * 2); + auto& plines = prim->lines; + plines.resize(constraints.size()); + auto& tclrs = pverts.add_attr(tclr_tag); + auto& ltclrs = plines.add_attr(tclr_tag); + + ompPol(zs::range(constraints.size()),[ + <clrs,&pverts,&plines,&tclrs,cvis = proxy(cvis),cclrs = proxy(cclrs)] (int ci) mutable { + zeno::vec3f cverts[4] = {}; + for(int i = 0;i != 4;++i) + cverts[i] = cvis[ci * 4 + i].to_array(); + + pverts[ci * 2 + 0] = (cverts[0] + cverts[1] + cverts[2]) / (T)3.0; + pverts[ci * 2 + 1] = (cverts[0] + cverts[1] + cverts[3]) / (T)3.0; + tclrs[ci * 2 + 0] = cclrs[ci]; + tclrs[ci * 2 + 1] = cclrs[ci]; + ltclrs[ci] = cclrs[ci]; + + plines[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + }); + } + + set_output("prim",std::move(prim)); + } +}; + +ZENDEFNODE(VisualizePBDConstraint, {{{"zsparticles"},{"constraints"}}, + {{"prim"}}, + { + {"string","tclrTag","tclrTag"}, + }, + {"PBD"}}); + +struct XPBDSolve : INode { + virtual void apply() override { + using namespace zs; constexpr auto space = execspace_e::cuda; auto zsparticles = get_input("zsparticles"); - auto constraints = get_input("constraints"); + auto constraints = get_input("constraints"); set_output("constraints",constraints); set_output("zsparticles",zsparticles); From 0e6461624537dcc28f7e143a71d55bfd1b57db3e Mon Sep 17 00:00:00 2001 From: ShuliangLu Date: Wed, 19 Jul 2023 14:47:08 +0800 Subject: [PATCH 05/21] XPBD basic cloth simulation --- projects/CuLagrange/geometry/BasicGeoNodes.cu | 22 +- projects/CuLagrange/geometry/Topology.cu | 19 +- .../CuLagrange/geometry/kernel/topology.hpp | 38 +- projects/CuLagrange/pbd/Pipeline.cu | 399 +++++++- .../constraint_function_kernel/constraint.cuh | 946 +++++++++--------- 5 files changed, 896 insertions(+), 528 deletions(-) diff --git a/projects/CuLagrange/geometry/BasicGeoNodes.cu b/projects/CuLagrange/geometry/BasicGeoNodes.cu index b5b4d12721..595bff13a9 100755 --- a/projects/CuLagrange/geometry/BasicGeoNodes.cu +++ b/projects/CuLagrange/geometry/BasicGeoNodes.cu @@ -41,14 +41,18 @@ struct ZSComputeSurfaceArea : zeno::INode { } TILEVEC_OPS::fill(cudaPol,verts,attrName,(T)0.0); - zs::Vector nmIncidentTris{verts.get_allocator(),verts.size()}; - cudaPol(zs::range(nmIncidentTris),[] ZS_LAMBDA(int& count) mutable {count = 0;}); + // zs::Vector nmIncidentTris{verts.get_allocator(),verts.size()}; + zs::Vector nodal_area{verts.get_allocator(),verts.size()}; + + // cudaPol(zs::range(nmIncidentTris),[] ZS_LAMBDA(auto& count) mutable {count = 0;}); + cudaPol(zs::range(nodal_area),[] ZS_LAMBDA(auto& A) mutable {A = 0;}); cudaPol(zs::range(tris.size()),[ exec_tag, attrName = zs::SmallString(attrName), tris = proxy({},tris), - nmIncidentTris = proxy(nmIncidentTris), + nodal_area = proxy(nodal_area), + // nmIncidentTris = proxy(nmIncidentTris), verts = proxy({},verts)] ZS_LAMBDA(int ti) mutable { auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); zs::vec tV[3] = {}; @@ -57,17 +61,17 @@ struct ZSComputeSurfaceArea : zeno::INode { auto A = LSL_GEO::area(tV[0],tV[1],tV[2]); tris(attrName,ti) = A; for(int i = 0;i != 3;++i) { - atomic_add(exec_tag,&verts(attrName,tri[i]),A); - atomic_add(exec_tag,&nmIncidentTris[0],(int)1); + atomic_add(exec_tag,&nodal_area[tri[i]],A / (T)3.0); + // atomic_add(exec_tag,&nmIncidentTris[0],(int)1); } - }); + }); cudaPol(zs::range(verts.size()),[ verts = proxy({},verts), attrName = zs::SmallString(attrName), - nmIncidentTris = proxy(nmIncidentTris)] ZS_LAMBDA(int vi) mutable { - if(nmIncidentTris[vi] > 0) - verts(attrName,vi) = verts(attrName,vi) / (T)nmIncidentTris[vi]; + nodal_area = proxy(nodal_area)] ZS_LAMBDA(int vi) mutable { + // if(nmIncidentTris[vi] > 0) + verts(attrName,vi) = nodal_area[vi]; }); set_output("zsparticles",zsparticles); diff --git a/projects/CuLagrange/geometry/Topology.cu b/projects/CuLagrange/geometry/Topology.cu index ce8595ba6d..31325dea65 100644 --- a/projects/CuLagrange/geometry/Topology.cu +++ b/projects/CuLagrange/geometry/Topology.cu @@ -722,13 +722,15 @@ struct DoTopogicalColoring : zeno::INode { }); zs::Vector colors{elms.get_allocator(),elms.size()}; - std::cout << "do topological coloring" << std::endl; + // std::cout << "do topological coloring" << std::endl; topological_coloring(cudaPol,topos,colors); zs::Vector reordered_map{elms.get_allocator(),elms.size()}; cudaPol(zs::range(reordered_map.size()),[reordered_map = proxy(reordered_map)] ZS_LAMBDA(int ti) mutable {reordered_map[ti] = ti;}); - if(do_sort_color) - sort_topology_by_coloring_tag(cudaPol,reordered_map,colors); - std::cout << "finish topological coloring" << std::endl; + // if(do_sort_color) + zs::Vector color_offset{elms.get_allocator(),0}; + sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); + + // std::cout << "finish topological coloring" << std::endl; cudaPol(zs::range(elms.size()),[ elms = proxy({},elms), @@ -737,11 +739,16 @@ struct DoTopogicalColoring : zeno::INode { reordered_map = proxy(reordered_map), cdim, colors = proxy(colors)] ZS_LAMBDA(int ei) mutable { - elms(color_tag,ei) = colors[reordered_map[ei]]; + elms(color_tag,ei) = colors[reordered_map[ei]]; for(int i = 0;i != cdim;++i) elms("inds",i,ei) = zs::reinterpret_bits(topos[reordered_map[ei]][i]); }); + zsparticles->setMeta("color_offset",color_offset); + printf("offset : "); + for(int i = 0;i != color_offset.size();++i) + printf("%d\t",color_offset.getVal(i)); + printf("\n"); set_output("zsparticles",zsparticles); } }; @@ -752,7 +759,7 @@ ZENDEFNODE(DoTopogicalColoring, {{{"zsparticles"}}, }, { {"string","colorTag","colorTag"}, - {"bool","sort_color","1"} + // {"bool","sort_color","1"} }, {"ZSGeometry"}}); diff --git a/projects/CuLagrange/geometry/kernel/topology.hpp b/projects/CuLagrange/geometry/kernel/topology.hpp index 98dbe271f0..12217e53f2 100644 --- a/projects/CuLagrange/geometry/kernel/topology.hpp +++ b/projects/CuLagrange/geometry/kernel/topology.hpp @@ -1112,20 +1112,21 @@ namespace zeno { } - template= 2), (VecTI::etent <= 4)> = 0*/> + template= 2), (VecTI::etent <= 4)> = 0*/> void topological_incidence_matrix(Pol& pol, // size_t nm_points, - const zs::Vector& topos, + const TopoRangT& topos, zs::SparseMatrix& spmat) { using namespace zs; using ICoord = zs::vec; - constexpr auto CDIM = VecTI::extent; + // constexpr auto CDIM = VecTI::extent; + constexpr auto CDIM = RM_CVREF_T(topos[0])::extent; constexpr auto space = Pol::exec_tag::value; constexpr auto execTag = wrapv{}; zs::Vector max_pi_vec{topos.get_allocator(),1}; max_pi_vec.setVal(0); - pol(zs::range(topos),[max_pi_vec = proxy(max_pi_vec),execTag,CDIM] ZS_LAMBDA(const auto& topo) { + pol(zs::range(topos),[max_pi_vec = proxy(max_pi_vec),execTag,CDIM] ZS_LAMBDA(const auto& topo) mutable { for(int i = 0;i != CDIM;++i) if(topo[i] >= 0) atomic_max(execTag,&max_pi_vec[0],(int)topo[i]); @@ -1192,8 +1193,6 @@ namespace zeno { // printf("p[%d] -> t[%d]\n",pi,ti); } }); - - // std::cout << "finish computing p2ts" << std::endl; } @@ -1235,6 +1234,8 @@ namespace zeno { // pol(zs::range(is.size()),[is = proxy(is),js = proxy(js)] ZS_LAMBDA(int i) mutable {printf("ijs[%d] : %d %d\n",i,is[i],js[i]);}); // std::cout << "topos.size() = " << topos.size() << std::endl; + // for(int i = 0;i != topos.size();++i) + // std::cout << topos.getVal(i)[0] << "\t" << topos.getVal(i)[1] << std::endl; // spmat = zs::SparseMatrix{topos.get_allocator(),(int)topos.size(),(int)topos.size()}; spmat.build(pol,(int)topos.size(),(int)topos.size(),zs::range(is),zs::range(js)/*,zs::range(rs)*/,zs::true_c); @@ -1246,13 +1247,15 @@ namespace zeno { // spmat._vals.reset((int)1); } - template + template void topological_coloring(Pol& pol, // int nm_points, - const zs::Vector& topo, - zs::Vector& colors) { + const TopoRangeT& topo, + ColorRangeT& colors) { using namespace zs; constexpr auto space = Pol::exec_tag::value; + using Ti = RM_CVREF_T(colors[0]); + colors.resize(topo.size()); zs::SparseMatrix topo_incidence_matrix{topo.get_allocator(),topo.size(),topo.size()}; @@ -1277,17 +1280,23 @@ namespace zeno { w = v; }); } + + // pol(zs::range()) weights = weights.clone(colors.memoryLocation()); + // for(int i = 0;i != weights.size();++i) + // printf("w[%d] : %u\n",i,weights.getVal(i)); auto iterRef = maximum_independent_sets(pol, topo_incidence_matrix, weights, colors); std::cout << "nm_colors : " << iterRef << std::endl; + pol(zs::range(colors),[] ZS_LAMBDA(auto& clr) mutable {clr = clr - (Ti)1;}); + } - template + template void sort_topology_by_coloring_tag(Pol& pol, - // zs::Vector& topos, - zs::Vector& reordered_map, - const zs::Vector& colors) { + const COLOR_RANGE& colors, + REORDERED_MAP_RANGE& reordered_map, + EXCLUSIVE_OFFSET_RANGE& offset_out) { using namespace zs; constexpr auto space = Pol::exec_tag::value; constexpr auto exec_tag = wrapv{}; @@ -1319,6 +1328,9 @@ namespace zeno { exclusive_scan(pol,std::begin(nm_colors),std::end(nm_colors),std::begin(exclusive_offsets)); pol(zs::range(nm_colors),[] ZS_LAMBDA(auto& nclr) {nclr = 0;}); + offset_out.resize(nm_total_colors); + + pol(zip(zs::range(exclusive_offsets.size()),exclusive_offsets),[offset_out = proxy(offset_out)] ZS_LAMBDA(auto i,auto offset) mutable {offset_out[i] = offset;}); pol(zs::range(colors.size()),[ nm_colors = proxy(nm_colors), colors = proxy(colors), diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index ffe9da29f5..a529e35af3 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -12,6 +12,7 @@ #include #include +#include "constraint_function_kernel/constraint.cuh" #include "../geometry/kernel/topology.hpp" #include "../geometry/kernel/geo_math.hpp" @@ -35,7 +36,7 @@ struct MakeSurfaceConstraintTopology : INode { auto source = get_input("source"); auto constraint = std::make_shared(); - auto type = get_param("topo_type"); + auto type = get_input2("topo_type"); if(source->category != ZenoParticles::surface) throw std::runtime_error("Try adding Constraint topology to non-surface ZenoParticles"); @@ -45,52 +46,87 @@ struct MakeSurfaceConstraintTopology : INode { auto uniform_stiffness = get_input2("stiffness"); + zs::Vector colors{quads.get_allocator(),0}; + zs::Vector reordered_map{quads.get_allocator(),0}; + zs::Vector color_offset{quads.get_allocator(),0}; + + constraint->sprayedOffset = 0; + constraint->elements = typename ZenoParticles::particles_t({{"stiffness",1},{"lambda",1},{"tclr",1}}, 0, zs::memsrc_e::device,0); + auto &eles = constraint->getQuadraturePoints(); if(type == "stretch") { + constraint->category = ZenoParticles::curve; + auto quads_vec = tilevec_topo_to_zsvec_topo(cudaPol,quads,wrapv<3>{}); zs::Vector> edge_topos{quads.get_allocator(),0}; retrieve_edges_topology(cudaPol,quads_vec,edge_topos); + // zs::Vector> edge_topos{quads.get_allocator(),edge_topos_broken.size()}; + // cudaPol(zs::range(edge_topos.size()),[ + // edge_topos = proxy(edge_topos), + // edge_topos_broken = proxy(edge_topos_broken)] ZS_LAMBDA(int ei) mutable { + // edge_topos[ei][0] = edge_topos_broken[ei][0]; + // edge_topos[ei][1] = edge_topos_broken[ei][1]; + // edge_topos[ei][2] = -1; + // edge_topos[ei][3] = -1; + // }); + + + eles.resize(edge_topos.size()); + + topological_coloring(cudaPol,edge_topos,colors); + sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); // std::cout << "quads.size() = " << quads.size() << "\t" << "edge_topos.size() = " << edge_topos.size() << std::endl; - - constraint->category = ZenoParticles::curve; - constraint->sprayedOffset = 0; - constraint->elements = typename ZenoParticles::particles_t({{"inds",2},{"r",1},{"stiffness",1},{"lambda",1}}, edge_topos.size(), zs::memsrc_e::device,0); - auto &eles = constraint->getQuadraturePoints(); + eles.append_channels(cudaPol,{{"inds",2},{"r",1}}); + + auto rest_scale = get_input2("rest_scale"); + cudaPol(zs::range(eles.size()),[ verts = proxy({},verts), eles = proxy({},eles), + reordered_map = proxy(reordered_map), uniform_stiffness = uniform_stiffness, - edge_topos = proxy(edge_topos)] ZS_LAMBDA(int ei) mutable { - eles.tuple(dim_c<2>,"inds",ei) = edge_topos[ei].reinterpret_bits(float_c); + colors = proxy(colors), + rest_scale = rest_scale, + edge_topos = proxy(edge_topos)] ZS_LAMBDA(int oei) mutable { + auto ei = reordered_map[oei]; + // auto edge_full = edge_topos[ei]; + // vec2i edge{edge_full[0],edge_full[1]}; + + eles.tuple(dim_c<2>,"inds",oei) = edge_topos[ei].reinterpret_bits(float_c); vec3 x[2] = {}; for(int i = 0;i != 2;++i) x[i] = verts.pack(dim_c<3>,"x",edge_topos[ei][i]); - eles("r",ei) = (x[0] - x[1]).norm(); - eles("stiffness",1) = uniform_stiffness; - eles("lambda",1) = .0f; - }); + eles("r",oei) = (x[0] - x[1]).norm() * rest_scale; + }); + } - if(type == "iso_bending") { + if(type == "bending") { constraint->category = ZenoParticles::tri_bending_spring; - constraint->sprayedOffset = 0; + // constraint->sprayedOffset = 0; const auto& halfedges = (*source)[ZenoParticles::s_surfHalfEdgeTag]; zs::Vector> bd_topos{quads.get_allocator(),0}; retrieve_tri_bending_topology(cudaPol,quads,halfedges,bd_topos); - // std::cout << "halfedges.size() = " << halfedges.size() << "\t" << "bd_topos.size() = " << bd_topos.size() << std::endl; + eles.resize(bd_topos.size()); + + topological_coloring(cudaPol,bd_topos,colors); + sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); + // std::cout << "quads.size() = " << quads.size() << "\t" << "edge_topos.size() = " << edge_topos.size() << std::endl; + + eles.append_channels(cudaPol,{{"inds",4},{"Q",4 * 4}}); - constraint->elements = typename ZenoParticles::particles_t({{"inds",4},{"Q", 4 * 4},{"stiffness",1},{"lambda",1}},bd_topos.size(),zs::memsrc_e::device,0); - auto& eles = constraint->getQuadraturePoints(); + // std::cout << "halfedges.size() = " << halfedges.size() << "\t" << "bd_topos.size() = " << bd_topos.size() << std::endl; cudaPol(zs::range(eles.size()),[ eles = proxy({},eles), - uniform_stiffness = uniform_stiffness, bd_topos = proxy(bd_topos), - verts = proxy({},verts)] ZS_LAMBDA(int ei) mutable { - eles.tuple(dim_c<4>,"inds",ei) = bd_topos[ei].reinterpret_bits(float_c); + reordered_map = proxy(reordered_map), + verts = proxy({},verts)] ZS_LAMBDA(int oei) mutable { + auto ei = reordered_map[oei]; + eles.tuple(dim_c<4>,"inds",oei) = bd_topos[ei].reinterpret_bits(float_c); vec3 x[4] = {}; for(int i = 0;i != 4;++i) x[i] = verts.pack(dim_c<3>,"x",bd_topos[ei][i]); @@ -124,21 +160,39 @@ struct MakeSurfaceConstraintTopology : INode { Q(j, j) = K[j] * K2[j]; } - eles.tuple(dim_c<16>,"Q",ei) = Q; - eles("stiffness",1) = uniform_stiffness; - eles("lambda",1) = .0f; + eles.tuple(dim_c<16>,"Q",oei) = Q; }); } + cudaPol(zs::range(eles.size()),[ + eles = proxy({},eles), + uniform_stiffness = uniform_stiffness, + colors = proxy(colors), + // exec_tag, + reordered_map = proxy(reordered_map)] ZS_LAMBDA(int oei) mutable { + auto ei = reordered_map[oei]; + eles("lambda",oei) = 0.0; + eles("stiffness",oei) = uniform_stiffness; + eles("tclr",oei) = colors[ei]; + // auto + }); + + constraint->setMeta("color_offset",color_offset); + set_output("source",source); set_output("constraint",constraint); }; }; -ZENDEFNODE(MakeSurfaceConstraintTopology, {{{"source"},{"float","stiffness","0.5"}}, +ZENDEFNODE(MakeSurfaceConstraintTopology, {{ + {"source"}, + {"float","stiffness","0.5"}, + {"string","topo_type","stretch"}, + {"float","rest_scale","1.0"} + }, {{"constraint"}}, { - {"enum stretch iso_bending","topo_type","stretch"}, + // {"string","groupID",""}, }, {"PBD"}}); @@ -239,22 +293,309 @@ ZENDEFNODE(VisualizePBDConstraint, {{{"zsparticles"},{"constraints"}}, }, {"PBD"}}); +// solve a specific type of constraint for one iterations struct XPBDSolve : INode { virtual void apply() override { using namespace zs; + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + + auto zsparticles = get_input("zsparticles"); + auto constraints = get_input("constraints"); + auto dt = get_input2("dt"); + auto ptag = get_param("ptag"); + + auto coffsets = constraints->readMeta("color_offset",zs::wrapt>{}); + int nm_group = coffsets.size(); + + auto& verts = zsparticles->getParticles(); + auto& cquads = constraints->getQuadraturePoints(); + auto category = constraints->category; + + // zs::Vector pv_buffer{verts.get_allocator(),verts.size()}; + // zs::Vector total_ghost_impulse_X{verts.get_allocator(),1}; + // zs::Vector total_ghost_impulse_Y{verts.get_allocator(),1}; + // zs::Vector total_ghost_impulse_Z{verts.get_allocator(),1}; + + for(int g = 0;g != nm_group;++g) { + auto coffset = coffsets.getVal(g); + int group_size = 0; + if(g == nm_group - 1) + group_size = cquads.size() - coffsets.getVal(g); + else + group_size = coffsets.getVal(g + 1) - coffsets.getVal(g); + + // cudaPol(zs::range(verts.size()),[ + // ptag = zs::SmallString(ptag), + // verts = proxy({},verts), + // pv_buffer = proxy(pv_buffer)] ZS_LAMBDA(int vi) mutable { + // pv_buffer[vi] = verts.pack(dim_c<3>,ptag,vi); + // }); + + cudaPol(zs::range(group_size),[ + coffset, + verts = proxy({},verts), + category, + dt, + ptag = zs::SmallString(ptag), + cquads = proxy({},cquads)] ZS_LAMBDA(int gi) mutable { + float s = cquads("stiffness",coffset + gi); + float lambda = cquads("lambda",coffset + gi); + + if(category == ZenoParticles::curve) { + auto edge = cquads.pack(dim_c<2>,"inds",coffset + gi,int_c); + vec3 p0{},p1{}; + p0 = verts.pack(dim_c<3>,ptag,edge[0]); + p1 = verts.pack(dim_c<3>,ptag,edge[1]); + float minv0 = verts("minv",edge[0]); + float minv1 = verts("minv",edge[1]); + float r = cquads("r",coffset + gi); + + vec3 dp0{},dp1{}; + CONSTRAINT::solve_DistanceConstraint( + p0,minv0, + p1,minv1, + r, + s, + dt, + lambda, + dp0,dp1); + + + verts.tuple(dim_c<3>,ptag,edge[0]) = p0 + dp0; + verts.tuple(dim_c<3>,ptag,edge[1]) = p1 + dp1; + + // float m0 = verts("m",edge[0]); + // float m1 = verts("m",edge[1]); + // auto ghost_impulse = (dp0 * m0 + dp1 * m1).norm(); + // if(ghost_impulse > 1e-6) + // printf("dmomentum : %f\n",(float)ghost_impulse); + } + if(category == ZenoParticles::tri_bending_spring) { + auto quad = cquads.pack(dim_c<4>,"inds",coffset + gi,int_c); + vec3 p[4] = {}; + float minv[4] = {}; + for(int i = 0;i != 4;++i) { + p[i] = verts.pack(dim_c<3>,ptag,quad[i]); + minv[i] = verts("minv",quad[i]); + } + + auto Q = cquads.pack(dim_c<4,4>,"Q",coffset + gi); + + vec3 dp[4] = {}; + CONSTRAINT::solve_IsometricBendingConstraint( + p[0],minv[0], + p[1],minv[1], + p[2],minv[2], + p[3],minv[3], + Q, + s, + dt, + lambda, + dp[0],dp[1],dp[2],dp[3]); + + for(int i = 0;i != 4;++i) + verts.tuple(dim_c<3>,ptag,quad[i]) = p[i] + dp[i]; + } + cquads("lambda",coffset + gi) = lambda; + }); + + // total_ghost_impulse_X.setVal(0); + // total_ghost_impulse_Y.setVal(0); + // total_ghost_impulse_Z.setVal(0); + // cudaPol(zs::range(verts.size()),[ + // verts = proxy({},verts), + // ptag = zs::SmallString(ptag), + // exec_tag, + // total_ghost_impulse_X = proxy(total_ghost_impulse_X), + // total_ghost_impulse_Y = proxy(total_ghost_impulse_Y), + // total_ghost_impulse_Z = proxy(total_ghost_impulse_Z), + // pv_buffer = proxy(pv_buffer)] ZS_LAMBDA(int vi) mutable { + // auto cv = verts.pack(dim_c<3>,ptag,vi); + // auto pv = pv_buffer[vi]; + // auto m = verts("m",vi); + // auto dv = m * (cv - pv); + // // for(int i = 0;i != 3;++i) + // atomic_add(exec_tag,&total_ghost_impulse_X[0],dv[0]); + // atomic_add(exec_tag,&total_ghost_impulse_Y[0],dv[1]); + // atomic_add(exec_tag,&total_ghost_impulse_Z[0],dv[2]); + // }); + + // auto tgi = total_ghost_impulse.getVal(0); + // auto tgx = total_ghost_impulse_X.getVal(0); + // auto tgy = total_ghost_impulse_Y.getVal(0); + // auto tgz = total_ghost_impulse_Z.getVal(0); + // printf("ghost_impulse[%d][%d] : %f %f %f\n",(int)coffset,(int)group_size,(float)tgx,(float)tgy,(float)tgz); + } + + set_output("constraints",constraints); + set_output("zsparticles",zsparticles); + }; +}; + +ZENDEFNODE(XPBDSolve, {{{"zsparticles"},{"constraints"},{"float","dt","0.5"}}, + {{"zsparticles"},{"constraints"}}, + {{"string","ptag","X"}}, + {"PBD"}}); + +struct XPBDSolveSmooth : INode { + + virtual void apply() override { + using namespace zs; + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + constexpr auto space = execspace_e::cuda; - auto zsparticles = get_input("zsparticles"); - auto constraints = get_input("constraints"); + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + + auto zsparticles = get_input("zsparticles"); + auto constraints = get_input("constraints"); + auto dt = get_input2("dt"); + auto ptag = get_param("ptag"); + + auto coffsets = constraints->readMeta("color_offset",zs::wrapt>{}); + int nm_group = coffsets.size(); + + auto& verts = zsparticles->getParticles(); + auto& cquads = constraints->getQuadraturePoints(); + auto category = constraints->category; + + // zs::Vector pv_buffer{verts.get_allocator(),verts.size()}; + // zs::Vector total_ghost_impulse_X{verts.get_allocator(),1}; + // zs::Vector total_ghost_impulse_Y{verts.get_allocator(),1}; + // zs::Vector total_ghost_impulse_Z{verts.get_allocator(),1}; + + for(int g = 0;g != nm_group;++g) { + auto coffset = coffsets.getVal(g); + int group_size = 0; + if(g == nm_group - 1) + group_size = cquads.size() - coffsets.getVal(g); + else + group_size = coffsets.getVal(g + 1) - coffsets.getVal(g); + + // cudaPol(zs::range(verts.size()),[ + // ptag = zs::SmallString(ptag), + // verts = proxy({},verts), + // pv_buffer = proxy(pv_buffer)] ZS_LAMBDA(int vi) mutable { + // pv_buffer[vi] = verts.pack(dim_c<3>,ptag,vi); + // }); + + cudaPol(zs::range(group_size),[ + coffset, + verts = proxy({},verts), + category, + dt, + ptag = zs::SmallString(ptag), + cquads = proxy({},cquads)] ZS_LAMBDA(int gi) mutable { + float s = cquads("stiffness",coffset + gi); + float lambda = cquads("lambda",coffset + gi); + + if(category == ZenoParticles::curve) { + auto edge = cquads.pack(dim_c<2>,"inds",coffset + gi,int_c); + vec3 p0{},p1{}; + p0 = verts.pack(dim_c<3>,ptag,edge[0]); + p1 = verts.pack(dim_c<3>,ptag,edge[1]); + float minv0 = verts("minv",edge[0]); + float minv1 = verts("minv",edge[1]); + float r = cquads("r",coffset + gi); + + vec3 dp0{},dp1{}; + CONSTRAINT::solve_DistanceConstraint( + p0,minv0, + p1,minv1, + r, + s, + dt, + lambda, + dp0,dp1); + + + verts.tuple(dim_c<3>,ptag,edge[0]) = p0 + dp0; + verts.tuple(dim_c<3>,ptag,edge[1]) = p1 + dp1; + + // float m0 = verts("m",edge[0]); + // float m1 = verts("m",edge[1]); + // auto ghost_impulse = (dp0 * m0 + dp1 * m1).norm(); + // if(ghost_impulse > 1e-6) + // printf("dmomentum : %f\n",(float)ghost_impulse); + } + if(category == ZenoParticles::tri_bending_spring) { + auto quad = cquads.pack(dim_c<4>,"inds",coffset + gi,int_c); + vec3 p[4] = {}; + float minv[4] = {}; + for(int i = 0;i != 4;++i) { + p[i] = verts.pack(dim_c<3>,ptag,quad[i]); + minv[i] = verts("minv",quad[i]); + } + + auto Q = cquads.pack(dim_c<4,4>,"Q",coffset + gi); + + vec3 dp[4] = {}; + CONSTRAINT::solve_IsometricBendingConstraint( + p[0],minv[0], + p[1],minv[1], + p[2],minv[2], + p[3],minv[3], + Q, + s, + dt, + lambda, + dp[0],dp[1],dp[2],dp[3]); + + for(int i = 0;i != 4;++i) + verts.tuple(dim_c<3>,ptag,quad[i]) = p[i] + dp[i]; + } + cquads("lambda",coffset + gi) = lambda; + }); + + // total_ghost_impulse_X.setVal(0); + // total_ghost_impulse_Y.setVal(0); + // total_ghost_impulse_Z.setVal(0); + // cudaPol(zs::range(verts.size()),[ + // verts = proxy({},verts), + // ptag = zs::SmallString(ptag), + // exec_tag, + // total_ghost_impulse_X = proxy(total_ghost_impulse_X), + // total_ghost_impulse_Y = proxy(total_ghost_impulse_Y), + // total_ghost_impulse_Z = proxy(total_ghost_impulse_Z), + // pv_buffer = proxy(pv_buffer)] ZS_LAMBDA(int vi) mutable { + // auto cv = verts.pack(dim_c<3>,ptag,vi); + // auto pv = pv_buffer[vi]; + // auto m = verts("m",vi); + // auto dv = m * (cv - pv); + // // for(int i = 0;i != 3;++i) + // atomic_add(exec_tag,&total_ghost_impulse_X[0],dv[0]); + // atomic_add(exec_tag,&total_ghost_impulse_Y[0],dv[1]); + // atomic_add(exec_tag,&total_ghost_impulse_Z[0],dv[2]); + // }); + + // auto tgi = total_ghost_impulse.getVal(0); + // auto tgx = total_ghost_impulse_X.getVal(0); + // auto tgy = total_ghost_impulse_Y.getVal(0); + // auto tgz = total_ghost_impulse_Z.getVal(0); + // printf("ghost_impulse[%d][%d] : %f %f %f\n",(int)coffset,(int)group_size,(float)tgx,(float)tgy,(float)tgz); + } set_output("constraints",constraints); set_output("zsparticles",zsparticles); }; }; -ZENDEFNODE(XPBDSolve, {{{"zsparticles"},{"constraints"}}, +ZENDEFNODE(XPBDSolveSmooth, {{{"zsparticles"},{"constraints"},{"float","dt","0.5"}}, {{"zsparticles"},{"constraints"}}, - {}, + {{"string","ptag","X"}}, {"PBD"}}); }; diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh index 063c9141ea..5b73a871fe 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh @@ -1,5 +1,7 @@ #pragma once +#include "../../geometry/kernel/geo_math.hpp" + namespace zeno { namespace CONSTRAINT { // FOR CLOTH SIMULATION template @@ -20,8 +22,8 @@ namespace zeno { namespace CONSTRAINT { n /= d; else { - corr0.setZero(); - corr1.setZero(); + corr0 = VECTOR3d::uniform(0); + corr1 = VECTOR3d::uniform(0);; return true; } @@ -33,12 +35,12 @@ namespace zeno { namespace CONSTRAINT { } SCALER Kinv = 0.0; - if (fabs(K) > static_cast(1e-6)) + if (zs::abs(K) > static_cast(1e-6)) Kinv = static_cast(1.0) / K; else { - corr0.setZero(); - corr1.setZero(); + corr0 = VECTOR3d::uniform(0);; + corr1 = VECTOR3d::uniform(0);; return true; } @@ -53,7 +55,7 @@ namespace zeno { namespace CONSTRAINT { // ---------------------------------------------------------------------------------------------- template - constexpr bool XPBD::solve_VolumeConstraint( + constexpr bool solve_VolumeConstraint( const VECTOR3d& p0, SCALER invMass0, const VECTOR3d& p1, SCALER invMass1, const VECTOR3d& p2, SCALER invMass2, @@ -64,9 +66,10 @@ namespace zeno { namespace CONSTRAINT { SCALER& lambda, VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3) { + constexpr SCALER eps = (SCALER)1e-6; SCALER volume = static_cast(1.0 / 6.0) * (p1 - p0).cross(p2 - p0).dot(p3 - p0); - corr0.setZero(); corr1.setZero(); corr2.setZero(); corr3.setZero(); + corr0 = VECTOR3d::uniform(0);; corr1 = VECTOR3d::uniform(0);; corr2 = VECTOR3d::uniform(0);; corr3 = VECTOR3d::uniform(0);; VECTOR3d grad0 = (p1 - p2).cross(p3 - p2); VECTOR3d grad1 = (p2 - p0).cross(p3 - p0); @@ -74,10 +77,10 @@ namespace zeno { namespace CONSTRAINT { VECTOR3d grad3 = (p1 - p0).cross(p2 - p0); SCALER K = - invMass0 * grad0.squaredNorm() + - invMass1 * grad1.squaredNorm() + - invMass2 * grad2.squaredNorm() + - invMass3 * grad3.squaredNorm(); + invMass0 * grad0.l2NormSqr() + + invMass1 * grad1.l2NormSqr() + + invMass2 * grad2.l2NormSqr() + + invMass3 * grad3.l2NormSqr(); SCALER alpha = 0.0; if (stiffness != 0.0) @@ -86,7 +89,7 @@ namespace zeno { namespace CONSTRAINT { K += alpha; } - if (fabs(K) < eps) + if (zs::abs(K) < eps) return false; const SCALER C = volume - restVolume; @@ -119,10 +122,10 @@ namespace zeno { namespace CONSTRAINT { const VECTOR3d e3 = *x[2] - *x[1]; const VECTOR3d e4 = *x[3] - *x[1]; - const SCALER c01 = MathFunctions::cotTheta(e0, e1); - const SCALER c02 = MathFunctions::cotTheta(e0, e2); - const SCALER c03 = MathFunctions::cotTheta(-e0, e3); - const SCALER c04 = MathFunctions::cotTheta(-e0, e4); + const SCALER c01 = LSL_GEO::cotTheta(e0, e1); + const SCALER c02 = LSL_GEO::cotTheta(e0, e2); + const SCALER c03 = LSL_GEO::cotTheta(-e0, e3); + const SCALER c04 = LSL_GEO::cotTheta(-e0, e4); const SCALER A0 = static_cast(0.5) * (e0.cross(e1)).norm(); const SCALER A1 = static_cast(0.5) * (e0.cross(e2)).norm(); @@ -156,6 +159,7 @@ namespace zeno { namespace CONSTRAINT { SCALER& lambda, VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3) { + constexpr SCALER eps = (SCALER)1e-6; const VECTOR3d* x[4] = { &p2, &p3, &p0, &p1 }; SCALER invMass[4] = { invMass2, invMass3, invMass0, invMass1 }; @@ -165,11 +169,11 @@ namespace zeno { namespace CONSTRAINT { energy += Q(j, k) * (x[k]->dot(*x[j])); energy *= 0.5; - VECTOR3d gradC[4]; - gradC[0].setZero(); - gradC[1].setZero(); - gradC[2].setZero(); - gradC[3].setZero(); + VECTOR3d gradC[4] = {}; + gradC[0] = VECTOR3d::uniform(0);; + gradC[1] = VECTOR3d::uniform(0);; + gradC[2] = VECTOR3d::uniform(0);; + gradC[3] = VECTOR3d::uniform(0);; for (unsigned char k = 0; k < 4; k++) for (unsigned char j = 0; j < 4; j++) gradC[j] += Q(j, k) * *x[k]; @@ -180,7 +184,7 @@ namespace zeno { namespace CONSTRAINT { { // compute sum of squared gradient norms if (invMass[j] != 0.0) - sum_normGradC += invMass[j] * gradC[j].squaredNorm(); + sum_normGradC += invMass[j] * gradC[j].l2NormSqr(); } SCALER alpha = 0.0; @@ -191,7 +195,7 @@ namespace zeno { namespace CONSTRAINT { } // exit early if required - if (fabs(sum_normGradC) > eps) + if (zs::abs(sum_normGradC) > eps) { // compute impulse-based scaling factor const SCALER delta_lambda = -(energy + alpha * lambda) / sum_normGradC; @@ -322,10 +326,10 @@ namespace zeno { namespace CONSTRAINT { VECTOR3d d0 = p1 - p0; VECTOR3d d1 = p3 - p2; - SCALER a = d0.squaredNorm(); + SCALER a = d0.l2NormSqr(); SCALER b = -d0.dot(d1); SCALER c = d0.dot(d1); - SCALER d = -d1.squaredNorm(); + SCALER d = -d1.l2NormSqr(); SCALER e = (p2 - p0).dot(d0); SCALER f = (p2 - p0).dot(d1); SCALER det = a*d - b*c; @@ -403,456 +407,456 @@ namespace zeno { namespace CONSTRAINT { // FOR ELASTIC RODS SIMULATION // ---------------------------------------------------------------------------------------------- - template - constexpr bool PositionBasedCosseratRods::solve_StretchShearConstraint( - const VECTOR3d& p0, SCALER invMass0, - const VECTOR3d& p1, SCALER invMass1, - const QUATERNION& q0, SCALER invMassq0, - const VECTOR3d& stretchingAndShearingKs, - const SCALER restLength, - VECTOR3d& corr0, VECTOR3d& corr1, QUATERNION& corrq0) - { - VECTOR3d d3; //third director d3 = q0 * e_3 * q0_conjugate - d3[0] = static_cast(2.0) * (q0.x() * q0.z() + q0.w() * q0.y()); - d3[1] = static_cast(2.0) * (q0.y() * q0.z() - q0.w() * q0.x()); - d3[2] = q0.w() * q0.w() - q0.x() * q0.x() - q0.y() * q0.y() + q0.z() * q0.z(); - - VECTOR3d gamma = (p1 - p0) / restLength - d3; - gamma /= (invMass1 + invMass0) / restLength + invMassq0 * static_cast(4.0)*restLength + eps; - - if (std::abs(stretchingAndShearingKs[0] - stretchingAndShearingKs[1]) < eps && std::abs(stretchingAndShearingKs[0] - stretchingAndShearingKs[2]) < eps) //all Ks are approx. equal - for (int i = 0; i<3; i++) gamma[i] *= stretchingAndShearingKs[i]; - else //diffenent stretching and shearing Ks. Transform diag(Ks[0], Ks[1], Ks[2]) into world space using Ks_w = R(q0) * diag(Ks[0], Ks[1], Ks[2]) * R^T(q0) and multiply it with gamma - { - MATRIX3d R = q0.toRotationMatrix(); - gamma = (R.transpose() * gamma).eval(); - for (int i = 0; i<3; i++) gamma[i] *= stretchingAndShearingKs[i]; - gamma = (R * gamma).eval(); - } - - corr0 = invMass0 * gamma; - corr1 = -invMass1 * gamma; - - QUATERNION q_e_3_bar(q0.z(), -q0.y(), q0.x(), -q0.w()); //compute q*e_3.conjugate (cheaper than quaternion product) - corrq0 = QUATERNION(0.0, gamma.x(), gamma.y(), gamma.z()) * q_e_3_bar; - corrq0.coeffs() *= static_cast(2.0) * invMassq0 * restLength; - - return true; - } - -// ---------------------------------------------------------------------------------------------- - template - constexpr bool PositionBasedCosseratRods::solve_BendTwistConstraint( - const QUATERNION& q0, SCALER invMassq0, - const QUATERNION& q1, SCALER invMassq1, - const VECTOR3d& bendingAndTwistingKs, - const QUATERNION& restDarbouxVector, - QUATERNION& corrq0, QUATERNION& corrq1) - { - QUATERNION omega = q0.conjugate() * q1; //darboux vector - - QUATERNION omega_plus; - omega_plus.coeffs() = omega.coeffs() + restDarbouxVector.coeffs(); //delta Omega with -Omega_0 - omega.coeffs() = omega.coeffs() - restDarbouxVector.coeffs(); //delta Omega with + omega_0 - if (omega.squaredNorm() > omega_plus.squaredNorm()) omega = omega_plus; - - for (int i = 0; i < 3; i++) omega.coeffs()[i] *= bendingAndTwistingKs[i] / (invMassq0 + invMassq1 + static_cast(1.0e-6)); - omega.w() = 0.0; //discrete Darboux vector does not have vanishing scalar part - - corrq0 = q1 * omega; - corrq1 = q0 * omega; - corrq0.coeffs() *= invMassq0; - corrq1.coeffs() *= -invMassq1; - return true; - } - - // ---------------------------------------------------------------------------------------------- - template - constexpr bool solve_PerpendiculaBisectorConstraint( - const VECTOR3d &p0, SCALER invMass0, - const VECTOR3d &p1, SCALER invMass1, - const VECTOR3d &p2, SCALER invMass2, - const SCALER stiffness, - VECTOR3d &corr0, VECTOR3d &corr1, VECTOR3d &corr2) - { - const VECTOR3d pm = 0.5 * (p0 + p1); - const VECTOR3d p0p2 = p0 - p2; - const VECTOR3d p2p1 = p2 - p1; - const VECTOR3d p1p0 = p1 - p0; - const VECTOR3d p2pm = p2 - pm; - - SCALER wSum = invMass0 * p0p2.squaredNorm() + invMass1 * p2p1.squaredNorm() + invMass2 * p1p0.squaredNorm(); - if (wSum < eps) - return false; - - const SCALER lambda = stiffness * p2pm.dot(p1p0) / wSum; - - corr0 = -invMass0 * lambda * p0p2; - corr1 = -invMass1 * lambda * p2p1; - corr2 = -invMass2 * lambda * p1p0; - - return true; - } - - // ---------------------------------------------------------------------------------------------- - template - constexpr bool solve_GhostPointEdgeDistanceConstraint( - const VECTOR3d& p0, SCALER invMass0, - const VECTOR3d& p1, SCALER invMass1, - const VECTOR3d& p2, SCALER invMass2, - const SCALER stiffness, - const SCALER ghostEdgeRestLength, - VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2) - { - // Ghost-Edge constraint - VECTOR3d pm = 0.5 * (p0 + p1); - VECTOR3d p2pm = p2 - pm; - SCALER wSum = static_cast(0.25) * invMass0 + static_cast(0.25) * invMass1 + static_cast(1.0) * invMass2; - - if (wSum < eps) - return false; - - SCALER p2pm_mag = p2pm.norm(); - p2pm *= static_cast(1.0) / p2pm_mag; - - const SCALER lambda = stiffness * (p2pm_mag - ghostEdgeRestLength) / wSum; - - corr0 = 0.5 * invMass0 * lambda * p2pm; - corr1 = 0.5 * invMass1 * lambda * p2pm; - corr2 = -1.0 * invMass2 * lambda * p2pm; - - return true; - } - - // ---------------------------------------------------------------------------------------------- - template - constexpr bool solve_DarbouxVectorConstraint( - const VECTOR3d& p0, SCALER invMass0, - const VECTOR3d& p1, SCALER invMass1, - const VECTOR3d& p2, SCALER invMass2, - const VECTOR3d& p3, SCALER invMass3, - const VECTOR3d& p4, SCALER invMass4, - const VECTOR3d& bendingAndTwistingKs, - const SCALER midEdgeLength, - const VECTOR3d& restDarbouxVector, - VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3, VECTOR3d& corr4) - { - // Single rod element: - // 3 4 //ghost points - // | | - // --0---1---2-- // rod points - - VECTOR3d darboux_vector; - MATRIX3d d0, d1; - - computeMaterialFrame(p0, p1, p3, d0); - computeMaterialFrame(p1, p2, p4, d1); - - computeDarbouxVector(d0, d1, midEdgeLength, darboux_vector); - - MATRIX3d dajpi[3][3]; - computeMaterialFrameDerivative(p0, p1, p3, d0, - dajpi[0][0], dajpi[0][1], dajpi[0][2], - dajpi[1][0], dajpi[1][1], dajpi[1][2], - dajpi[2][0], dajpi[2][1], dajpi[2][2]); - - MATRIX3d dbjpi[3][3]; - computeMaterialFrameDerivative(p1, p2, p4, d1, - dbjpi[0][0], dbjpi[0][1], dbjpi[0][2], - dbjpi[1][0], dbjpi[1][1], dbjpi[1][2], - dbjpi[2][0], dbjpi[2][1], dbjpi[2][2]); - - MATRIX3d constraint_jacobian[5]; - computeDarbouxGradient( - darboux_vector, midEdgeLength, d0, d1, - dajpi, dbjpi, - //bendingAndTwistingKs, - constraint_jacobian[0], - constraint_jacobian[1], - constraint_jacobian[2], - constraint_jacobian[3], - constraint_jacobian[4]); - - const VECTOR3d constraint_value(bendingAndTwistingKs[0] * (darboux_vector[0] - restDarbouxVector[0]), - bendingAndTwistingKs[1] * (darboux_vector[1] - restDarbouxVector[1]), - bendingAndTwistingKs[2] * (darboux_vector[2] - restDarbouxVector[2])); - - MATRIX3d factor_matrix; - factor_matrix.setZero(); - - MATRIX3d tmp_mat; - SCALER invMasses[]{ invMass0, invMass1, invMass2, invMass3, invMass4 }; - for (int i = 0; i < 5; ++i) - { - tmp_mat = constraint_jacobian[i].transpose() * constraint_jacobian[i]; - tmp_mat.col(0) *= invMasses[i]; - tmp_mat.col(1) *= invMasses[i]; - tmp_mat.col(2) *= invMasses[i]; - - factor_matrix += tmp_mat; - } - - VECTOR3d dp[5]; - tmp_mat = factor_matrix.inverse(); - - for (int i = 0; i < 5; ++i) - { - constraint_jacobian[i].col(0) *= invMasses[i]; - constraint_jacobian[i].col(1) *= invMasses[i]; - constraint_jacobian[i].col(2) *= invMasses[i]; - dp[i] = -(constraint_jacobian[i]) * (tmp_mat * constraint_value); - } - - corr0 = dp[0]; - corr1 = dp[1]; - corr2 = dp[2]; - corr3 = dp[3]; - corr4 = dp[4]; - - return true; - } - - // ---------------------------------------------------------------------------------------------- - template - constexpr bool computeMaterialFrame( - const VECTOR3d& p0, - const VECTOR3d& p1, - const VECTOR3d& p2, - MATRIX3d& frame) - { - frame.col(2) = (p1 - p0); - frame.col(2).normalize(); - - frame.col(1) = (frame.col(2).cross(p2 - p0)); - frame.col(1).normalize(); - - frame.col(0) = frame.col(1).cross(frame.col(2)); - return true; - } - - // ---------------------------------------------------------------------------------------------- - template - constexpr bool computeDarbouxVector(const MATRIX3d& dA, const MATRIX3d& dB, const SCALER mid_edge_length, VECTOR3d& darboux_vector) - { - SCALER factor = static_cast(1.0) + dA.col(0).dot(dB.col(0)) + dA.col(1).dot(dB.col(1)) + dA.col(2).dot(dB.col(2)); - - factor = static_cast(2.0) / (mid_edge_length * factor); - - for (int c = 0; c < 3; ++c) - { - const int i = permutation[c][0]; - const int j = permutation[c][1]; - const int k = permutation[c][2]; - darboux_vector[i] = dA.col(j).dot(dB.col(k)) - dA.col(k).dot(dB.col(j)); - } - darboux_vector *= factor; - return true; - } - - // ---------------------------------------------------------------------------------------------- - template - constexpr bool computeMaterialFrameDerivative( - const VECTOR3d& p0, const VECTOR3d& p1, const VECTOR3d& p2, const MATRIX3d& d, - MATRIX3d& d1p0, MATRIX3d& d1p1, MATRIX3d& d1p2, - MATRIX3d& d2p0, MATRIX3d& d2p1, MATRIX3d& d2p2, - MATRIX3d& d3p0, MATRIX3d& d3p1, MATRIX3d& d3p2) - { - ////////////////////////////////////////////////////////////////////////// - // d3pi - ////////////////////////////////////////////////////////////////////////// - const VECTOR3d p01 = p1 - p0; - SCALER length_p01 = p01.norm(); - - d3p0.col(0) = d.col(2)[0] * d.col(2); - d3p0.col(1) = d.col(2)[1] * d.col(2); - d3p0.col(2) = d.col(2)[2] * d.col(2); - - d3p0.col(0)[0] -= 1.0; - d3p0.col(1)[1] -= 1.0; - d3p0.col(2)[2] -= 1.0; - - d3p0.col(0) *= (static_cast(1.0) / length_p01); - d3p0.col(1) *= (static_cast(1.0) / length_p01); - d3p0.col(2) *= (static_cast(1.0) / length_p01); - - d3p1.col(0) = -d3p0.col(0); - d3p1.col(1) = -d3p0.col(1); - d3p1.col(2) = -d3p0.col(2); - - d3p2.col(0).setZero(); - d3p2.col(1).setZero(); - d3p2.col(2).setZero(); - - ////////////////////////////////////////////////////////////////////////// - // d2pi - ////////////////////////////////////////////////////////////////////////// - const VECTOR3d p02 = p2 - p0; - const VECTOR3d p01_cross_p02 = p01.cross(p02); - - const SCALER length_cross = p01_cross_p02.norm(); - - MATRIX3d mat; - mat.col(0) = d.col(1)[0] * d.col(1); - mat.col(1) = d.col(1)[1] * d.col(1); - mat.col(2) = d.col(1)[2] * d.col(1); - - mat.col(0)[0] -= 1.0; - mat.col(1)[1] -= 1.0; - mat.col(2)[2] -= 1.0; - - mat.col(0) *= (-static_cast(1.0) / length_cross); - mat.col(1) *= (-static_cast(1.0) / length_cross); - mat.col(2) *= (-static_cast(1.0) / length_cross); - - MATRIX3d product_matrix; - MathFunctions::crossProductMatrix(p2 - p1, product_matrix); - d2p0 = mat * product_matrix; - - MathFunctions::crossProductMatrix(p0 - p2, product_matrix); - d2p1 = mat * product_matrix; - - MathFunctions::crossProductMatrix(p1 - p0, product_matrix); - d2p2 = mat * product_matrix; - - ////////////////////////////////////////////////////////////////////////// - // d1pi - ////////////////////////////////////////////////////////////////////////// - MATRIX3d product_mat_d3; - MATRIX3d product_mat_d2; - MathFunctions::crossProductMatrix(d.col(2), product_mat_d3); - MathFunctions::crossProductMatrix(d.col(1), product_mat_d2); - - d1p0 = product_mat_d2 * d3p0 - product_mat_d3 * d2p0; - d1p1 = product_mat_d2 * d3p1 - product_mat_d3 * d2p1; - d1p2 = -product_mat_d3 * d2p2; - return true; - } - - // ---------------------------------------------------------------------------------------------- - template - constexpr bool computeDarbouxGradient( - const VECTOR3d& darboux_vector, const SCALER length, - const MATRIX3d& da, const MATRIX3d& db, - const MATRIX3d dajpi[3][3], const MATRIX3d dbjpi[3][3], - //const VECTOR3d& bendAndTwistKs, - MATRIX3d& omega_pa, MATRIX3d& omega_pb, MATRIX3d& omega_pc, MATRIX3d& omega_pd, MATRIX3d& omega_pe - ) - { - SCALER X = static_cast(1.0) + da.col(0).dot(db.col(0)) + da.col(1).dot(db.col(1)) + da.col(2).dot(db.col(2)); - X = static_cast(2.0) / (length * X); - - for (int c = 0; c < 3; ++c) - { - const int i = permutation[c][0]; - const int j = permutation[c][1]; - const int k = permutation[c][2]; - // pa - { - VECTOR3d term1(0,0,0); - VECTOR3d term2(0,0,0); - VECTOR3d tmp(0,0,0); - - // first term - term1 = dajpi[j][0].transpose() * db.col(k); - tmp = dajpi[k][0].transpose() * db.col(j); - term1 = term1 - tmp; - // second term - for (int n = 0; n < 3; ++n) - { - tmp = dajpi[n][0].transpose() * db.col(n); - term2 = term2 + tmp; - } - omega_pa.col(i) = X * (term1-(0.5 * darboux_vector[i] * length) * term2); - //omega_pa.col(i) *= bendAndTwistKs[i]; - } - // pb - { - VECTOR3d term1(0, 0, 0); - VECTOR3d term2(0, 0, 0); - VECTOR3d tmp(0, 0, 0); - // first term - term1 = dajpi[j][1].transpose() * db.col(k); - tmp = dajpi[k][1].transpose() * db.col(j); - term1 = term1 - tmp; - // third term - tmp = dbjpi[j][0].transpose() * da.col(k); - term1 = term1 - tmp; +// template +// constexpr bool solve_StretchShearConstraint( +// const VECTOR3d& p0, SCALER invMass0, +// const VECTOR3d& p1, SCALER invMass1, +// const QUATERNION& q0, SCALER invMassq0, +// const VECTOR3d& stretchingAndShearingKs, +// const SCALER restLength, +// VECTOR3d& corr0, VECTOR3d& corr1, QUATERNION& corrq0) +// { +// VECTOR3d d3; //third director d3 = q0 * e_3 * q0_conjugate +// d3[0] = static_cast(2.0) * (q0.x() * q0.z() + q0.w() * q0.y()); +// d3[1] = static_cast(2.0) * (q0.y() * q0.z() - q0.w() * q0.x()); +// d3[2] = q0.w() * q0.w() - q0.x() * q0.x() - q0.y() * q0.y() + q0.z() * q0.z(); + +// VECTOR3d gamma = (p1 - p0) / restLength - d3; +// gamma /= (invMass1 + invMass0) / restLength + invMassq0 * static_cast(4.0)*restLength + eps; + +// if (std::abs(stretchingAndShearingKs[0] - stretchingAndShearingKs[1]) < eps && std::abs(stretchingAndShearingKs[0] - stretchingAndShearingKs[2]) < eps) //all Ks are approx. equal +// for (int i = 0; i<3; i++) gamma[i] *= stretchingAndShearingKs[i]; +// else //diffenent stretching and shearing Ks. Transform diag(Ks[0], Ks[1], Ks[2]) into world space using Ks_w = R(q0) * diag(Ks[0], Ks[1], Ks[2]) * R^T(q0) and multiply it with gamma +// { +// MATRIX3d R = q0.toRotationMatrix(); +// gamma = (R.transpose() * gamma).eval(); +// for (int i = 0; i<3; i++) gamma[i] *= stretchingAndShearingKs[i]; +// gamma = (R * gamma).eval(); +// } + +// corr0 = invMass0 * gamma; +// corr1 = -invMass1 * gamma; + +// QUATERNION q_e_3_bar(q0.z(), -q0.y(), q0.x(), -q0.w()); //compute q*e_3.conjugate (cheaper than quaternion product) +// corrq0 = QUATERNION(0.0, gamma.x(), gamma.y(), gamma.z()) * q_e_3_bar; +// corrq0.coeffs() *= static_cast(2.0) * invMassq0 * restLength; + +// return true; +// } + +// // ---------------------------------------------------------------------------------------------- +// template +// constexpr bool solve_BendTwistConstraint( +// const QUATERNION& q0, SCALER invMassq0, +// const QUATERNION& q1, SCALER invMassq1, +// const VECTOR3d& bendingAndTwistingKs, +// const QUATERNION& restDarbouxVector, +// QUATERNION& corrq0, QUATERNION& corrq1) +// { +// QUATERNION omega = q0.conjugate() * q1; //darboux vector + +// QUATERNION omega_plus; +// omega_plus.coeffs() = omega.coeffs() + restDarbouxVector.coeffs(); //delta Omega with -Omega_0 +// omega.coeffs() = omega.coeffs() - restDarbouxVector.coeffs(); //delta Omega with + omega_0 +// if (omega.l2NormSqr() > omega_plus.l2NormSqr()) omega = omega_plus; + +// for (int i = 0; i < 3; i++) omega.coeffs()[i] *= bendingAndTwistingKs[i] / (invMassq0 + invMassq1 + static_cast(1.0e-6)); +// omega.w() = 0.0; //discrete Darboux vector does not have vanishing scalar part + +// corrq0 = q1 * omega; +// corrq1 = q0 * omega; +// corrq0.coeffs() *= invMassq0; +// corrq1.coeffs() *= -invMassq1; +// return true; +// } + +// // ---------------------------------------------------------------------------------------------- +// template +// constexpr bool solve_PerpendiculaBisectorConstraint( +// const VECTOR3d &p0, SCALER invMass0, +// const VECTOR3d &p1, SCALER invMass1, +// const VECTOR3d &p2, SCALER invMass2, +// const SCALER stiffness, +// VECTOR3d &corr0, VECTOR3d &corr1, VECTOR3d &corr2) +// { +// const VECTOR3d pm = 0.5 * (p0 + p1); +// const VECTOR3d p0p2 = p0 - p2; +// const VECTOR3d p2p1 = p2 - p1; +// const VECTOR3d p1p0 = p1 - p0; +// const VECTOR3d p2pm = p2 - pm; + +// SCALER wSum = invMass0 * p0p2.l2NormSqr() + invMass1 * p2p1.l2NormSqr() + invMass2 * p1p0.l2NormSqr(); +// if (wSum < eps) +// return false; + +// const SCALER lambda = stiffness * p2pm.dot(p1p0) / wSum; + +// corr0 = -invMass0 * lambda * p0p2; +// corr1 = -invMass1 * lambda * p2p1; +// corr2 = -invMass2 * lambda * p1p0; + +// return true; +// } + +// // ---------------------------------------------------------------------------------------------- +// template +// constexpr bool solve_GhostPointEdgeDistanceConstraint( +// const VECTOR3d& p0, SCALER invMass0, +// const VECTOR3d& p1, SCALER invMass1, +// const VECTOR3d& p2, SCALER invMass2, +// const SCALER stiffness, +// const SCALER ghostEdgeRestLength, +// VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2) +// { +// // Ghost-Edge constraint +// VECTOR3d pm = 0.5 * (p0 + p1); +// VECTOR3d p2pm = p2 - pm; +// SCALER wSum = static_cast(0.25) * invMass0 + static_cast(0.25) * invMass1 + static_cast(1.0) * invMass2; + +// if (wSum < eps) +// return false; + +// SCALER p2pm_mag = p2pm.norm(); +// p2pm *= static_cast(1.0) / p2pm_mag; + +// const SCALER lambda = stiffness * (p2pm_mag - ghostEdgeRestLength) / wSum; + +// corr0 = 0.5 * invMass0 * lambda * p2pm; +// corr1 = 0.5 * invMass1 * lambda * p2pm; +// corr2 = -1.0 * invMass2 * lambda * p2pm; + +// return true; +// } + +// // ---------------------------------------------------------------------------------------------- +// template +// constexpr bool solve_DarbouxVectorConstraint( +// const VECTOR3d& p0, SCALER invMass0, +// const VECTOR3d& p1, SCALER invMass1, +// const VECTOR3d& p2, SCALER invMass2, +// const VECTOR3d& p3, SCALER invMass3, +// const VECTOR3d& p4, SCALER invMass4, +// const VECTOR3d& bendingAndTwistingKs, +// const SCALER midEdgeLength, +// const VECTOR3d& restDarbouxVector, +// VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3, VECTOR3d& corr4) +// { +// // Single rod element: +// // 3 4 //ghost points +// // | | +// // --0---1---2-- // rod points + +// VECTOR3d darboux_vector; +// MATRIX3d d0, d1; + +// computeMaterialFrame(p0, p1, p3, d0); +// computeMaterialFrame(p1, p2, p4, d1); + +// computeDarbouxVector(d0, d1, midEdgeLength, darboux_vector); + +// MATRIX3d dajpi[3][3]; +// computeMaterialFrameDerivative(p0, p1, p3, d0, +// dajpi[0][0], dajpi[0][1], dajpi[0][2], +// dajpi[1][0], dajpi[1][1], dajpi[1][2], +// dajpi[2][0], dajpi[2][1], dajpi[2][2]); + +// MATRIX3d dbjpi[3][3]; +// computeMaterialFrameDerivative(p1, p2, p4, d1, +// dbjpi[0][0], dbjpi[0][1], dbjpi[0][2], +// dbjpi[1][0], dbjpi[1][1], dbjpi[1][2], +// dbjpi[2][0], dbjpi[2][1], dbjpi[2][2]); + +// MATRIX3d constraint_jacobian[5]; +// computeDarbouxGradient( +// darboux_vector, midEdgeLength, d0, d1, +// dajpi, dbjpi, +// //bendingAndTwistingKs, +// constraint_jacobian[0], +// constraint_jacobian[1], +// constraint_jacobian[2], +// constraint_jacobian[3], +// constraint_jacobian[4]); + +// const VECTOR3d constraint_value(bendingAndTwistingKs[0] * (darboux_vector[0] - restDarbouxVector[0]), +// bendingAndTwistingKs[1] * (darboux_vector[1] - restDarbouxVector[1]), +// bendingAndTwistingKs[2] * (darboux_vector[2] - restDarbouxVector[2])); + +// MATRIX3d factor_matrix; +// factor_matrix = VECTOR3d::uniform(0);; + +// MATRIX3d tmp_mat; +// SCALER invMasses[]{ invMass0, invMass1, invMass2, invMass3, invMass4 }; +// for (int i = 0; i < 5; ++i) +// { +// tmp_mat = constraint_jacobian[i].transpose() * constraint_jacobian[i]; +// tmp_mat.col(0) *= invMasses[i]; +// tmp_mat.col(1) *= invMasses[i]; +// tmp_mat.col(2) *= invMasses[i]; + +// factor_matrix += tmp_mat; +// } + +// VECTOR3d dp[5]; +// tmp_mat = factor_matrix.inverse(); + +// for (int i = 0; i < 5; ++i) +// { +// constraint_jacobian[i].col(0) *= invMasses[i]; +// constraint_jacobian[i].col(1) *= invMasses[i]; +// constraint_jacobian[i].col(2) *= invMasses[i]; +// dp[i] = -(constraint_jacobian[i]) * (tmp_mat * constraint_value); +// } + +// corr0 = dp[0]; +// corr1 = dp[1]; +// corr2 = dp[2]; +// corr3 = dp[3]; +// corr4 = dp[4]; + +// return true; +// } + +// // ---------------------------------------------------------------------------------------------- +// template +// constexpr bool computeMaterialFrame( +// const VECTOR3d& p0, +// const VECTOR3d& p1, +// const VECTOR3d& p2, +// MATRIX3d& frame) +// { +// frame.col(2) = (p1 - p0); +// frame.col(2).normalize(); + +// frame.col(1) = (frame.col(2).cross(p2 - p0)); +// frame.col(1).normalize(); + +// frame.col(0) = frame.col(1).cross(frame.col(2)); +// return true; +// } + +// // ---------------------------------------------------------------------------------------------- +// template +// constexpr bool computeDarbouxVector(const MATRIX3d& dA, const MATRIX3d& dB, const SCALER mid_edge_length, VECTOR3d& darboux_vector) +// { +// SCALER factor = static_cast(1.0) + dA.col(0).dot(dB.col(0)) + dA.col(1).dot(dB.col(1)) + dA.col(2).dot(dB.col(2)); + +// factor = static_cast(2.0) / (mid_edge_length * factor); + +// for (int c = 0; c < 3; ++c) +// { +// const int i = permutation[c][0]; +// const int j = permutation[c][1]; +// const int k = permutation[c][2]; +// darboux_vector[i] = dA.col(j).dot(dB.col(k)) - dA.col(k).dot(dB.col(j)); +// } +// darboux_vector *= factor; +// return true; +// } + +// // ---------------------------------------------------------------------------------------------- +// template +// constexpr bool computeMaterialFrameDerivative( +// const VECTOR3d& p0, const VECTOR3d& p1, const VECTOR3d& p2, const MATRIX3d& d, +// MATRIX3d& d1p0, MATRIX3d& d1p1, MATRIX3d& d1p2, +// MATRIX3d& d2p0, MATRIX3d& d2p1, MATRIX3d& d2p2, +// MATRIX3d& d3p0, MATRIX3d& d3p1, MATRIX3d& d3p2) +// { +// ////////////////////////////////////////////////////////////////////////// +// // d3pi +// ////////////////////////////////////////////////////////////////////////// +// const VECTOR3d p01 = p1 - p0; +// SCALER length_p01 = p01.norm(); + +// d3p0.col(0) = d.col(2)[0] * d.col(2); +// d3p0.col(1) = d.col(2)[1] * d.col(2); +// d3p0.col(2) = d.col(2)[2] * d.col(2); + +// d3p0.col(0)[0] -= 1.0; +// d3p0.col(1)[1] -= 1.0; +// d3p0.col(2)[2] -= 1.0; + +// d3p0.col(0) *= (static_cast(1.0) / length_p01); +// d3p0.col(1) *= (static_cast(1.0) / length_p01); +// d3p0.col(2) *= (static_cast(1.0) / length_p01); + +// d3p1.col(0) = -d3p0.col(0); +// d3p1.col(1) = -d3p0.col(1); +// d3p1.col(2) = -d3p0.col(2); + +// d3p2.col(0) = VECTOR3d::uniform(0);; +// d3p2.col(1) = VECTOR3d::uniform(0);; +// d3p2.col(2) = VECTOR3d::uniform(0);; + +// ////////////////////////////////////////////////////////////////////////// +// // d2pi +// ////////////////////////////////////////////////////////////////////////// +// const VECTOR3d p02 = p2 - p0; +// const VECTOR3d p01_cross_p02 = p01.cross(p02); + +// const SCALER length_cross = p01_cross_p02.norm(); + +// MATRIX3d mat; +// mat.col(0) = d.col(1)[0] * d.col(1); +// mat.col(1) = d.col(1)[1] * d.col(1); +// mat.col(2) = d.col(1)[2] * d.col(1); + +// mat.col(0)[0] -= 1.0; +// mat.col(1)[1] -= 1.0; +// mat.col(2)[2] -= 1.0; + +// mat.col(0) *= (-static_cast(1.0) / length_cross); +// mat.col(1) *= (-static_cast(1.0) / length_cross); +// mat.col(2) *= (-static_cast(1.0) / length_cross); + +// MATRIX3d product_matrix; +// LSL_GEO::crossProductMatrix(p2 - p1, product_matrix); +// d2p0 = mat * product_matrix; + +// LSL_GEO::crossProductMatrix(p0 - p2, product_matrix); +// d2p1 = mat * product_matrix; + +// LSL_GEO::crossProductMatrix(p1 - p0, product_matrix); +// d2p2 = mat * product_matrix; + +// ////////////////////////////////////////////////////////////////////////// +// // d1pi +// ////////////////////////////////////////////////////////////////////////// +// MATRIX3d product_mat_d3; +// MATRIX3d product_mat_d2; +// LSL_GEO::crossProductMatrix(d.col(2), product_mat_d3); +// LSL_GEO::crossProductMatrix(d.col(1), product_mat_d2); + +// d1p0 = product_mat_d2 * d3p0 - product_mat_d3 * d2p0; +// d1p1 = product_mat_d2 * d3p1 - product_mat_d3 * d2p1; +// d1p2 = -product_mat_d3 * d2p2; +// return true; +// } + +// // ---------------------------------------------------------------------------------------------- +// template +// constexpr bool computeDarbouxGradient( +// const VECTOR3d& darboux_vector, const SCALER length, +// const MATRIX3d& da, const MATRIX3d& db, +// const MATRIX3d dajpi[3][3], const MATRIX3d dbjpi[3][3], +// //const VECTOR3d& bendAndTwistKs, +// MATRIX3d& omega_pa, MATRIX3d& omega_pb, MATRIX3d& omega_pc, MATRIX3d& omega_pd, MATRIX3d& omega_pe +// ) +// { +// SCALER X = static_cast(1.0) + da.col(0).dot(db.col(0)) + da.col(1).dot(db.col(1)) + da.col(2).dot(db.col(2)); +// X = static_cast(2.0) / (length * X); + +// for (int c = 0; c < 3; ++c) +// { +// const int i = permutation[c][0]; +// const int j = permutation[c][1]; +// const int k = permutation[c][2]; +// // pa +// { +// VECTOR3d term1(0,0,0); +// VECTOR3d term2(0,0,0); +// VECTOR3d tmp(0,0,0); + +// // first term +// term1 = dajpi[j][0].transpose() * db.col(k); +// tmp = dajpi[k][0].transpose() * db.col(j); +// term1 = term1 - tmp; +// // second term +// for (int n = 0; n < 3; ++n) +// { +// tmp = dajpi[n][0].transpose() * db.col(n); +// term2 = term2 + tmp; +// } +// omega_pa.col(i) = X * (term1-(0.5 * darboux_vector[i] * length) * term2); +// //omega_pa.col(i) *= bendAndTwistKs[i]; +// } +// // pb +// { +// VECTOR3d term1(0, 0, 0); +// VECTOR3d term2(0, 0, 0); +// VECTOR3d tmp(0, 0, 0); +// // first term +// term1 = dajpi[j][1].transpose() * db.col(k); +// tmp = dajpi[k][1].transpose() * db.col(j); +// term1 = term1 - tmp; +// // third term +// tmp = dbjpi[j][0].transpose() * da.col(k); +// term1 = term1 - tmp; - tmp = dbjpi[k][0].transpose() * da.col(j); - term1 = term1 + tmp; - - // second term - for (int n = 0; n < 3; ++n) - { - tmp = dajpi[n][1].transpose() * db.col(n); - term2 = term2 + tmp; +// tmp = dbjpi[k][0].transpose() * da.col(j); +// term1 = term1 + tmp; + +// // second term +// for (int n = 0; n < 3; ++n) +// { +// tmp = dajpi[n][1].transpose() * db.col(n); +// term2 = term2 + tmp; - tmp = dbjpi[n][0].transpose() * da.col(n); - term2 = term2 + tmp; - } - omega_pb.col(i) = X * (term1-(0.5 * darboux_vector[i] * length) * term2); - //omega_pb.col(i) *= bendAndTwistKs[i]; - } - // pc - { - VECTOR3d term1(0, 0, 0); - VECTOR3d term2(0, 0, 0); - VECTOR3d tmp(0, 0, 0); +// tmp = dbjpi[n][0].transpose() * da.col(n); +// term2 = term2 + tmp; +// } +// omega_pb.col(i) = X * (term1-(0.5 * darboux_vector[i] * length) * term2); +// //omega_pb.col(i) *= bendAndTwistKs[i]; +// } +// // pc +// { +// VECTOR3d term1(0, 0, 0); +// VECTOR3d term2(0, 0, 0); +// VECTOR3d tmp(0, 0, 0); - // first term - term1 = dbjpi[j][1].transpose() * da.col(k); - tmp = dbjpi[k][1].transpose() * da.col(j); - term1 = term1 - tmp; - - // second term - for (int n = 0; n < 3; ++n) - { - tmp = dbjpi[n][1].transpose() * da.col(n); - term2 = term2 + tmp; - } - omega_pc.col(i) = -X*(term1+(0.5 * darboux_vector[i] * length) * term2); - //omega_pc.col(i) *= bendAndTwistKs[i]; - } - // pd - { - VECTOR3d term1(0, 0, 0); - VECTOR3d term2(0, 0, 0); - VECTOR3d tmp(0, 0, 0); - // first term - term1 = dajpi[j][2].transpose() * db.col(k); - tmp = dajpi[k][2].transpose() * db.col(j); - term1 = term1 - tmp; - // second term - for (int n = 0; n < 3; ++n) { - tmp = dajpi[n][2].transpose() * db.col(n); - term2 = term2 + tmp; - } - omega_pd.col(i) = X*(term1-(0.5 * darboux_vector[i] * length) * term2); - //omega_pd.col(i) *= bendAndTwistKs[i]; - } - // pe - { - VECTOR3d term1(0, 0, 0); - VECTOR3d term2(0, 0, 0); - VECTOR3d tmp(0, 0, 0); - // first term - term1 = dbjpi[j][2].transpose() * da.col(k); - tmp = dbjpi[k][2].transpose() * da.col(j); - term1 -= tmp; +// // first term +// term1 = dbjpi[j][1].transpose() * da.col(k); +// tmp = dbjpi[k][1].transpose() * da.col(j); +// term1 = term1 - tmp; + +// // second term +// for (int n = 0; n < 3; ++n) +// { +// tmp = dbjpi[n][1].transpose() * da.col(n); +// term2 = term2 + tmp; +// } +// omega_pc.col(i) = -X*(term1+(0.5 * darboux_vector[i] * length) * term2); +// //omega_pc.col(i) *= bendAndTwistKs[i]; +// } +// // pd +// { +// VECTOR3d term1(0, 0, 0); +// VECTOR3d term2(0, 0, 0); +// VECTOR3d tmp(0, 0, 0); +// // first term +// term1 = dajpi[j][2].transpose() * db.col(k); +// tmp = dajpi[k][2].transpose() * db.col(j); +// term1 = term1 - tmp; +// // second term +// for (int n = 0; n < 3; ++n) { +// tmp = dajpi[n][2].transpose() * db.col(n); +// term2 = term2 + tmp; +// } +// omega_pd.col(i) = X*(term1-(0.5 * darboux_vector[i] * length) * term2); +// //omega_pd.col(i) *= bendAndTwistKs[i]; +// } +// // pe +// { +// VECTOR3d term1(0, 0, 0); +// VECTOR3d term2(0, 0, 0); +// VECTOR3d tmp(0, 0, 0); +// // first term +// term1 = dbjpi[j][2].transpose() * da.col(k); +// tmp = dbjpi[k][2].transpose() * da.col(j); +// term1 -= tmp; - // second term - for (int n = 0; n < 3; ++n) - { - tmp = dbjpi[n][2].transpose() * da.col(n); - term2 += tmp; - } - - omega_pe.col(i) = -X*(term1+(0.5 * darboux_vector[i] * length) * term2); - //omega_pe.col(i) *= bendAndTwistKs[i]; - } - } - return true; - } +// // second term +// for (int n = 0; n < 3; ++n) +// { +// tmp = dbjpi[n][2].transpose() * da.col(n); +// term2 += tmp; +// } + +// omega_pe.col(i) = -X*(term1+(0.5 * darboux_vector[i] * length) * term2); +// //omega_pe.col(i) *= bendAndTwistKs[i]; +// } +// } +// return true; +// } }; }; From 626afda8db8cca9c33aa98405c8bdd78c21eb787 Mon Sep 17 00:00:00 2001 From: ShuliangLu Date: Fri, 21 Jul 2023 20:59:55 +0800 Subject: [PATCH 06/21] pp collision wip --- .../CuLagrange/geometry/kernel/geo_math.hpp | 35 ++++ projects/CuLagrange/pbd/Pipeline.cu | 184 +++++++++++++++++- 2 files changed, 211 insertions(+), 8 deletions(-) diff --git a/projects/CuLagrange/geometry/kernel/geo_math.hpp b/projects/CuLagrange/geometry/kernel/geo_math.hpp index b3fd989788..a8f89dbeb9 100644 --- a/projects/CuLagrange/geometry/kernel/geo_math.hpp +++ b/projects/CuLagrange/geometry/kernel/geo_math.hpp @@ -838,5 +838,40 @@ constexpr REAL pointTriangleDistance(const VECTOR3& v0, const VECTOR3& v1, } return std::numeric_limits::infinity(); } + + constexpr REAL ray_ray_intersect(VECTOR3 const& x0,VECTOR3 const& v0,VECTOR3 const &x1,VECTOR3 const& v1,REAL thickness) { + auto x = x1 - x0; + auto v = v1 - v0; + + if(x.norm() < thickness) + return (REAL)0; + if(x.dot(v) > 0) + return std::numeric_limits::infinity(); + // if(vv.norm() < 1e) + + auto xx = x.dot(x); + auto vv = v.dot(v); + auto xv = x.dot(v); + + // auto closest_dist = (x - xv / vv * v).norm(); + // if(closest_dist > thickness) + // return std::numeric_limits::infinity(); + + auto delta = 4 * xv * xv - 4 * vv * (xx - thickness * thickness); + if(delta < 0) + return std::numeric_limits::infinity(); + + auto sqrt_delta = zs::sqrt(delta); + auto alpha = xv/vv; + auto beta = sqrt_delta / 2 / vv; + + auto t0 = -alpha + beta; + auto t1 = -alpha - beta; + + t0 = t0 < (REAL)1.0 && t0 > (REAL)0.0 ? t0 : std::numeric_limits::infinity(); + t1 = t1 < (REAL)1.0 && t1 > (REAL)0.0 ? t1 : std::numeric_limits::infinity(); + + return zs::min(t0,t1); + } }; }; \ No newline at end of file diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index a529e35af3..9efa9f5d64 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -293,6 +293,164 @@ ZENDEFNODE(VisualizePBDConstraint, {{{"zsparticles"},{"constraints"}}, }, {"PBD"}}); + +struct KinematicCollisionDetection : INode { + using T = float; + using tiles_t = typename ZenoParticles::particles_t; + using dtiles_t = zs::TileVector; + using bvh_t = zs::LBvh<3, int, T>; + using vec3 = zs::vec; + using vec2i = zs::vec; + // using ICOORD = vec2i; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + template + void build_ray_bvh(zs::CudaExecutionPolicy &pol, TileVecT &verts,ZenoLinearBvh::lbvh_t &bvh) { + using namespace zs; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto space = execspace_e::cuda; + Vector bvs{verts.get_allocator(),verts.size()}; + pol(range(verts.size()),[ + verts = proxy({},verts),bvs = proxy(bvs)] ZS_LAMBDA(int vi) { + auto x = verts.pack(dim_c<3>,"x",vi); + auto px = verts.pack(dim_c<3>,"px",vi); + auto pscale = verts("pscale",vi); + + bv_t bv{x,xp}; + bv._min -= pscale; + bv._max += pscale; + + bvs[vi] = bv; + }); + bvh.build(pol,bvs); + } + + template + void build_particles_bvh(zs::CudaExecutionPolicy& pol,TileVecT &kverts,ZenoLinearBvh::lbvh_t& bvh) { + using namespace zs; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto space = execspace_e::cuda; + Vector bvs{kverts.get_allocator(),kverts.size()}; + pol(range(kverts.size()),[ + kverts = proxy({},kverts),bvs = proxy(bvs)] ZS_LAMBDA(int kvi) { + auto x = kverts.pack(dim_c<3>,"x",vi); + auto pscale = kverts("pscale",vi); + bv_t bv{x - pscale,x + pscale}; + bvs[vi] = bv; + }); + bvh.build(pol,bvs); + } + + + virtual void apply() override { + using namespace zs; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + + auto zsparticles = get_input("zsparticles"); + auto boundary = get_input("kboundary"); + auto sdf = get_input("sdf"); + + auto constraints = std::make_shared(); + + constraint->sprayedOffset = 0; + constraint->elements = typename ZenoParticles::particles_t({{"stiffness",1},{"lambda",1},{"tclr",1}}, 0, zs::memsrc_e::device,0); + auto &eles = constraint->getQuadraturePoints(); + + // auto xtag = get_param("xtag"); + // auto pxtag = get_param("pxtag"); + // auto kxtag = get_param("kxtag"); + // auto pkxtag = get_param("pkxtag"); + + const auto& verts = zsparticles->getParticles(); + const auto& kverts = boundary->getParticles(); + + // use PP collision only + bvh_t kbvh{}; + build_particles_bvh(cudaPol,kverts,kbvh); + + zs::Vector hit_points{verts.get_allocator(),verts.size()}; + zs:Vector hit_distances{verts.get_allocator(),verts.size()}; + pol(range(hit_points),[]ZS_LAMBDA(auto &hp) mutable {hp = -1;}); + pol(range(hit_distance),[]ZS_LAMBDA(auto &dist) mutable { dist = std::numeric_limits::infinity();}); + + zs::bht csPP{verts.get_allocator(),verts.size()}; + csPP.reset(pol,true); + + cudaPol(zs::range(verts.size()),[ + kverts = proxy({},kverts), + verts = proxy({},verts), + hit_points = proxy(hit_points), + csPP = proxy(csPP), + kbvh = proxy(kbvh)] ZS_LAMBDA(int vi) mutable { + auto x = verts.pack(dim_c<3>,"x",vi); + auto px = verts.pack(dim_c<3>,"px",vi); + auto pscale = verts("pscale",vi); + + auto bv = bv_t{get_bounding_box(x,px)}; + bv._min -= pscale; + bv._max += pscale; + + T mint = std::numeric_limits::infinity(); + int min_kvi = -1; + + auto process_ccd_collision = [&](int kvi) mutable { + auto kpscale = kverts("pscale",kvi); + auto kx = kverts.pack(dim_c<3>,"x",kvi); + auto pkx = kx; + if(kverts.hasProperty("px")) + pkx = kverts.pack(dim_c<3>,"px",kvi); + + auto t = LSL_GEO::ray_ray_intersect(px,x - px,pkx,kx - pkx,pscale + kpscale); + if(t < mint) { + mint = t; + min_kvi = kvi; + } + }; + iter_neighbors(bv,process_ccd_collision); + + if(min_kvi >= 0) { + csPP.insert(vec2i{vi,min_kvi}); + } + + hit_points[vi] = min_kvi; + hit_distance[vi] = mint; + }); + + + } +}; + +struct SelfCollisionDetection : INode { + virtual void apply() override { + using namespace zs; + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + + auto zsparticles = get_input("zsparticles"); + + auto constraints = std::make_shared(); + + constraint->sprayedOffset = 0; + constraint->elements = typename ZenoParticles::particles_t({{"stiffness",1},{"lambda",1},{"tclr",1}}, 0, zs::memsrc_e::device,0); + auto &eles = constraint->getQuadraturePoints(); + + + + } +}; + // solve a specific type of constraint for one iterations struct XPBDSolve : INode { @@ -394,8 +552,7 @@ struct XPBDSolve : INode { CONSTRAINT::solve_IsometricBendingConstraint( p[0],minv[0], p[1],minv[1], - p[2],minv[2], - p[3],minv[3], + p[2],minv[2],XPBDCCD Q, s, dt, @@ -440,12 +597,12 @@ struct XPBDSolve : INode { set_output("zsparticles",zsparticles); }; }; - ZENDEFNODE(XPBDSolve, {{{"zsparticles"},{"constraints"},{"float","dt","0.5"}}, {{"zsparticles"},{"constraints"}}, {{"string","ptag","X"}}, {"PBD"}}); + struct XPBDSolveSmooth : INode { virtual void apply() override { @@ -461,22 +618,31 @@ struct XPBDSolveSmooth : INode { constexpr auto exec_tag = wrapv{}; auto zsparticles = get_input("zsparticles"); - auto constraints = get_input("constraints"); + + auto all_constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "constraints"); + // auto constraints = get_input("constraints"); auto dt = get_input2("dt"); auto ptag = get_param("ptag"); - auto coffsets = constraints->readMeta("color_offset",zs::wrapt>{}); - int nm_group = coffsets.size(); + // auto coffsets = constraints->readMeta("color_offset",zs::wrapt>{}); + // int nm_group = coffsets.size(); auto& verts = zsparticles->getParticles(); - auto& cquads = constraints->getQuadraturePoints(); - auto category = constraints->category; + // auto& cquads = constraints->getQuadraturePoints(); + // auto category = constraints->category; // zs::Vector pv_buffer{verts.get_allocator(),verts.size()}; // zs::Vector total_ghost_impulse_X{verts.get_allocator(),1}; // zs::Vector total_ghost_impulse_Y{verts.get_allocator(),1}; // zs::Vector total_ghost_impulse_Z{verts.get_allocator(),1}; + + zs::Vector dv_buffer{verts.get_allocator(),verts.size() * 3}; + pol(zs::range(dv_buffer),[]ZS_LAMBDA(auto& v) {v = 0;}); + + // for(auto &&constraints) + // how does + for(int g = 0;g != nm_group;++g) { auto coffset = coffsets.getVal(g); int group_size = 0; @@ -598,4 +764,6 @@ ZENDEFNODE(XPBDSolveSmooth, {{{"zsparticles"},{"constraints"},{"float","dt","0.5 {{"string","ptag","X"}}, {"PBD"}}); + + }; From 397b321fc0d51514ddcc8db2e1c7b36fe76fa29e Mon Sep 17 00:00:00 2001 From: ShuliangLu Date: Tue, 25 Jul 2023 21:28:27 +0800 Subject: [PATCH 07/21] fix gia occasionally crash bug --- projects/CuLagrange/geometry/CollisionVis.cu | 4 +-- .../geometry/kernel/intersection.hpp | 34 +++++++++++++++++-- 2 files changed, 34 insertions(+), 4 deletions(-) diff --git a/projects/CuLagrange/geometry/CollisionVis.cu b/projects/CuLagrange/geometry/CollisionVis.cu index beb0d4f859..d8b97c87f3 100644 --- a/projects/CuLagrange/geometry/CollisionVis.cu +++ b/projects/CuLagrange/geometry/CollisionVis.cu @@ -470,8 +470,8 @@ namespace zeno { },tri_buffer.size()}; auto& halfedges = (*zsparticles)[ZenoParticles::s_surfHalfEdgeTag]; - auto nm_insts = do_global_self_intersection_analysis_on_surface_mesh_info( - cudaPol,verts_buffer,"x",tri_buffer,halfedges,inst_buffer_info,gia_res,false); + // auto nm_insts = do_global_self_intersection_analysis_on_surface_mesh_info( + // cudaPol,verts_buffer,"x",tri_buffer,halfedges,inst_buffer_info,gia_res,false); // zs::bht conn_of_first_ring{halfedges.get_allocator(),halfedges.size()}; auto ring_mask_width = do_global_self_intersection_analysis(cudaPol, verts_buffer,"x",tri_buffer,halfedges,gia_res,tris_gia_res); diff --git a/projects/CuLagrange/geometry/kernel/intersection.hpp b/projects/CuLagrange/geometry/kernel/intersection.hpp index 2915fe0351..ba17ace0aa 100644 --- a/projects/CuLagrange/geometry/kernel/intersection.hpp +++ b/projects/CuLagrange/geometry/kernel/intersection.hpp @@ -740,6 +740,7 @@ int do_global_self_intersection_analysis(Pol& pol, {"pair",2}, {"int_points",3}, {"r",1}, + {"is_broken",1} },max_nm_intersections}; if(!halfedges.hasProperty("broken")) @@ -747,6 +748,7 @@ int do_global_self_intersection_analysis(Pol& pol, TILEVEC_OPS::fill(pol,halfedges,"broken",(T)0.0); auto nm_insts = retrieve_self_intersection_tri_halfedge_list_info(pol,verts,xtag,tris,halfedges,ints_buffer); + TILEVEC_OPS::fill(pol,ints_buffer,"is_broken",(T)0); table_vec2i_type cftab{ints_buffer.get_allocator(),(size_t)nm_insts}; cftab.reset(pol,true); zs::Vector cfbuffer{ints_buffer.get_allocator(),(size_t)nm_insts}; @@ -821,7 +823,9 @@ int do_global_self_intersection_analysis(Pol& pol, printf("do_global_self_intersection_analysis::impossible reaching here, the hi and ohi should both have been inserted %f %f %f\n",(float)hr,(float)ohr,(float)ints_buffer("r",isi)); + ints_buffer("is_broken",isi) = (T)1.0; atomic_add(exec_tag,&nmInvalid[0],(int)1); + return; } } auto corner_idx = zs::reinterpret_bits(ints_buffer("corner_idx",isi)); @@ -854,12 +858,15 @@ int do_global_self_intersection_analysis(Pol& pol, } printf("do_global_self_intersection_analysis::impossible reaching here with broken insertion ring %f\n",(float)ints_buffer("r",isi)); + ints_buffer("is_broken",isi) = (T)1.0; atomic_add(exec_tag,&nmInvalid[0],(int)1); }); auto nmInvalidCount = nmInvalid.getVal(0); if(nmInvalidCount > 0) - throw std::runtime_error("SELF GIA invalid state detected"); + printf("SELF GIA invalid state detected\n"); + // there might be some broken rings + auto nmEntries = incidentItsTab.size(); zs::Vector> conn_topo{tris.get_allocator(),nmEntries}; @@ -871,6 +878,20 @@ int do_global_self_intersection_analysis(Pol& pol, auto nm_rings = mark_disconnected_island(pol,conn_topo,ringTag); + zs::Vector is_broken_rings{ringTag.get_allocator(),(size_t)nm_rings}; + pol(zs::range(is_broken_rings),[] ZS_LAMBDA(auto& is_br) mutable {is_br = 0;}); + pol(zs::range(nm_insts),[ringTag = proxy(ringTag),is_broken_rings = proxy(is_broken_rings),ints_buffer = proxy({},ints_buffer)] ZS_LAMBDA(int isi) mutable { + if(ints_buffer("is_broken",isi) > (T)0.5) { + auto ring_id = ringTag[isi]; + is_broken_rings[ring_id] = 1; + } + }); + + std::cout << "broken_ring_tag : "; + for(int i = 0;i != nm_rings;++i) + std::cout << is_broken_rings.getVal(i) << "\t"; + std::cout << std::endl; + // std::cout << "finish Mark disconnected island with nm_rings : " << nm_rings << std::endl; auto ring_mask_width = (nm_rings + 31) / 32; @@ -907,6 +928,11 @@ int do_global_self_intersection_analysis(Pol& pol, atomic_add(exec_tag,&ringSize[ringTag[isi]],(int)1); }); + // pol(zs::range(nm_rings),[ringSize = proxy(ringSize),is_broken_rings = proxy(is_broken_rings)] ZS_LAMBDA(int ri) mutable { + // if(is_broken_rings[ri]) + // ringSize[ri] = 0; + // }); + zs::Vector island_buffer{verts.get_allocator(),verts.size()}; @@ -929,7 +955,11 @@ int do_global_self_intersection_analysis(Pol& pol, for(int ri = 0;ri != nm_rings;++ri) { auto rsize = (size_t)ringSize.getVal(ri); - + // if(rsize == 0) + // continue; + auto is_broken_ring = is_broken_rings.getVal(ri); + if(is_broken_ring) + continue; // if(output_intermediate_information) // printf("ring[%d] Size : %d\n",ri,rsize); From 0787d6a1423a2eede93a70b24309813560fa43ce Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Fri, 4 Aug 2023 14:51:01 +0800 Subject: [PATCH 08/21] align XPBD branch --- .../CuLagrange/fem/FleshDynamicStepping.cu | 72 +- .../geometry/kernel/intersection.hpp | 18 +- .../CuLagrange/geometry/kernel/topology.hpp | 14 +- .../geometry/linear_system/mfcg.hpp | 4 +- projects/CuLagrange/pbd/Pipeline.cu | 639 +++++++++++------- .../constraint_function_kernel/constraint.cuh | 145 +++- .../constraint_types.hpp | 18 + 7 files changed, 607 insertions(+), 303 deletions(-) create mode 100644 projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp diff --git a/projects/CuLagrange/fem/FleshDynamicStepping.cu b/projects/CuLagrange/fem/FleshDynamicStepping.cu index df1f50a378..62558f2dda 100644 --- a/projects/CuLagrange/fem/FleshDynamicStepping.cu +++ b/projects/CuLagrange/fem/FleshDynamicStepping.cu @@ -681,7 +681,7 @@ struct FleshDynamicStepping : INode { dt = dt,offset = offset] ZS_LAMBDA(int ei) mutable { auto m = eles("m",ei)/(T)4.0; auto inds = eles.pack(dim_c<4>,"inds",ei).reinterpret_bits(int_c); - auto pgrad = zs::vec::zeros(); + // auto pgrad = zs::vec::zeros(); // auto H = zs::vec::zeros(); // if(eles.hasProperty("dt")) { // dt2 = eles("dt",ei) * eles("dt",ei); @@ -701,8 +701,13 @@ struct FleshDynamicStepping : INode { auto x0 = vtemp.pack(dim_c<3>,"xp",inds[i]); auto v0 = vtemp.pack(dim_c<3>,"vp",inds[i]); - auto alpha = inertia * m/dt2 * A; + auto alpha = (inertia * m / dt2) * A; auto nodal_pgrad = -alpha * (x1 - x0 - v0 * dt); + + if(isnan(nodal_pgrad.norm())) { + printf("nan nodal pgrad detected : %f %f %f %f\n",(float)alpha.norm(),(float)x1.norm(),(float)x0.norm(),(float)v0.norm()); + } + for(int d = 0;d != 3;++d){ auto idx = i * 3 + d; gh_buffer("grad",idx,ei + offset) = nodal_pgrad[d]; @@ -719,6 +724,9 @@ struct FleshDynamicStepping : INode { }); } + // auto gradn_after_inertia = TILEVEC_OPS::dot<12>(cudaPol,gh_buffer,"grad","grad"); + // std::cout << "gradn_after_inertia : " << gradn_after_inertia << std::endl; + cudaPol(zs::range(eles.size()), [dt = dt,dt2 = dt2,aniso_strength = aniso_strength, verts = proxy({},verts), vtemp = proxy({}, vtemp), @@ -767,6 +775,7 @@ struct FleshDynamicStepping : INode { auto dFdXT = dFdX.transpose(); auto vf = -vole * (dFdXT * vecP); + auto mg = volf * vole / (T)4.0; for(int i = 0;i != 4;++i) for(int d = 0;d !=3 ;++d){ @@ -784,25 +793,34 @@ struct FleshDynamicStepping : INode { auto fiber = eles.pack(dim_c<3>,"fiber",ei); if(zs::abs(fiber.norm() - 1.0) < 1e-3) { fiber /= fiber.norm(); - // if(eles.hasProperty("mu")) { - // amodel.mu = eles("mu",ei); - // // amodel.lam = eles("lam",ei); + if(eles.hasProperty("mu") && eles.hasProperty("lam")) { + amodel.mu = eles("mu",ei); + // amodel.lam = eles("lam",ei); - // } + } + // COMMIT FIND A ANISOTROPIC BUG HERE auto aP = amodel.do_first_piola(FAct,fiber); - auto vecAP = flatten(P); - vecAP = dFActdF.transpose() * vecP; + auto vecAP = flatten(aP); + vecAP = dFActdF.transpose() * vecAP; vf -= vole * dFdXT * vecAP *aniso_strength; - // auto aHq = amodel.do_first_piola_derivative(FAct,fiber); - auto aHq = zs::vec::zeros(); + auto aHq = amodel.do_first_piola_derivative(FAct,fiber); + // auto aHq = zs::vec::zeros(); H += dFdAct_dFdX.transpose() * aHq * dFdAct_dFdX * vole * aniso_strength; // if((int)eles("Muscle_ID",ei) == 0){ // printf("fiber : %f %f %f,Fa = %f,aP = %f,aHq = %f,H = %f\n",fiber[0],fiber[1],fiber[2],(float)FAct.norm(),(float)aP.norm(),(float)aHq.norm(),(float)H.norm()); // } + + if(isnan(vf.norm())) { + printf("nan nodal aniso_vf detected : %f %f %f %f\n",(float)vecP.norm(),(float)volf.norm(),(float)vecAP.norm(),(float)aHq.norm()); + } } } + // if(isnan(vf.norm())) { + // printf("nan nodal vf detected : %f %f %f %f\n",(float)vecP.norm(),(float)volf.norm(),(float)P.norm(),(float)FAct.norm()); + // } + gh_buffer.tuple(dim_c<12>,"grad",ei + offset) = gh_buffer.pack(dim_c<12>,"grad",ei + offset) + vf/* - rdamping*/; // gh_buffer.tuple(dim_c<12>,"grad",ei + offset) = gh_buffer.pack(dim_c<12>,"grad",ei + offset) - rdamping; // H += kd_beta*H/dt; @@ -813,6 +831,10 @@ struct FleshDynamicStepping : INode { // T lambda = model.lam; // T mu = model.mu; + + // auto gradn_after_elastic = TILEVEC_OPS::dot<12>(cudaPol,gh_buffer,"grad","grad"); + // std::cout << "gradn_after_elastic : " << gradn_after_elastic << std::endl; + auto nmEmbedVerts = b_verts.size(); cudaPol(zs::range(nmEmbedVerts), [ @@ -882,6 +904,8 @@ struct FleshDynamicStepping : INode { }); + // auto gradn_after_driver = TILEVEC_OPS::dot<12>(cudaPol,gh_buffer,"grad","grad"); + // std::cout << "gradn_after_driver : " << gradn_after_driver << std::endl; } @@ -1997,6 +2021,17 @@ struct FleshDynamicStepping : INode { },[](...) { throw std::runtime_error("unsupported anisotropic elasticity model"); })(models.getElasticModel(),models.getAnisoElasticModel()); + + // { + // auto gradn = TILEVEC_OPS::dot<12>(cudaPol,gh_buffer,"grad","grad"); + // std::cout << "gradn after elastic and inertia : " << gradn << std::endl; + // if(std::isnan(gradn)) { + // printf("nan gradn = %f detected after compute computeGradientAndHessian\n",gradn); + // // printf("Hn = ") + // throw std::runtime_error("nan gradn detected after compute computeGradientAndHessian"); + // } + // } + // std::cout << "computePositionConstraintGradientAndHessian : " << kverts.size() << std::endl; // the binder constraint gradient and hessian if(use_binder_constraint) { @@ -2270,7 +2305,7 @@ struct FleshDynamicStepping : INode { timer.tick(); spmat._vals.reset(0); - std::cout << "update gh_buffer spmat" << std::endl; + // std::cout << "update gh_buffer spmat" << std::endl; cudaPol(zs::range(eles.size()), [gh_buffer = proxy({},gh_buffer), @@ -2285,7 +2320,7 @@ struct FleshDynamicStepping : INode { update_hessian(spmat,inds,H,true); }); - std::cout << "update sttemp spmat" << std::endl; + // std::cout << "update sttemp spmat" << std::endl; cudaPol(zs::range(sttemp.size()), [sttemp = proxy({},sttemp),vsize = verts.size(),spmat = proxy(spmat)] ZS_LAMBDA(int vi) mutable { @@ -2297,7 +2332,7 @@ struct FleshDynamicStepping : INode { update_hessian(spmat,inds,H,true); }); - std::cout << "update vtemp spmat" << std::endl; + // std::cout << "update vtemp spmat" << std::endl; cudaPol(zs::range(vtemp.size()), [vtemp = proxy({},vtemp),vsize = verts.size(),spmat = proxy(spmat)] ZS_LAMBDA(int vi) mutable { @@ -2308,7 +2343,7 @@ struct FleshDynamicStepping : INode { update_hessian(spmat,inds,H,true); }); - std::cout << "update kinematics spmat" << std::endl; + // std::cout << "update kinematics spmat" << std::endl; if(use_kinematics_collision && kinematics.size() > 0) cudaPol(zs::range(ktris_vert_collision_buffer.size()),[ @@ -2319,7 +2354,7 @@ struct FleshDynamicStepping : INode { update_hessian(spmat,inds,H,true); }); - std::cout << "finish upate hessian" << std::endl; + // std::cout << "finish upate hessian" << std::endl; timer.tock("spmat evaluation"); #endif @@ -2341,6 +2376,13 @@ struct FleshDynamicStepping : INode { // std::cout << "Hn : " << Hn << std::endl; int nm_CG_iters = 0; #ifdef USE_SPARSE_MATRIX + // auto gradn = TILEVEC_OPS::dot<3>(cudaPol,vtemp,"grad","grad"); + // if(std::isnan(gradn)) { + // printf("nan gradn = %f detected\n",gradn); + // // printf("Hn = ") + // throw std::runtime_error("nan gradn detected"); + // } + if(turn_on_self_collision) nm_CG_iters = PCG::pcg_with_fixed_sol_solve<3>(cudaPol,vtemp,spmat,self_collision_fp_buffer,"dir","bou_tag","grad","P","inds","H",(T)cg_res,max_cg_iters,100); else diff --git a/projects/CuLagrange/geometry/kernel/intersection.hpp b/projects/CuLagrange/geometry/kernel/intersection.hpp index ba17ace0aa..a82b66786e 100644 --- a/projects/CuLagrange/geometry/kernel/intersection.hpp +++ b/projects/CuLagrange/geometry/kernel/intersection.hpp @@ -1796,14 +1796,14 @@ int do_global_self_intersection_analysis_on_surface_mesh_info(Pol& pol, vec3 v1[3] = {}; vec3 e0s[3] = {}; vec3 e1s[3] = {}; - for(int i = 0;i != 3;++i) { - v0[i] = verts.pack(dim_c<3>,xtag,tri_t0[i]); - v1[i] = verts.pack(dim_c<3>,xtag,tri_t1[i]); + for(int j = 0;j != 3;++j) { + v0[j] = verts.pack(dim_c<3>,xtag,tri_t0[j]); + v1[j] = verts.pack(dim_c<3>,xtag,tri_t1[j]); } - for(int i = 0;i != 3;++i) { - e0s[i] = v0[(i + 1) % 3] - v0[i]; - e1s[i] = v1[(i + 1) % 3] - v1[i]; + for(int j = 0;j != 3;++j) { + e0s[j] = v0[(j + 1) % 3] - v0[j]; + e1s[j] = v1[(j + 1) % 3] - v1[j]; } auto nrm0 = (v0[1] - v0[0]).cross(v0[2] - v0[0]).normalized(); @@ -1926,9 +1926,9 @@ int do_global_self_intersection_analysis_on_surface_mesh_info(Pol& pol, vec3 v1[3] = {}; vec3 e0s[3] = {}; vec3 e1s[3] = {}; - for(int i = 0;i != 3;++i) { - v0[i] = verts.pack(dim_c<3>,xtag,tri_t0[i]); - v1[i] = verts.pack(dim_c<3>,xtag,tri_t1[i]); + for(int j = 0;j != 3;++j) { + v0[j] = verts.pack(dim_c<3>,xtag,tri_t0[j]); + v1[j] = verts.pack(dim_c<3>,xtag,tri_t1[j]); } for(int i = 0;i != 3;++i) { diff --git a/projects/CuLagrange/geometry/kernel/topology.hpp b/projects/CuLagrange/geometry/kernel/topology.hpp index 12217e53f2..0fbd4c2038 100644 --- a/projects/CuLagrange/geometry/kernel/topology.hpp +++ b/projects/CuLagrange/geometry/kernel/topology.hpp @@ -843,12 +843,12 @@ namespace zeno { tet[facet_indices[i * 3 + 2]]}; int min_idx = 0; int min_id = facet[i]; - for(int i = 1;i != 3;++i) - if(facet[i] < min_id){ - min_idx = i; - min_id = facet[i]; + for(int j = 1;j != 3;++j) + if(facet[j] < min_id){ + min_idx = j; + min_id = facet[j]; } - for(int i = 0;i != min_idx;++i) { + for(int j = 0;j != min_idx;++j) { auto tmp = facet[0]; facet[0] = facet[1]; facet[1] = facet[2]; @@ -1258,7 +1258,7 @@ namespace zeno { colors.resize(topo.size()); - zs::SparseMatrix topo_incidence_matrix{topo.get_allocator(),topo.size(),topo.size()}; + zs::SparseMatrix topo_incidence_matrix{topo.get_allocator(),(int)topo.size(),(int)topo.size()}; // std::cout << "compute incidence matrix " << std::endl; topological_incidence_matrix(pol,topo,topo_incidence_matrix); // std::cout << "finish compute incidence matrix " << std::endl; @@ -1314,7 +1314,7 @@ namespace zeno { atomic_max(exec_tag,&max_color[0],color); }); - int nm_total_colors = max_color.getVal(0) + 1; + size_t nm_total_colors = max_color.getVal(0) + 1; // zs::bht color_buffer{} zs::Vector nm_colors{colors.get_allocator(),nm_total_colors}; pol(zs::range(nm_colors),[] ZS_LAMBDA(auto& nclr) mutable {nclr = 0;}); diff --git a/projects/CuLagrange/geometry/linear_system/mfcg.hpp b/projects/CuLagrange/geometry/linear_system/mfcg.hpp index b0eb3045ac..806ffb0a0a 100644 --- a/projects/CuLagrange/geometry/linear_system/mfcg.hpp +++ b/projects/CuLagrange/geometry/linear_system/mfcg.hpp @@ -738,8 +738,8 @@ if(do_cuda_profile) auto rn = TILEVEC_OPS::dot<3>(cudaPol,vtemp,"r","r"); auto qn = TILEVEC_OPS::dot<3>(cudaPol,vtemp,"q","q"); auto Pn = TILEVEC_OPS::dot<9>(cudaPol,vtemp,"P","P"); - auto bn = TILEVEC_OPS::dot<3>(cudaPol,vtemp,"b","b"); - auto xn = TILEVEC_OPS::dot<3>(cudaPol,vtemp,"x","x"); + auto bn = TILEVEC_OPS::dot<3>(cudaPol,vert_buffer,btag,btag); + auto xn = TILEVEC_OPS::dot<3>(cudaPol,vert_buffer,xtag,xtag); auto tempn = TILEVEC_OPS::dot<3>(cudaPol,vtemp,"temp","temp"); fmt::print(fg(fmt::color::dark_cyan),"nan zTrk detected = {} qn = {} rn = {} Pn = {} bn = {} tempn = {} xn = {}\n",zTrk,qn,rn,Pn,bn,tempn,xn); throw std::runtime_error("nan zTrk detected"); diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index 9efa9f5d64..03b3f60d09 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -15,6 +15,7 @@ #include "constraint_function_kernel/constraint.cuh" #include "../geometry/kernel/topology.hpp" #include "../geometry/kernel/geo_math.hpp" +#include "constraint_function_kernel/constraint_types.hpp" namespace zeno { @@ -22,8 +23,37 @@ namespace zeno { // serve triangulate mesh or strands only currently struct MakeSurfaceConstraintTopology : INode { + template + void buildBvh(zs::CudaExecutionPolicy &pol, + TileVecT &verts, + const zs::SmallString& srcTag, + const zs::SmallString& dstTag, + const zs::SmallString& pscaleTag, + ZenoLinearBvh::lbvh_t &bvh) { + using namespace zs; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto space = execspace_e::cuda; + Vector bvs{verts.get_allocator(), verts.size()}; + pol(range(verts.size()), + [verts = proxy({}, verts), + bvs = proxy(bvs), + pscaleTag,srcTag,dstTag] ZS_LAMBDA(int vi) mutable { + auto src = verts.template pack<3>(srcTag, vi); + auto dst = verts.template pack<3>(dstTag, vi); + auto pscale = verts(pscaleTag,vi); + + bv_t bv{src,dst}; + bv._min -= pscale; + bv._max += pscale; + bvs[vi] = bv; + }); + bvh.build(pol, bvs); + } + virtual void apply() override { using namespace zs; + using namespace PBD_CONSTRAINT; + using vec3 = zs::vec; using vec2i = zs::vec; using vec3i = zs::vec; @@ -55,7 +85,8 @@ struct MakeSurfaceConstraintTopology : INode { auto &eles = constraint->getQuadraturePoints(); if(type == "stretch") { - constraint->category = ZenoParticles::curve; + constraint->setMeta(CONSTRAINT_KEY,category_c::edge_length_constraint); + // constraint->category = ZenoParticles::curve; auto quads_vec = tilevec_topo_to_zsvec_topo(cudaPol,quads,wrapv<3>{}); zs::Vector> edge_topos{quads.get_allocator(),0}; @@ -102,7 +133,8 @@ struct MakeSurfaceConstraintTopology : INode { } if(type == "bending") { - constraint->category = ZenoParticles::tri_bending_spring; + constraint->setMeta(CONSTRAINT_KEY,category_c::isometric_bending_constraint); + // constraint->category = ZenoParticles::tri_bending_spring; // constraint->sprayedOffset = 0; const auto& halfedges = (*source)[ZenoParticles::s_surfHalfEdgeTag]; @@ -163,6 +195,72 @@ struct MakeSurfaceConstraintTopology : INode { eles.tuple(dim_c<16>,"Q",oei) = Q; }); } + if(type == "kcollision") { + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + + constraint->setMeta(CONSTRAINT_KEY,category_c::p_kp_collision_constraint); + auto target = get_input("target"); + + const auto& kverts = target->getParticles(); + ZenoLinearBvh::lbvh_t kbvh{}; + buildBvh(cudaPol,kverts,"px","x","pscale",kbvh); + + zs::bht csPP{verts.get_allocator(),verts.size()}; + csPP.reset(cudaPol,true); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + kverts = proxy({},kverts), + kbvh = proxy(kbvh), + csPP = proxy(csPP)] ZS_LAMBDA(int vi) mutable { + auto x = verts.pack(dim_c<3>,"x",vi); + auto px = verts.pack(dim_c<3>,"px",vi); + auto mx = (x + px) / (T)2.0; + auto pscale = verts("pscale",vi); + + auto radius = (mx - px).norm() + pscale * (T)2.0; + auto bv = bv_t{mx - radius,mx + radius}; + + int contact_kvi = -1; + T min_contact_time = std::numeric_limits::max(); + + auto process_ccd_collision = [&](int kvi) { + auto kpscale = kverts("pscale",kvi); + auto kx = kverts.pack(dim_c<3>,"x",kvi); + auto pkx = kx; + if(kverts.hasProperty("px")) + pkx = kverts.pack(dim_c<3>,"px",kvi); + + auto t = LSL_GEO::ray_ray_intersect(px,x - px,pkx,kx - pkx,(pscale + kpscale) * (T)2); + if(t < min_contact_time) { + min_contact_time = t; + contact_kvi = kvi; + } + }; + kbvh.iter_neighbors(bv,process_ccd_collision); + + if(contact_kvi >= 0) { + csPP.insert(vec2i{vi,contact_kvi}); + } + }); + + eles.resize(csPP.size()); + colors.resize(csPP.size()); + reordered_map.resize(csPP.size()); + + eles.append_channels(cudaPol,{{"inds",2}}); + cudaPol(zip(zs::range(csPP.size()),csPP._activeKeys),[ + eles = proxy({},eles), + colors = proxy(colors), + reordered_map = proxy(reordered_map)] ZS_LAMBDA(auto ei,const auto& pair) mutable { + eles.tuple(dim_c<2>,"inds",ei) = pair.reinterpret_bits(float_c); + colors[ei] = (T)0; + reordered_map[ei] = ei; + }); + + color_offset.resize(1); + color_offset.setVal(0); + } cudaPol(zs::range(eles.size()),[ eles = proxy({},eles), @@ -179,20 +277,20 @@ struct MakeSurfaceConstraintTopology : INode { constraint->setMeta("color_offset",color_offset); - set_output("source",source); + // set_output("source",source); set_output("constraint",constraint); }; }; ZENDEFNODE(MakeSurfaceConstraintTopology, {{ {"source"}, + {"target"}, {"float","stiffness","0.5"}, {"string","topo_type","stretch"}, {"float","rest_scale","1.0"} }, {{"constraint"}}, - { - + { // {"string","groupID",""}, }, {"PBD"}}); @@ -206,6 +304,8 @@ struct VisualizePBDConstraint : INode { virtual void apply() override { using namespace zs; + using namespace PBD_CONSTRAINT; + constexpr auto space = execspace_e::cuda; auto cudaPol = cuda_exec(); @@ -243,7 +343,9 @@ struct VisualizePBDConstraint : INode { auto prim = std::make_shared(); auto& pverts = prim->verts; - if(constraints_ptr->category == ZenoParticles::curve) { + auto constraint_type = constraints_ptr->readMeta(CONSTRAINT_KEY,wrapt{}); + + if(constraint_type == category_c::edge_length_constraint) { pverts.resize(constraints.size() * 2); auto& plines = prim->lines; plines.resize(constraints.size()); @@ -259,7 +361,7 @@ struct VisualizePBDConstraint : INode { plines[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; ltclrs[ci] = cclrs[ci]; }); - }else if(constraints_ptr->category == ZenoParticles::tri_bending_spring) { + }else if(constraint_type == category_c::isometric_bending_constraint) { pverts.resize(constraints.size() * 2); auto& plines = prim->lines; plines.resize(constraints.size()); @@ -293,169 +395,13 @@ ZENDEFNODE(VisualizePBDConstraint, {{{"zsparticles"},{"constraints"}}, }, {"PBD"}}); - -struct KinematicCollisionDetection : INode { - using T = float; - using tiles_t = typename ZenoParticles::particles_t; - using dtiles_t = zs::TileVector; - using bvh_t = zs::LBvh<3, int, T>; - using vec3 = zs::vec; - using vec2i = zs::vec; - // using ICOORD = vec2i; - using vec3i = zs::vec; - using vec4i = zs::vec; - using mat4 = zs::vec; - - template - void build_ray_bvh(zs::CudaExecutionPolicy &pol, TileVecT &verts,ZenoLinearBvh::lbvh_t &bvh) { - using namespace zs; - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto space = execspace_e::cuda; - Vector bvs{verts.get_allocator(),verts.size()}; - pol(range(verts.size()),[ - verts = proxy({},verts),bvs = proxy(bvs)] ZS_LAMBDA(int vi) { - auto x = verts.pack(dim_c<3>,"x",vi); - auto px = verts.pack(dim_c<3>,"px",vi); - auto pscale = verts("pscale",vi); - - bv_t bv{x,xp}; - bv._min -= pscale; - bv._max += pscale; - - bvs[vi] = bv; - }); - bvh.build(pol,bvs); - } - - template - void build_particles_bvh(zs::CudaExecutionPolicy& pol,TileVecT &kverts,ZenoLinearBvh::lbvh_t& bvh) { - using namespace zs; - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto space = execspace_e::cuda; - Vector bvs{kverts.get_allocator(),kverts.size()}; - pol(range(kverts.size()),[ - kverts = proxy({},kverts),bvs = proxy(bvs)] ZS_LAMBDA(int kvi) { - auto x = kverts.pack(dim_c<3>,"x",vi); - auto pscale = kverts("pscale",vi); - bv_t bv{x - pscale,x + pscale}; - bvs[vi] = bv; - }); - bvh.build(pol,bvs); - } - - - virtual void apply() override { - using namespace zs; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - constexpr auto exec_tag = wrapv{}; - - auto zsparticles = get_input("zsparticles"); - auto boundary = get_input("kboundary"); - auto sdf = get_input("sdf"); - - auto constraints = std::make_shared(); - - constraint->sprayedOffset = 0; - constraint->elements = typename ZenoParticles::particles_t({{"stiffness",1},{"lambda",1},{"tclr",1}}, 0, zs::memsrc_e::device,0); - auto &eles = constraint->getQuadraturePoints(); - - // auto xtag = get_param("xtag"); - // auto pxtag = get_param("pxtag"); - // auto kxtag = get_param("kxtag"); - // auto pkxtag = get_param("pkxtag"); - - const auto& verts = zsparticles->getParticles(); - const auto& kverts = boundary->getParticles(); - - // use PP collision only - bvh_t kbvh{}; - build_particles_bvh(cudaPol,kverts,kbvh); - - zs::Vector hit_points{verts.get_allocator(),verts.size()}; - zs:Vector hit_distances{verts.get_allocator(),verts.size()}; - pol(range(hit_points),[]ZS_LAMBDA(auto &hp) mutable {hp = -1;}); - pol(range(hit_distance),[]ZS_LAMBDA(auto &dist) mutable { dist = std::numeric_limits::infinity();}); - - zs::bht csPP{verts.get_allocator(),verts.size()}; - csPP.reset(pol,true); - - cudaPol(zs::range(verts.size()),[ - kverts = proxy({},kverts), - verts = proxy({},verts), - hit_points = proxy(hit_points), - csPP = proxy(csPP), - kbvh = proxy(kbvh)] ZS_LAMBDA(int vi) mutable { - auto x = verts.pack(dim_c<3>,"x",vi); - auto px = verts.pack(dim_c<3>,"px",vi); - auto pscale = verts("pscale",vi); - - auto bv = bv_t{get_bounding_box(x,px)}; - bv._min -= pscale; - bv._max += pscale; - - T mint = std::numeric_limits::infinity(); - int min_kvi = -1; - - auto process_ccd_collision = [&](int kvi) mutable { - auto kpscale = kverts("pscale",kvi); - auto kx = kverts.pack(dim_c<3>,"x",kvi); - auto pkx = kx; - if(kverts.hasProperty("px")) - pkx = kverts.pack(dim_c<3>,"px",kvi); - - auto t = LSL_GEO::ray_ray_intersect(px,x - px,pkx,kx - pkx,pscale + kpscale); - if(t < mint) { - mint = t; - min_kvi = kvi; - } - }; - iter_neighbors(bv,process_ccd_collision); - - if(min_kvi >= 0) { - csPP.insert(vec2i{vi,min_kvi}); - } - - hit_points[vi] = min_kvi; - hit_distance[vi] = mint; - }); - - - } -}; - -struct SelfCollisionDetection : INode { - virtual void apply() override { - using namespace zs; - using vec3 = zs::vec; - using vec2i = zs::vec; - using vec3i = zs::vec; - using vec4i = zs::vec; - using mat4 = zs::vec; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - constexpr auto exec_tag = wrapv{}; - - auto zsparticles = get_input("zsparticles"); - - auto constraints = std::make_shared(); - - constraint->sprayedOffset = 0; - constraint->elements = typename ZenoParticles::particles_t({{"stiffness",1},{"lambda",1},{"tclr",1}}, 0, zs::memsrc_e::device,0); - auto &eles = constraint->getQuadraturePoints(); - - - - } -}; - // solve a specific type of constraint for one iterations struct XPBDSolve : INode { virtual void apply() override { using namespace zs; + using namespace PBD_CONSTRAINT; + using vec3 = zs::vec; using vec2i = zs::vec; using vec3i = zs::vec; @@ -468,6 +414,7 @@ struct XPBDSolve : INode { auto zsparticles = get_input("zsparticles"); auto constraints = get_input("constraints"); + auto dt = get_input2("dt"); auto ptag = get_param("ptag"); @@ -476,13 +423,16 @@ struct XPBDSolve : INode { auto& verts = zsparticles->getParticles(); auto& cquads = constraints->getQuadraturePoints(); - auto category = constraints->category; - + auto category = constraints->readMeta(CONSTRAINT_KEY,wrapt{}); + auto target = get_input("target"); + const auto& kverts = target->getParticles(); // zs::Vector pv_buffer{verts.get_allocator(),verts.size()}; // zs::Vector total_ghost_impulse_X{verts.get_allocator(),1}; // zs::Vector total_ghost_impulse_Y{verts.get_allocator(),1}; // zs::Vector total_ghost_impulse_Z{verts.get_allocator(),1}; + // std::cout << "SOVLE CONSTRAINT WITH GROUP : " << nm_group << "\t" << cquads.size() << std::endl; + for(int g = 0;g != nm_group;++g) { auto coffset = coffsets.getVal(g); int group_size = 0; @@ -498,17 +448,20 @@ struct XPBDSolve : INode { // pv_buffer[vi] = verts.pack(dim_c<3>,ptag,vi); // }); + + cudaPol(zs::range(group_size),[ coffset, verts = proxy({},verts), category, dt, ptag = zs::SmallString(ptag), + kverts = proxy({},kverts), cquads = proxy({},cquads)] ZS_LAMBDA(int gi) mutable { float s = cquads("stiffness",coffset + gi); float lambda = cquads("lambda",coffset + gi); - if(category == ZenoParticles::curve) { + if(category == category_c::edge_length_constraint) { auto edge = cquads.pack(dim_c<2>,"inds",coffset + gi,int_c); vec3 p0{},p1{}; p0 = verts.pack(dim_c<3>,ptag,edge[0]); @@ -537,7 +490,7 @@ struct XPBDSolve : INode { // if(ghost_impulse > 1e-6) // printf("dmomentum : %f\n",(float)ghost_impulse); } - if(category == ZenoParticles::tri_bending_spring) { + if(category == category_c::isometric_bending_constraint) { auto quad = cquads.pack(dim_c<4>,"inds",coffset + gi,int_c); vec3 p[4] = {}; float minv[4] = {}; @@ -552,7 +505,8 @@ struct XPBDSolve : INode { CONSTRAINT::solve_IsometricBendingConstraint( p[0],minv[0], p[1],minv[1], - p[2],minv[2],XPBDCCD + p[2],minv[2], + p[3],minv[3], Q, s, dt, @@ -562,6 +516,27 @@ struct XPBDSolve : INode { for(int i = 0;i != 4;++i) verts.tuple(dim_c<3>,ptag,quad[i]) = p[i] + dp[i]; } + + if(category == category_c::p_kp_collision_constraint) { + auto quad = cquads.pack(dim_c<2>,"inds",coffset + gi,int_c); + + vec3 p = verts.pack(dim_c<3>,ptag,quad[0]); + vec3 kp = kverts.pack(dim_c<3>,"x",quad[1]); + vec3 knrm = kverts.pack(dim_c<3>,"nrm",quad[1]); + + auto pscale = verts("pscale",quad[0]); + auto kpscale = kverts("pscale",quad[1]); + + T minv = verts("minv",quad[0]); + // T kminv = kverts("minv",quad[1]); + + vec3 dp = {}; + auto thickness = pscale + kpscale; + thickness *= (float)1.01; + CONSTRAINT::solve_PlaneConstraint(p,minv,kp,knrm,thickness,s,dt,lambda,dp); + verts.tuple(dim_c<3>,ptag,quad[0]) = p + dp; + } + cquads("lambda",coffset + gi) = lambda; }); @@ -595,18 +570,194 @@ struct XPBDSolve : INode { set_output("constraints",constraints); set_output("zsparticles",zsparticles); + set_output("target",target); }; }; -ZENDEFNODE(XPBDSolve, {{{"zsparticles"},{"constraints"},{"float","dt","0.5"}}, - {{"zsparticles"},{"constraints"}}, + +ZENDEFNODE(XPBDSolve, {{{"zsparticles"},{"constraints"},{"target"},{"float","dt","0.5"}}, + {{"zsparticles"},{"constraints"},{"target"}}, {{"string","ptag","X"}}, {"PBD"}}); +// struct ParticlesColliderProject : INode { +// using T = float; +// using vec3 = zs::vec; + +// virtual void apply() override { +// using namespace zs; +// constexpr auto space = execspace_e::cuda; +// auto cudaPol = cuda_exec(); + +// auto zsparticles = get_intput("zsparticles"); +// auto xtag = get_input2("xtag"); +// auto ptag = get_input2("ptag"); + +// auto& verts = zsparticles->getParticles(); + + +// } +// }; + + +struct SDFColliderProject : INode { + using T = float; + using vec3 = zs::vec; + + virtual void apply() override { + using namespace zs; + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + + auto zsparticles = get_input("zsparticles"); + + auto radius = get_input2("radius"); + auto center = get_input2("center"); + auto cv = get_input2("center_velocity"); + auto w = get_input2("angular_velocity"); + + // prex + auto xtag = get_input2("xtag"); + // x + auto ptag = get_input2("ptag"); + auto friction = get_input2("friction"); + + // auto do_stablize = get_input2("do_stablize"); + + auto& verts = zsparticles->getParticles(); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + ptag = zs::SmallString(ptag), + friction, + radius, + center, + // do_stablize, + cv,w] ZS_LAMBDA(int vi) mutable { + auto pred = verts.pack(dim_c<3>,ptag,vi); + auto pos = verts.pack(dim_c<3>,xtag,vi); + + auto center_vel = vec3::from_array(cv); + auto center_pos = vec3::from_array(center); + auto angular_velocity = vec3::from_array(w); + + auto disp = pred - center_pos; + auto dist = radius - disp.norm() + verts("pscale",vi); + + if(dist < 0) + return; + + auto nrm = disp.normalized(); + + auto dp = dist * nrm; + if(dp.norm() < (T)1e-6) + return; + + pred += dp; + + // if(do_stablize) { + // pos += dp; + // verts.tuple(dim_c<3>,xtag,vi) = pos; + // } + + auto collider_velocity_at_p = center_vel + angular_velocity.cross(pred - center_pos); + auto rel_vel = pred - pos - center_vel; + + auto tan_vel = rel_vel - nrm * rel_vel.dot(nrm); + auto tan_len = tan_vel.norm(); + auto max_tan_len = dp.norm() * friction; + + if(tan_len > (T)1e-6) { + auto alpha = (T)max_tan_len / (T)tan_len; + dp = -tan_vel * zs::min(alpha,(T)1.0); + pred += dp; + } + + verts.tuple(dim_c<3>,ptag,vi) = pred; + }); + set_output("zsparticles",zsparticles); + } + +}; + +ZENDEFNODE(SDFColliderProject, {{{"zsparticles"}, + {"float","radius","1"}, + {"center"}, + {"center_velocity"}, + {"angular_velocity"}, + {"string","xtag","x"}, + {"string","ptag","x"}, + {"float","friction","0"} + // {"bool","do_stablize","0"} + }, + {{"zsparticles"}}, + {}, + {"PBD"}}); + + +struct BuildZSLBvhFromAABB : INode { + + template + void buildBvh(zs::CudaExecutionPolicy &pol, + TileVecT &verts, + const zs::SmallString& srcTag, + const zs::SmallString& dstTag, + const zs::SmallString& pscaleTag, + ZenoLinearBvh::lbvh_t &bvh) { + using namespace zs; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto space = execspace_e::cuda; + Vector bvs{verts.get_allocator(), verts.size()}; + pol(range(verts.size()), + [verts = proxy({}, verts), + bvs = proxy(bvs), + pscaleTag,srcTag,dstTag] ZS_LAMBDA(int vi) mutable { + auto src = verts.template pack<3>(srcTag, vi); + auto dst = verts.template pack<3>(dstTag, vi); + auto pscale = verts(pscaleTag,vi); + + bv_t bv{src,dst}; + bv._min -= pscale; + bv._max += pscale; + bvs[vi] = bv; + }); + bvh.build(pol, bvs); + } + + virtual void apply() override { + using namespace zs; + auto cudaPol = cuda_exec().device(0); + + auto pars = get_input("ZSParticles"); + auto srcTag = get_input2("fromTag"); + auto dstTag = get_input2("toTag"); + auto pscaleTag = get_input2("pscaleTag"); + + auto out = std::make_shared(); + auto &bvh = out->get(); + + buildBvh(cudaPol,pars->getParticles(),srcTag,dstTag,pscaleTag,bvh); + out->thickness = 0; + + set_output("ZSLBvh",std::move(out)); + } +}; + +ZENDEFNODE(BuildZSLBvhFromAABB, {{{"ZSParticles"}, + {"string","fromTag","px"}, + {"string","toTag","x"}, + {"string","pscaleTag","pscale"} + }, {"ZSLBvh"}, {}, {"XPBD"}}); + + + struct XPBDSolveSmooth : INode { virtual void apply() override { using namespace zs; + using namespace PBD_CONSTRAINT; + using vec3 = zs::vec; using vec2i = zs::vec; using vec3i = zs::vec; @@ -619,10 +770,11 @@ struct XPBDSolveSmooth : INode { auto zsparticles = get_input("zsparticles"); - auto all_constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "constraints"); + auto all_constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "all_constraints"); // auto constraints = get_input("constraints"); - auto dt = get_input2("dt"); + // auto dt = get_input2("dt"); auto ptag = get_param("ptag"); + auto w = get_input2("relaxation_strength"); // auto coffsets = constraints->readMeta("color_offset",zs::wrapt>{}); // int nm_group = coffsets.size(); @@ -637,68 +789,57 @@ struct XPBDSolveSmooth : INode { // zs::Vector total_ghost_impulse_Z{verts.get_allocator(),1}; - zs::Vector dv_buffer{verts.get_allocator(),verts.size() * 3}; - pol(zs::range(dv_buffer),[]ZS_LAMBDA(auto& v) {v = 0;}); - - // for(auto &&constraints) - // how does + zs::Vector dp_buffer{verts.get_allocator(),verts.size() * 3}; + cudaPol(zs::range(dp_buffer),[]ZS_LAMBDA(auto& v) {v = 0;}); + zs::Vector dp_count{verts.get_allocator(),verts.size()}; + cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) {c = 0;}); - for(int g = 0;g != nm_group;++g) { - auto coffset = coffsets.getVal(g); - int group_size = 0; - if(g == nm_group - 1) - group_size = cquads.size() - coffsets.getVal(g); - else - group_size = coffsets.getVal(g + 1) - coffsets.getVal(g); + for(auto &&constraints : all_constraints) { + const auto& cquads = constraints->getQuadraturePoints(); + auto category = constraints->readMeta(CONSTRAINT_KEY,wrapt{}); - // cudaPol(zs::range(verts.size()),[ - // ptag = zs::SmallString(ptag), - // verts = proxy({},verts), - // pv_buffer = proxy(pv_buffer)] ZS_LAMBDA(int vi) mutable { - // pv_buffer[vi] = verts.pack(dim_c<3>,ptag,vi); - // }); + // std::cout << "computing smoothing for constraints" << std::endl; - cudaPol(zs::range(group_size),[ - coffset, + cudaPol(zs::range(cquads.size()),[ verts = proxy({},verts), category, - dt, + // dt, + w, + exec_tag, + dp_buffer = proxy(dp_buffer), + dp_count = proxy(dp_count), ptag = zs::SmallString(ptag), - cquads = proxy({},cquads)] ZS_LAMBDA(int gi) mutable { - float s = cquads("stiffness",coffset + gi); - float lambda = cquads("lambda",coffset + gi); + cquads = proxy({},cquads)] ZS_LAMBDA(int ci) mutable { + float s = cquads("stiffness",ci); + float lambda = cquads("lambda",ci); - if(category == ZenoParticles::curve) { - auto edge = cquads.pack(dim_c<2>,"inds",coffset + gi,int_c); + if(category == category_c::edge_length_constraint) { + auto edge = cquads.pack(dim_c<2>,"inds",ci,int_c); vec3 p0{},p1{}; p0 = verts.pack(dim_c<3>,ptag,edge[0]); p1 = verts.pack(dim_c<3>,ptag,edge[1]); float minv0 = verts("minv",edge[0]); float minv1 = verts("minv",edge[1]); - float r = cquads("r",coffset + gi); + float r = cquads("r",ci); vec3 dp0{},dp1{}; - CONSTRAINT::solve_DistanceConstraint( + if(!CONSTRAINT::solve_DistanceConstraint( p0,minv0, p1,minv1, r, - s, - dt, - lambda, - dp0,dp1); + dp0,dp1)) { - - verts.tuple(dim_c<3>,ptag,edge[0]) = p0 + dp0; - verts.tuple(dim_c<3>,ptag,edge[1]) = p1 + dp1; + for(int i = 0;i != 3;++i) + atomic_add(exec_tag,&dp_buffer[edge[0] * 3 + i],dp0[i] * w); + for(int i = 0;i != 3;++i) + atomic_add(exec_tag,&dp_buffer[edge[1] * 3 + i],dp1[i] * w); - // float m0 = verts("m",edge[0]); - // float m1 = verts("m",edge[1]); - // auto ghost_impulse = (dp0 * m0 + dp1 * m1).norm(); - // if(ghost_impulse > 1e-6) - // printf("dmomentum : %f\n",(float)ghost_impulse); + atomic_add(exec_tag,&dp_count[edge[0]],(int)1); + atomic_add(exec_tag,&dp_count[edge[1]],(int)1); + } } - if(category == ZenoParticles::tri_bending_spring) { - auto quad = cquads.pack(dim_c<4>,"inds",coffset + gi,int_c); + if(category == category_c::isometric_bending_constraint) { + auto quad = cquads.pack(dim_c<4>,"inds",ci,int_c); vec3 p[4] = {}; float minv[4] = {}; for(int i = 0;i != 4;++i) { @@ -706,7 +847,7 @@ struct XPBDSolveSmooth : INode { minv[i] = verts("minv",quad[i]); } - auto Q = cquads.pack(dim_c<4,4>,"Q",coffset + gi); + auto Q = cquads.pack(dim_c<4,4>,"Q",ci); vec3 dp[4] = {}; CONSTRAINT::solve_IsometricBendingConstraint( @@ -714,56 +855,42 @@ struct XPBDSolveSmooth : INode { p[1],minv[1], p[2],minv[2], p[3],minv[3], - Q, - s, - dt, - lambda, - dp[0],dp[1],dp[2],dp[3]); - + Q,dp[0],dp[1],dp[2],dp[3]); for(int i = 0;i != 4;++i) - verts.tuple(dim_c<3>,ptag,quad[i]) = p[i] + dp[i]; + for(int j = 0;j != 3;++j) + atomic_add(exec_tag,&dp_buffer[quad[i] * 3 + j],dp[i][j] * w); + for(int i = 0;i != 4;++i) + atomic_add(exec_tag,&dp_count[quad[1]],(int)1); } - cquads("lambda",coffset + gi) = lambda; }); + } - // total_ghost_impulse_X.setVal(0); - // total_ghost_impulse_Y.setVal(0); - // total_ghost_impulse_Z.setVal(0); - // cudaPol(zs::range(verts.size()),[ - // verts = proxy({},verts), - // ptag = zs::SmallString(ptag), - // exec_tag, - // total_ghost_impulse_X = proxy(total_ghost_impulse_X), - // total_ghost_impulse_Y = proxy(total_ghost_impulse_Y), - // total_ghost_impulse_Z = proxy(total_ghost_impulse_Z), - // pv_buffer = proxy(pv_buffer)] ZS_LAMBDA(int vi) mutable { - // auto cv = verts.pack(dim_c<3>,ptag,vi); - // auto pv = pv_buffer[vi]; - // auto m = verts("m",vi); - // auto dv = m * (cv - pv); - // // for(int i = 0;i != 3;++i) - // atomic_add(exec_tag,&total_ghost_impulse_X[0],dv[0]); - // atomic_add(exec_tag,&total_ghost_impulse_Y[0],dv[1]); - // atomic_add(exec_tag,&total_ghost_impulse_Z[0],dv[2]); - // }); + // zs::Vector avg_smooth{verts.get_allocator(),1}; + // avg_smooth.setVal(0); + // cudaPol(zs::range(dp_buffer),[avg_smooth = proxy(avg_smooth),exec_tag] ZS_LAMBDA(const auto& dp) mutable {atomic_add(exec_tag,&avg_smooth[0],dp);}); + // std::cout << "avg_smooth : " << avg_smooth.getVal(0) << std::endl; - // auto tgi = total_ghost_impulse.getVal(0); - // auto tgx = total_ghost_impulse_X.getVal(0); - // auto tgy = total_ghost_impulse_Y.getVal(0); - // auto tgz = total_ghost_impulse_Z.getVal(0); - // printf("ghost_impulse[%d][%d] : %f %f %f\n",(int)coffset,(int)group_size,(float)tgx,(float)tgy,(float)tgz); - } + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + ptag = zs::SmallString(ptag), + dp_count = proxy(dp_count), + dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { + if(dp_count[vi] > 0) { + auto dp = vec3{dp_buffer[vi * 3 + 0],dp_buffer[vi * 3 + 1],dp_buffer[vi * 3 + 2]}; + verts.tuple(dim_c<3>,ptag,vi) = verts.pack(dim_c<3>,ptag,vi) + dp / (T)dp_count[vi]; + } + }); - set_output("constraints",constraints); + // set_output("all_constraints",all_constraints); set_output("zsparticles",zsparticles); }; }; -ZENDEFNODE(XPBDSolveSmooth, {{{"zsparticles"},{"constraints"},{"float","dt","0.5"}}, - {{"zsparticles"},{"constraints"}}, - {{"string","ptag","X"}}, +ZENDEFNODE(XPBDSolveSmooth, {{{"zsparticles"},{"all_constraints"},{"float","relaxation_strength","1"}}, + {{"zsparticles"}}, + {{"string","ptag","x"}}, {"PBD"}}); -}; +}; \ No newline at end of file diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh index 5b73a871fe..585c24546d 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh @@ -23,7 +23,7 @@ namespace zeno { namespace CONSTRAINT { else { corr0 = VECTOR3d::uniform(0); - corr1 = VECTOR3d::uniform(0);; + corr1 = VECTOR3d::uniform(0); return true; } @@ -39,8 +39,8 @@ namespace zeno { namespace CONSTRAINT { Kinv = static_cast(1.0) / K; else { - corr0 = VECTOR3d::uniform(0);; - corr1 = VECTOR3d::uniform(0);; + corr0 = VECTOR3d::uniform(0); + corr1 = VECTOR3d::uniform(0); return true; } @@ -53,6 +53,32 @@ namespace zeno { namespace CONSTRAINT { return true; } + template + constexpr bool solve_DistanceConstraint( + const VECTOR3d &p0, SCALER invMass0, + const VECTOR3d &p1, SCALER invMass1, + const SCALER expectedDistance, + VECTOR3d &corr0, VECTOR3d &corr1){ + VECTOR3d diff = p0 - p1; + SCALER distance = diff.norm(); + + if (zs::abs((distance - expectedDistance)) > static_cast(1e-5) && (invMass0 + invMass1) > static_cast(1e-5)){ + VECTOR3d gradient = diff / (distance + static_cast(1e-6)); + SCALER denom = invMass0 + invMass1; + SCALER lambda = (distance - expectedDistance) /denom; + auto common = lambda * gradient; + corr0 = -invMass0 * common; + corr1 = invMass1 * common; + return false; + }else{ + corr0 = VECTOR3d::uniform(0); + corr1 = VECTOR3d::uniform(0); + } + + return true; + } + + // ---------------------------------------------------------------------------------------------- template constexpr bool solve_VolumeConstraint( @@ -69,7 +95,7 @@ namespace zeno { namespace CONSTRAINT { constexpr SCALER eps = (SCALER)1e-6; SCALER volume = static_cast(1.0 / 6.0) * (p1 - p0).cross(p2 - p0).dot(p3 - p0); - corr0 = VECTOR3d::uniform(0);; corr1 = VECTOR3d::uniform(0);; corr2 = VECTOR3d::uniform(0);; corr3 = VECTOR3d::uniform(0);; + corr0 = VECTOR3d::uniform(0); corr1 = VECTOR3d::uniform(0); corr2 = VECTOR3d::uniform(0); corr3 = VECTOR3d::uniform(0); VECTOR3d grad0 = (p1 - p2).cross(p3 - p2); VECTOR3d grad1 = (p2 - p0).cross(p3 - p0); @@ -145,7 +171,6 @@ namespace zeno { namespace CONSTRAINT { return true; } - // ---------------------------------------------------------------------------------------------- template constexpr bool solve_IsometricBendingConstraint( @@ -170,10 +195,10 @@ namespace zeno { namespace CONSTRAINT { energy *= 0.5; VECTOR3d gradC[4] = {}; - gradC[0] = VECTOR3d::uniform(0);; - gradC[1] = VECTOR3d::uniform(0);; - gradC[2] = VECTOR3d::uniform(0);; - gradC[3] = VECTOR3d::uniform(0);; + gradC[0] = VECTOR3d::uniform(0); + gradC[1] = VECTOR3d::uniform(0); + gradC[2] = VECTOR3d::uniform(0); + gradC[3] = VECTOR3d::uniform(0); for (unsigned char k = 0; k < 4; k++) for (unsigned char j = 0; j < 4; j++) gradC[j] += Q(j, k) * *x[k]; @@ -212,6 +237,98 @@ namespace zeno { namespace CONSTRAINT { } + template + constexpr bool solve_IsometricBendingConstraint( + const VECTOR3d& p0, SCALER invMass0, + const VECTOR3d& p1, SCALER invMass1, + const VECTOR3d& p2, SCALER invMass2, + const VECTOR3d& p3, SCALER invMass3, + const MATRIX4d& Q, + VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3){ + constexpr SCALER eps = (SCALER)1e-6; + const VECTOR3d* x[4] = { &p2, &p3, &p0, &p1 }; + SCALER invMass[4] = { invMass2, invMass3, invMass0, invMass1 }; + + SCALER energy = 0.0; + for (unsigned char k = 0; k < 4; k++) + for (unsigned char j = 0; j < 4; j++) + energy += Q(j, k) * (x[k]->dot(*x[j])); + energy *= 0.5; + + VECTOR3d gradC[4] = {}; + gradC[0] = VECTOR3d::uniform(0); + gradC[1] = VECTOR3d::uniform(0); + gradC[2] = VECTOR3d::uniform(0); + gradC[3] = VECTOR3d::uniform(0); + for (unsigned char k = 0; k < 4; k++) + for (unsigned char j = 0; j < 4; j++) + gradC[j] += Q(j, k) * *x[k]; + + + SCALER sum_normGradC = 0.0; + for (unsigned int j = 0; j < 4; j++) + { + // compute sum of squared gradient norms + if (invMass[j] != 0.0) + sum_normGradC += invMass[j] * gradC[j].l2NormSqr(); + } + + // exit early if required + if (zs::abs(sum_normGradC) > eps) + { + // compute impulse-based scaling factor + const SCALER s = -(energy) / sum_normGradC; + + corr0 = (s * invMass[2]) * gradC[2]; + corr1 = (s * invMass[3]) * gradC[3]; + corr2 = (s * invMass[0]) * gradC[0]; + corr3 = (s * invMass[1]) * gradC[1]; + + return true; + } + return false; + } + + template + constexpr bool solve_PlaneConstraint( + const VECTOR3d& p, SCALER invMass, + const VECTOR3d& root, + const VECTOR3d& nrm, + const SCALER& thickness, + const SCALER& stiffness, + const SCALER& dt, + SCALER& lambda, + VECTOR3d& dp) { + SCALER C = (p - root).dot(nrm) - thickness; + if(C > static_cast(1e-6)) { + dp = VECTOR3d::uniform(0); + return true; + } + + SCALER K = invMass * nrm.l2NormSqr(); + + SCALER alpha = 0.0; + if(stiffness != 0.0) { + alpha = static_cast(1.0) / (stiffness * dt * dt); + K += alpha; + } + + SCALER Kinv = 0.0; + if(zs::abs(K) > static_cast(1e-6)) + Kinv = static_cast(1.0) / K; + else + { + dp = VECTOR3d::uniform(0); + return true; + } + + const SCALER delta_lambda = -Kinv * (C + alpha * lambda); + lambda += delta_lambda; + const VECTOR3d pt = nrm * delta_lambda; + + dp = invMass * pt; + return true; + } // ---------------------------------------------------------------------------------------------- template @@ -582,7 +699,7 @@ namespace zeno { namespace CONSTRAINT { // bendingAndTwistingKs[2] * (darboux_vector[2] - restDarbouxVector[2])); // MATRIX3d factor_matrix; -// factor_matrix = VECTOR3d::uniform(0);; +// factor_matrix = VECTOR3d::uniform(0); // MATRIX3d tmp_mat; // SCALER invMasses[]{ invMass0, invMass1, invMass2, invMass3, invMass4 }; @@ -683,9 +800,9 @@ namespace zeno { namespace CONSTRAINT { // d3p1.col(1) = -d3p0.col(1); // d3p1.col(2) = -d3p0.col(2); -// d3p2.col(0) = VECTOR3d::uniform(0);; -// d3p2.col(1) = VECTOR3d::uniform(0);; -// d3p2.col(2) = VECTOR3d::uniform(0);; +// d3p2.col(0) = VECTOR3d::uniform(0); +// d3p2.col(1) = VECTOR3d::uniform(0); +// d3p2.col(2) = VECTOR3d::uniform(0); // ////////////////////////////////////////////////////////////////////////// // // d2pi @@ -859,4 +976,4 @@ namespace zeno { namespace CONSTRAINT { // } }; -}; +}; \ No newline at end of file diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp b/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp new file mode 100644 index 0000000000..04231db1d4 --- /dev/null +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp @@ -0,0 +1,18 @@ +#pragma once + +namespace zeno { namespace PBD_CONSTRAINT { + +constexpr auto CONSTRAINT_KEY = "XPBD_CONSTRAINT"; + +enum category_c : int { + edge_length_constraint, + isometric_bending_constraint, + p_kp_collision_constraint, + p_p_collision_constraint, + vert_bending_spring, + tri_bending_spring, + bending +}; + +}; +}; \ No newline at end of file From a26f950d935e5f439826ae8f2953f0984a582935 Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Fri, 11 Aug 2023 19:35:45 +0800 Subject: [PATCH 09/21] xpbd detangle --- .../CuLagrange/geometry/kernel/geo_math.hpp | 12 +- projects/CuLagrange/pbd/Pipeline.cu | 634 +++++++++++++++++- 2 files changed, 634 insertions(+), 12 deletions(-) diff --git a/projects/CuLagrange/geometry/kernel/geo_math.hpp b/projects/CuLagrange/geometry/kernel/geo_math.hpp index a8f89dbeb9..c467f91de8 100644 --- a/projects/CuLagrange/geometry/kernel/geo_math.hpp +++ b/projects/CuLagrange/geometry/kernel/geo_math.hpp @@ -843,10 +843,10 @@ constexpr REAL pointTriangleDistance(const VECTOR3& v0, const VECTOR3& v1, auto x = x1 - x0; auto v = v1 - v0; - if(x.norm() < thickness) - return (REAL)0; - if(x.dot(v) > 0) + if(x.norm() < thickness && x.dot(v) > 0) return std::numeric_limits::infinity(); + if(x.norm() < thickness && x.dot(v) < 0) + return (REAL)0; // if(vv.norm() < 1e) auto xx = x.dot(x); @@ -862,11 +862,11 @@ constexpr REAL pointTriangleDistance(const VECTOR3& v0, const VECTOR3& v1, return std::numeric_limits::infinity(); auto sqrt_delta = zs::sqrt(delta); - auto alpha = xv/vv; + auto alpha = xv / vv; auto beta = sqrt_delta / 2 / vv; - auto t0 = -alpha + beta; - auto t1 = -alpha - beta; + auto t0 = alpha + beta; + auto t1 = alpha - beta; t0 = t0 < (REAL)1.0 && t0 > (REAL)0.0 ? t0 : std::numeric_limits::infinity(); t1 = t1 < (REAL)1.0 && t1 > (REAL)0.0 ? t1 : std::numeric_limits::infinity(); diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index 03b3f60d09..54b1521452 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -433,7 +433,13 @@ struct XPBDSolve : INode { // std::cout << "SOVLE CONSTRAINT WITH GROUP : " << nm_group << "\t" << cquads.size() << std::endl; + + for(int g = 0;g != nm_group;++g) { + + // if(category == category_c::isometric_bending_constraint) + // break; + auto coffset = coffsets.getVal(g); int group_size = 0; if(g == nm_group - 1) @@ -634,6 +640,9 @@ struct SDFColliderProject : INode { center, // do_stablize, cv,w] ZS_LAMBDA(int vi) mutable { + if(verts("minv",vi) < (T)1e-6) + return; + auto pred = verts.pack(dim_c<3>,ptag,vi); auto pos = verts.pack(dim_c<3>,xtag,vi); @@ -673,6 +682,8 @@ struct SDFColliderProject : INode { pred += dp; } + // dp = dp * verts("m",vi) * verts("minv",vi); + verts.tuple(dim_c<3>,ptag,vi) = pred; }); set_output("zsparticles",zsparticles); @@ -750,7 +761,615 @@ ZENDEFNODE(BuildZSLBvhFromAABB, {{{"ZSParticles"}, }, {"ZSLBvh"}, {}, {"XPBD"}}); +struct XPBDDetangle : INode { + // ray trace bvh + template + void buildRayTraceBvh(zs::CudaExecutionPolicy &pol, + TileVecT &verts, + const zs::SmallString& srcTag, + const zs::SmallString& dstTag, + const zs::SmallString& pscaleTag, + ZenoLinearBvh::lbvh_t &bvh) { + using namespace zs; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto space = execspace_e::cuda; + Vector bvs{verts.get_allocator(), verts.size()}; + pol(range(verts.size()), + [verts = proxy({}, verts), + bvs = proxy(bvs), + pscaleTag,srcTag,dstTag] ZS_LAMBDA(int vi) mutable { + auto src = verts.template pack<3>(srcTag, vi); + auto dst = verts.template pack<3>(dstTag, vi); + auto pscale = verts(pscaleTag,vi); + + bv_t bv{src,dst}; + bv._min -= pscale; + bv._max += pscale; + bvs[vi] = bv; + }); + bvh.build(pol, bvs); + } + // particle sphere bvh + template + void buildBvh(zs::CudaExecutionPolicy &pol, + TileVecT &verts, + const zs::SmallString& xtag, + const zs::SmallString& pscaleTag, + ZenoLinearBvh::lbvh_t& bvh) { + using namespace zs; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto space = execspace_e::cuda; + Vector bvs{verts.get_allocator(), verts.size()}; + + pol(range(verts.size()), + [verts = proxy({}, verts), + bvs = proxy(bvs), + pscaleTag,xtag] ZS_LAMBDA(int vi) mutable { + auto pos = verts.template pack<3>(xtag, vi); + auto pscale = verts(pscaleTag,vi); + bv_t bv{pos - pscale,pos + pscale}; + bvs[vi] = bv; + }); + bvh.build(pol, bvs); + } + + virtual void apply() override { + using namespace zs; + using bvh_t = typename ZenoLinearBvh::lbvh_t; + using bv_t = typename bvh_t::Box; + + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + + auto zsparticles = get_input("zsparticles"); + auto xtag = get_input2("xtag"); + auto pxtag = get_input2("pxtag"); + auto Xtag = get_input2("Xtag"); + // auto ccdTag = get_input2("ccdTag"); + // x + auto pscaleTag = get_input2("pscaleTag"); + // auto friction = get_input2("friction"); + + auto& verts = zsparticles->getParticles(); + + auto spBvh = bvh_t{}; + buildRayTraceBvh(cudaPol,verts,pxtag,xtag,pscaleTag,spBvh); + + zs::Vector ccd_buffer{verts.get_allocator(),verts.size()}; + cudaPol(zs::range(ccd_buffer),[]ZS_LAMBDA(auto& t) mutable {t = (float)1;}); + // zs::Vector dp_count{verts.get_allocator(),verts.size()}; + // cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + Xtag = zs::SmallString(Xtag), + pxtag = zs::SmallString(pxtag), + exec_tag, + ccd_buffer = proxy(ccd_buffer), + pscaleTag = zs::SmallString(pscaleTag), + spBvh = proxy(spBvh)] ZS_LAMBDA(int vi) mutable { + auto dst = verts.pack(dim_c<3>,xtag,vi); + auto src = verts.pack(dim_c<3>,pxtag,vi); + auto vel = dst - src; + auto rpos = verts.pack(dim_c<3>,Xtag,vi); + // auto r = verts(ptag,vi); + + auto pscale = verts(pscaleTag,vi); + bv_t bv{src,dst}; + bv._min -= pscale; + bv._max += pscale; + + auto find_particle_sphere_collision_pairs = [&] (int nvi) mutable { + if(vi >= nvi) + return; + + auto npscale = verts(pscaleTag,nvi); + auto thickness = pscale + npscale; + + auto nrpos = verts.pack(dim_c<3>,Xtag,nvi); + auto rdist = (rpos - nrpos).norm(); + if(rdist < thickness) + return; + + auto ndst = verts.pack(dim_c<3>,xtag,nvi); + auto nsrc = verts.pack(dim_c<3>,pxtag,nvi); + auto nvel = ndst - nsrc; + + auto t = LSL_GEO::ray_ray_intersect(src,vel,nsrc,nvel,thickness); + if(t <= (float)1 && t >= (float)0) { + atomic_min(exec_tag,&ccd_buffer[vi],t); + atomic_min(exec_tag,&ccd_buffer[nvi],t); + } + }; + + spBvh.iter_neighbors(bv,find_particle_sphere_collision_pairs); + }); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + pxtag = zs::SmallString(pxtag), + // friction, + ccd_buffer = proxy(ccd_buffer)] ZS_LAMBDA(int vi) mutable { + auto src = verts.pack(dim_c<3>,pxtag,vi); + auto dst = verts.pack(dim_c<3>,xtag,vi); + + auto vel = dst - src; + auto project = src + vel * ccd_buffer[vi]; + + verts.tuple(dim_c<3>,xtag,vi) = project; + }); + + set_output("zsparticles",zsparticles); + } + +}; + +ZENDEFNODE(XPBDDetangle, {{{"zsparticles"}, + // {"float","relaxation_strength","1"}, + {"string","xtag","x"}, + {"string","pxtag","px"}, + {"string","Xtag","X"}, + {"string","pscaleTag","pscale"} + // {"float","friction","0"} + }, + {{"zsparticles"}}, + { + // {"string","ptag","x"} + }, + {"PBD"}}); + +struct XPBDDetangle2 : INode { + // ray trace bvh + template + void buildRayTraceBvh(zs::CudaExecutionPolicy &pol, + TileVecT &verts, + const zs::SmallString& srcTag, + const zs::SmallString& dstTag, + const zs::SmallString& pscaleTag, + ZenoLinearBvh::lbvh_t &bvh) { + using namespace zs; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto space = execspace_e::cuda; + Vector bvs{verts.get_allocator(), verts.size()}; + pol(range(verts.size()), + [verts = proxy({}, verts), + bvs = proxy(bvs), + pscaleTag,srcTag,dstTag] ZS_LAMBDA(int vi) mutable { + auto src = verts.template pack<3>(srcTag, vi); + auto dst = verts.template pack<3>(dstTag, vi); + auto pscale = verts(pscaleTag,vi); + + bv_t bv{src,dst}; + bv._min -= pscale; + bv._max += pscale; + bvs[vi] = bv; + }); + bvh.build(pol, bvs); + } + // particle sphere bvh + // template + // void buildBvh(zs::CudaExecutionPolicy &pol, + // TileVecT &verts, + // const zs::SmallString& xtag, + // const zs::SmallString& pscaleTag, + // ZenoLinearBvh::lbvh_t& bvh) { + // using namespace zs; + // using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + // constexpr auto space = execspace_e::cuda; + // Vector bvs{verts.get_allocator(), verts.size()}; + + // pol(range(verts.size()), + // [verts = proxy({}, verts), + // bvs = proxy(bvs), + // pscaleTag,xtag] ZS_LAMBDA(int vi) mutable { + // auto pos = verts.template pack<3>(xtag, vi); + // auto pscale = verts(pscaleTag,vi); + // bv_t bv{pos - pscale,pos + pscale}; + // bvs[vi] = bv; + // }); + // bvh.build(pol, bvs); + // } + + virtual void apply() override { + using namespace zs; + using bvh_t = typename ZenoLinearBvh::lbvh_t; + using bv_t = typename bvh_t::Box; + + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + constexpr auto eps = (T)1e-7; + + auto zsparticles = get_input("zsparticles"); + auto xtag = get_input2("xtag"); + auto pxtag = get_input2("pxtag"); + auto Xtag = get_input2("Xtag"); + // auto ccdTag = get_input2("ccdTag"); + // x + auto pscaleTag = get_input2("pscaleTag"); + // auto friction = get_input2("friction"); + + auto& verts = zsparticles->getParticles(); + + auto restitution_rate = get_input2("restitution"); + auto relaxation_rate = get_input2("relaxation"); + + auto spBvh = bvh_t{}; + buildRayTraceBvh(cudaPol,verts,pxtag,xtag,pscaleTag,spBvh); + + // zs::Vector ccd_buffer{verts.get_allocator(),verts.size()}; + // cudaPol(zs::range(ccd_buffer),[]ZS_LAMBDA(auto& t) mutable {t = (float)1;}); + + zs::Vector dp_buffer{verts.get_allocator(),verts.size()}; + cudaPol(zs::range(dp_buffer),[]ZS_LAMBDA(auto& dp) mutable {dp = vec3::uniform(0);}); + + zs::Vector dp_count{verts.get_allocator(),verts.size()}; + cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + Xtag = zs::SmallString(Xtag), + pxtag = zs::SmallString(pxtag), + restitution_rate = restitution_rate, + exec_tag, + eps = eps, + dp_buffer = proxy(dp_buffer), + dp_count = proxy(dp_count), + pscaleTag = zs::SmallString(pscaleTag), + spBvh = proxy(spBvh)] ZS_LAMBDA(int vi) mutable { + auto dst = verts.pack(dim_c<3>,xtag,vi); + auto src = verts.pack(dim_c<3>,pxtag,vi); + auto vel = dst - src; + auto rpos = verts.pack(dim_c<3>,Xtag,vi); + // auto r = verts(ptag,vi); + + auto pscale = verts(pscaleTag,vi); + bv_t bv{src,dst}; + bv._min -= pscale; + bv._max += pscale; + + auto find_particle_sphere_collision_pairs = [&] (int nvi) mutable { + if(vi >= nvi) + return; + + auto npscale = verts(pscaleTag,nvi); + auto thickness = pscale + npscale; + + auto nrpos = verts.pack(dim_c<3>,Xtag,nvi); + auto rdist = (rpos - nrpos).norm(); + if(rdist < thickness) + return; + + auto ndst = verts.pack(dim_c<3>,xtag,nvi); + auto nsrc = verts.pack(dim_c<3>,pxtag,nvi); + auto nvel = ndst - nsrc; + + auto t = LSL_GEO::ray_ray_intersect(src,vel,nsrc,nvel,thickness); + if(t <= (float)1 && t >= (float)0) { + // atomic_min(exec_tag,&ccd_buffer[vi],t); + // atomic_min(exec_tag,&ccd_buffer[nvi],t); + // auto rel_vel = vel - nvel; + auto CR = restitution_rate; + + auto minv_a = verts("minv",vi); + auto minv_b = verts("minv",nvi); + if(minv_a + minv_b < (T)2 * eps) + return; + + auto ua = vel; + auto ub = nvel; + auto ur = ua - ub; + + vec3 va{},vb{}; + + if(minv_a > (T)eps) { + // ma / mb + auto ratio = minv_b / minv_a; + auto m = ratio + (T)1; + auto momt = ratio * ua + ub; + va = (momt - CR * ur) / m; + vb = (momt + CR * ratio * ur) / m; + + if(isnan(va.norm()) || isnan(vb.norm())) { + printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); + } + }else if(minv_b > (T)eps) { + // mb / ma + auto ratio = minv_a / minv_b; + auto m = ratio + (T)1; + auto momt = ua + ratio * ub; + va = (momt - CR * ratio * ur) / m; + vb = (momt + CR * ur) / m; + if(isnan(va.norm()) || isnan(vb.norm())) { + printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); + } + + }else { + printf("impossible reaching here\n"); + } + + // auto ma = verts("m",vi); + // auto mb = verts("m",nvi); + + // auto m = ma + mb; + // auto momt = ma * ua + mb * ub; + + + // auto va = (momt - CR * mb * ur) / m; + // auto vb = (momt + CR * ma * ur) / m; + + auto dpa = (va - ua) * (1 - t); + auto dpb = (vb - ub) * (1 - t); + + // auto dpa = (va - ua); + // auto dpb = (vb - ub); + // printf("find collision pair : %d %d\n",vi,nvi); + atomic_add(exec_tag,&dp_count[vi],1); + atomic_add(exec_tag,&dp_count[nvi],1); + + for(int i = 0;i != 3;++i) { + atomic_add(exec_tag,&dp_buffer[vi][i],dpa[i]); + atomic_add(exec_tag,&dp_buffer[nvi][i],dpb[i]); + } + } + }; + + spBvh.iter_neighbors(bv,find_particle_sphere_collision_pairs); + }); + + // zs:Vector dpnorm{verts.get_allocator(),1}; + // dpnorm.setVal(0); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + relaxation_rate = relaxation_rate, + pxtag = zs::SmallString(pxtag), + exec_tag, + // dpnorm = proxy(dpnorm), + dp_count = proxy(dp_count), + dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { + if(dp_count[vi] == 0) + return; + auto minv = verts("minv",vi); + auto dp = dp_buffer[vi] * relaxation_rate / (T)dp_count[vi]; + // atomic_add(exec_tag,&dpnorm[0],dp.norm()); + verts.tuple(dim_c<3>,xtag,vi) = verts.pack(dim_c<3>,xtag,vi) + dp; + }); + + // std::cout << "detangle_dp_norm : " << dpnorm.getVal(0) << std::endl; + + set_output("zsparticles",zsparticles); + } + +}; + +ZENDEFNODE(XPBDDetangle2, {{{"zsparticles"}, + // {"float","relaxation_strength","1"}, + {"string","xtag","x"}, + {"string","pxtag","px"}, + {"string","Xtag","X"}, + {"string","pscaleTag","pscale"}, + {"float","restitution","0.1"}, + {"float","relaxation","1"}, + // {"float","friction","0"} + }, + {{"zsparticles"}}, + { + // {"string","ptag","x"} + }, + {"PBD"}}); + +struct XPBDParticlesCollider : INode { + // particle sphere bvh + template + void buildBvh(zs::CudaExecutionPolicy &pol, + TileVecT &verts, + const zs::SmallString& xtag, + const zs::SmallString& pscaleTag, + ZenoLinearBvh::lbvh_t& bvh) { + using namespace zs; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto space = execspace_e::cuda; + Vector bvs{verts.get_allocator(), verts.size()}; + + pol(range(verts.size()), + [verts = proxy({}, verts), + bvs = proxy(bvs), + pscaleTag,xtag] ZS_LAMBDA(int vi) mutable { + auto pos = verts.template pack<3>(xtag, vi); + auto pscale = verts(pscaleTag,vi); + bv_t bv{pos - pscale,pos + pscale}; + bvs[vi] = bv; + }); + bvh.build(pol, bvs); + } + + virtual void apply() override { + using namespace zs; + using bvh_t = typename ZenoLinearBvh::lbvh_t; + using bv_t = typename bvh_t::Box; + + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + + auto zsparticles = get_input("zsparticles"); + // auto bvh = get_input("lbvh"); + // prex + auto xtag = get_input2("xtag"); + auto pxtag = get_input2("pxtag"); + auto Xtag = get_input2("Xtag"); + // x + auto ptag = get_input2("pscaleTag"); + auto friction = get_input2("friction"); + + auto& verts = zsparticles->getParticles(); + + auto spBvh = bvh_t{}; + buildBvh(cudaPol,verts,xtag,ptag,spBvh); + + // auto collisionThickness = + + zs::Vector dp_buffer{verts.get_allocator(),verts.size() * 3}; + cudaPol(zs::range(dp_buffer),[]ZS_LAMBDA(auto& v) mutable {v = 0;}); + zs::Vector dp_count{verts.get_allocator(),verts.size()}; + cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + + // find collision pairs + zs::bht csPT{verts.get_allocator(),100000}; + csPT.reset(cudaPol,true); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + pxtag = zs::SmallString(pxtag), + Xtag = zs::SmallString(Xtag), + ptag = zs::SmallString(ptag), + spBvh = proxy(spBvh), + csPT = proxy(csPT)] ZS_LAMBDA(int vi) mutable { + auto pos = verts.pack(dim_c<3>,xtag,vi); + auto ppos = verts.pack(dim_c<3>,pxtag,vi); + auto vel = pos - ppos; + auto rpos = verts.pack(dim_c<3>,Xtag,vi); + auto r = verts(ptag,vi); + + auto bv = bv_t{pos - r,pos + r}; + + auto find_particle_sphere_collision_pairs = [&] (int nvi) { + if(vi >= nvi) + return; + + auto npos = verts.pack(dim_c<3>,xtag,nvi); + // auto nppos = verts.pack(dim_c<3>,pxtag,nvi); + // auto nvel = npos - nppos; + + auto nrpos = verts.pack(dim_c<3>,Xtag,nvi); + auto nr = verts(ptag,nvi); + + auto rdist = (rpos - nrpos).norm(); + if(rdist < r + nr) + return; + + auto dist = (pos - npos).norm(); + if(dist > r + nr) + return; + + // find a collision pairs + csPT.insert(zs::vec{vi,nvi}); + }; + + spBvh.iter_neighbors(bv,find_particle_sphere_collision_pairs); + }); + + // std::cout << "csPT.size() = " << csPT.size() << std::endl; + + auto w = get_input2("relaxation_strength"); + // process collision pairs + cudaPol(zip(zs::range(csPT.size()),csPT._activeKeys),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + ptag = zs::SmallString(ptag), + pxtag = zs::SmallString(pxtag), + dp_buffer = proxy(dp_buffer), + dp_count = proxy(dp_count), + exec_tag, + friction] ZS_LAMBDA(auto,const auto& pair) mutable { + auto vi = pair[0]; + auto nvi = pair[1]; + auto p0 = verts.pack(dim_c<3>,xtag,vi); + auto p1 = verts.pack(dim_c<3>,xtag,nvi); + auto vel0 = p0 - verts.pack(dim_c<3>,pxtag,vi); + auto vel1 = p1 - verts.pack(dim_c<3>,pxtag,nvi); + + auto nrm = (p0 - p1).normalized(); + + auto r = verts(ptag,vi) + verts(ptag,nvi); + + float minv0 = verts("minv",vi); + float minv1 = verts("minv",nvi); + + vec3 dp0{},dp1{}; + if(!CONSTRAINT::solve_DistanceConstraint( + p0,minv0, + p1,minv1, + r, + dp0,dp1)) { + // auto rel_vel = vel0 - vel1 + dp0 - dp1; + // auto tan_vel = rel_vel - nrm * rel_vel.dot(nrm); + // auto tan_len = tan_vel.norm(); + // auto max_tan_len = (dp0 - dp1).norm() * friction; + + // if(tan_len > (T)1e-6) { + // auto ratio = (T)max_tan_len / (T)tan_len; + // auto alpha = zs::min(ratio,(T)1.0); + + // dp = -tan_vel * zs::min(alpha,(T)1.0); + // pred += dp; + // } + + for(int i = 0;i != 3;++i) + atomic_add(exec_tag,&dp_buffer[vi * 3 + i], dp0[i]); + for(int i = 0;i != 3;++i) + atomic_add(exec_tag,&dp_buffer[nvi * 3 + i],dp1[i]); + + atomic_add(exec_tag,&dp_count[vi],(int)1); + atomic_add(exec_tag,&dp_count[nvi],(int)1); + } + }); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + w, + dp_count = proxy(dp_count), + dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { + if(dp_count[vi] > 0) { + auto dp = vec3{dp_buffer[vi * 3 + 0],dp_buffer[vi * 3 + 1],dp_buffer[vi * 3 + 2]}; + verts.tuple(dim_c<3>,xtag,vi) = verts.pack(dim_c<3>,xtag,vi) + w * dp / (T)dp_count[vi]; + } + }); + + set_output("zsparticles",zsparticles); + } +}; + +ZENDEFNODE(XPBDParticlesCollider, {{{"zsparticles"}, + {"float","relaxation_strength","1"}, + {"string","xtag","x"}, + {"string","pxtag","px"}, + {"string","Xtag","X"}, + {"string","pscaleTag","pscale"}, + {"float","friction","0"} + }, + {{"zsparticles"}}, + { + // {"string","ptag","x"} + }, + {"PBD"}}); struct XPBDSolveSmooth : INode { @@ -804,7 +1423,7 @@ struct XPBDSolveSmooth : INode { verts = proxy({},verts), category, // dt, - w, + // w, exec_tag, dp_buffer = proxy(dp_buffer), dp_count = proxy(dp_count), @@ -830,9 +1449,9 @@ struct XPBDSolveSmooth : INode { dp0,dp1)) { for(int i = 0;i != 3;++i) - atomic_add(exec_tag,&dp_buffer[edge[0] * 3 + i],dp0[i] * w); + atomic_add(exec_tag,&dp_buffer[edge[0] * 3 + i],dp0[i]); for(int i = 0;i != 3;++i) - atomic_add(exec_tag,&dp_buffer[edge[1] * 3 + i],dp1[i] * w); + atomic_add(exec_tag,&dp_buffer[edge[1] * 3 + i],dp1[i]); atomic_add(exec_tag,&dp_count[edge[0]],(int)1); atomic_add(exec_tag,&dp_count[edge[1]],(int)1); @@ -858,7 +1477,7 @@ struct XPBDSolveSmooth : INode { Q,dp[0],dp[1],dp[2],dp[3]); for(int i = 0;i != 4;++i) for(int j = 0;j != 3;++j) - atomic_add(exec_tag,&dp_buffer[quad[i] * 3 + j],dp[i][j] * w); + atomic_add(exec_tag,&dp_buffer[quad[i] * 3 + j],dp[i][j]); for(int i = 0;i != 4;++i) atomic_add(exec_tag,&dp_count[quad[1]],(int)1); } @@ -872,11 +1491,11 @@ struct XPBDSolveSmooth : INode { cudaPol(zs::range(verts.size()),[ verts = proxy({},verts), - ptag = zs::SmallString(ptag), + ptag = zs::SmallString(ptag),w, dp_count = proxy(dp_count), dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { if(dp_count[vi] > 0) { - auto dp = vec3{dp_buffer[vi * 3 + 0],dp_buffer[vi * 3 + 1],dp_buffer[vi * 3 + 2]}; + auto dp = w * vec3{dp_buffer[vi * 3 + 0],dp_buffer[vi * 3 + 1],dp_buffer[vi * 3 + 2]}; verts.tuple(dim_c<3>,ptag,vi) = verts.pack(dim_c<3>,ptag,vi) + dp / (T)dp_count[vi]; } }); @@ -893,4 +1512,7 @@ ZENDEFNODE(XPBDSolveSmooth, {{{"zsparticles"},{"all_constraints"},{"float","rela + + + }; \ No newline at end of file From 486e1091d866671c1075c42dfcf9f253259a7464 Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Thu, 17 Aug 2023 22:00:58 +0800 Subject: [PATCH 10/21] fix bending energy and PP ccd detangle --- .../CuLagrange/geometry/kernel/geo_math.hpp | 9 +++ .../CuLagrange/geometry/kernel/topology.hpp | 15 +++-- projects/CuLagrange/pbd/Pipeline.cu | 60 ++++++++++++----- .../constraint_function_kernel/constraint.cuh | 65 ++++++++++++------- 4 files changed, 104 insertions(+), 45 deletions(-) diff --git a/projects/CuLagrange/geometry/kernel/geo_math.hpp b/projects/CuLagrange/geometry/kernel/geo_math.hpp index c467f91de8..ac800562b5 100644 --- a/projects/CuLagrange/geometry/kernel/geo_math.hpp +++ b/projects/CuLagrange/geometry/kernel/geo_math.hpp @@ -50,6 +50,15 @@ namespace zeno { namespace LSL_GEO { return (costheta / sintheta); } + template 1)> = 0,typename REAL = VecT::value_type> + constexpr auto cotTheta(const zs::VecInterface& e0,const zs::VecInterface& e1){ + auto de0 = e0.cast(); + auto de1 = e1.cast(); + auto costheta = e0.dot(e1); + auto sintheta = e0.cross(e1).norm(); + return (REAL)(costheta / sintheta); + } + template 1)> = 0> constexpr int orient(const zs::VecInterface& p0, const zs::VecInterface& p1, diff --git a/projects/CuLagrange/geometry/kernel/topology.hpp b/projects/CuLagrange/geometry/kernel/topology.hpp index 0fbd4c2038..4b380566b2 100644 --- a/projects/CuLagrange/geometry/kernel/topology.hpp +++ b/projects/CuLagrange/geometry/kernel/topology.hpp @@ -1256,18 +1256,22 @@ namespace zeno { constexpr auto space = Pol::exec_tag::value; using Ti = RM_CVREF_T(colors[0]); + std::cout << "do coloring " << std::endl; + colors.resize(topo.size()); zs::SparseMatrix topo_incidence_matrix{topo.get_allocator(),(int)topo.size(),(int)topo.size()}; - // std::cout << "compute incidence matrix " << std::endl; + std::cout << "compute incidence matrix " << std::endl; + + topological_incidence_matrix(pol,topo,topo_incidence_matrix); - // std::cout << "finish compute incidence matrix " << std::endl; + std::cout << "finish compute incidence matrix " << std::endl; auto ompPol = omp_exec(); constexpr auto omp_space = execspace_e::openmp; zs::Vector weights(/*topo.get_allocator(),*/topo.size()); { - bht tab{weights.get_allocator(),topo.size() * 2}; + bht tab{weights.get_allocator(),topo.size() * 100}; tab.reset(ompPol, true); ompPol(enumerate(weights), [tab1 = proxy(tab)] (int seed, u32 &w) mutable { using tab_t = RM_CVREF_T(tab); @@ -1285,7 +1289,7 @@ namespace zeno { weights = weights.clone(colors.memoryLocation()); // for(int i = 0;i != weights.size();++i) // printf("w[%d] : %u\n",i,weights.getVal(i)); - + std::cout << "do maximum set " << std::endl; auto iterRef = maximum_independent_sets(pol, topo_incidence_matrix, weights, colors); std::cout << "nm_colors : " << iterRef << std::endl; pol(zs::range(colors),[] ZS_LAMBDA(auto& clr) mutable {clr = clr - (Ti)1;}); @@ -1406,7 +1410,8 @@ namespace zeno { auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); auto otri = tris.pack(dim_c<3>,"inds",oti,int_c); - tb_topos[id] = zs::vec(tri[(vid + 0) % 3],tri[(vid + 1) % 3],tri[(vid + 2) % 3],otri[(ovid + 2) % 3]); + // tb_topos[id] = zs::vec(tri[(vid + 0) % 3],tri[(vid + 1) % 3],tri[(vid + 2) % 3],otri[(ovid + 2) % 3]); + tb_topos[id] = zs::vec(tri[(vid + 2) % 3],otri[(ovid + 2) % 3],tri[(vid + 1) % 3],tri[(vid + 0) % 3]); }); } diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index 54b1521452..f5aed55511 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -158,11 +158,17 @@ struct MakeSurfaceConstraintTopology : INode { reordered_map = proxy(reordered_map), verts = proxy({},verts)] ZS_LAMBDA(int oei) mutable { auto ei = reordered_map[oei]; + printf("bd_topos[%d] : %d %d %d %d\n",ei,bd_topos[ei][0],bd_topos[ei][1],bd_topos[ei][2],bd_topos[ei][3]); eles.tuple(dim_c<4>,"inds",oei) = bd_topos[ei].reinterpret_bits(float_c); vec3 x[4] = {}; for(int i = 0;i != 4;++i) x[i] = verts.pack(dim_c<3>,"x",bd_topos[ei][i]); + mat4 Q = mat4::uniform(0); + { + // CONSTRAINT::init_IsometricBendingConstraint(x[0],x[1],x[2],x[3],Q); + } + { auto e0 = x[1] - x[0]; auto e1 = x[2] - x[0]; auto e2 = x[3] - x[0]; @@ -181,7 +187,7 @@ struct MakeSurfaceConstraintTopology : INode { float K[4] = { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 }; float K2[4] = { coef * K[0], coef * K[1], coef * K[2], coef * K[3] }; - mat4 Q = mat4::uniform(0); + for (unsigned char j = 0; j < 4; j++) { @@ -191,6 +197,10 @@ struct MakeSurfaceConstraintTopology : INode { } Q(j, j) = K[j] * K2[j]; } + } + + + eles.tuple(dim_c<16>,"Q",oei) = Q; }); @@ -374,8 +384,8 @@ struct VisualizePBDConstraint : INode { for(int i = 0;i != 4;++i) cverts[i] = cvis[ci * 4 + i].to_array(); - pverts[ci * 2 + 0] = (cverts[0] + cverts[1] + cverts[2]) / (T)3.0; - pverts[ci * 2 + 1] = (cverts[0] + cverts[1] + cverts[3]) / (T)3.0; + pverts[ci * 2 + 0] = (cverts[0] + cverts[2] + cverts[3]) / (T)3.0; + pverts[ci * 2 + 1] = (cverts[1] + cverts[2] + cverts[3]) / (T)3.0; tclrs[ci * 2 + 0] = cclrs[ci]; tclrs[ci * 2 + 1] = cclrs[ci]; ltclrs[ci] = cclrs[ci]; @@ -519,8 +529,14 @@ struct XPBDSolve : INode { lambda, dp[0],dp[1],dp[2],dp[3]); - for(int i = 0;i != 4;++i) + + + for(int i = 0;i != 4;++i) { + // printf("dp[%d][%d] : %f %f %f %f\n",gi,i,s,(float)dp[i][0],(float)dp[i][1],(float)dp[i][2]); verts.tuple(dim_c<3>,ptag,quad[i]) = p[i] + dp[i]; + } + + } if(category == category_c::p_kp_collision_constraint) { @@ -549,22 +565,29 @@ struct XPBDSolve : INode { // total_ghost_impulse_X.setVal(0); // total_ghost_impulse_Y.setVal(0); // total_ghost_impulse_Z.setVal(0); + if(category == category_c::isometric_bending_constraint) + std::cout << "isometric_bending_constraint output" << std::endl; + if(category == category_c::edge_length_constraint) + std::cout << "edge_length_constraint output" << std::endl; + // cudaPol(zs::range(verts.size()),[ // verts = proxy({},verts), - // ptag = zs::SmallString(ptag), - // exec_tag, - // total_ghost_impulse_X = proxy(total_ghost_impulse_X), - // total_ghost_impulse_Y = proxy(total_ghost_impulse_Y), - // total_ghost_impulse_Z = proxy(total_ghost_impulse_Z), - // pv_buffer = proxy(pv_buffer)] ZS_LAMBDA(int vi) mutable { + // ptag = zs::SmallString(ptag) + // // exec_tag, + // // total_ghost_impulse_X = proxy(total_ghost_impulse_X), + // // total_ghost_impulse_Y = proxy(total_ghost_impulse_Y), + // // total_ghost_impulse_Z = proxy(total_ghost_impulse_Z), + // // pv_buffer = proxy(pv_buffer) + // ] ZS_LAMBDA(int vi) mutable { // auto cv = verts.pack(dim_c<3>,ptag,vi); - // auto pv = pv_buffer[vi]; - // auto m = verts("m",vi); - // auto dv = m * (cv - pv); + // // auto pv = pv_buffer[vi]; + // // auto m = verts("m",vi); + // // auto dv = m * (cv - pv); // // for(int i = 0;i != 3;++i) - // atomic_add(exec_tag,&total_ghost_impulse_X[0],dv[0]); - // atomic_add(exec_tag,&total_ghost_impulse_Y[0],dv[1]); - // atomic_add(exec_tag,&total_ghost_impulse_Z[0],dv[2]); + // // atomic_add(exec_tag,&total_ghost_impulse_X[0],dv[0]); + // // atomic_add(exec_tag,&total_ghost_impulse_Y[0],dv[1]); + // // atomic_add(exec_tag,&total_ghost_impulse_Z[0],dv[2]); + // printf("cv[%d] : %f %f %f\n",vi,(float)cv[0],(float)cv[1],(float)cv[2]); // }); // auto tgi = total_ghost_impulse.getVal(0); @@ -574,6 +597,8 @@ struct XPBDSolve : INode { // printf("ghost_impulse[%d][%d] : %f %f %f\n",(int)coffset,(int)group_size,(float)tgx,(float)tgy,(float)tgz); } + // cudaPol(zs::range(verts.size())) + set_output("constraints",constraints); set_output("zsparticles",zsparticles); set_output("target",target); @@ -1417,6 +1442,9 @@ struct XPBDSolveSmooth : INode { const auto& cquads = constraints->getQuadraturePoints(); auto category = constraints->readMeta(CONSTRAINT_KEY,wrapt{}); + if(category == category_c::edge_length_constraint) + continue; + // std::cout << "computing smoothing for constraints" << std::endl; cudaPol(zs::range(cquads.size()),[ diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh index 585c24546d..db64ef477e 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh @@ -131,7 +131,7 @@ namespace zeno { namespace CONSTRAINT { } // ---------------------------------------------------------------------------------------------- - template + template constexpr bool init_IsometricBendingConstraint( const VECTOR3d& p0, const VECTOR3d& p1, @@ -139,14 +139,16 @@ namespace zeno { namespace CONSTRAINT { const VECTOR3d& p3, MATRIX4d& Q) { + using SCALER = typename VECTOR3d::value_type; // Compute matrix Q for quadratic bending - const VECTOR3d* x[4] = { &p2, &p3, &p0, &p1 }; + const VECTOR3d x[4] = { p2, p3, p0, p1 }; + // Q = MATRIX4d::uniform(0); - const VECTOR3d e0 = *x[1] - *x[0]; - const VECTOR3d e1 = *x[2] - *x[0]; - const VECTOR3d e2 = *x[3] - *x[0]; - const VECTOR3d e3 = *x[2] - *x[1]; - const VECTOR3d e4 = *x[3] - *x[1]; + const VECTOR3d e0 = x[1] - x[0]; + const VECTOR3d e1 = x[2] - x[0]; + const VECTOR3d e2 = x[3] - x[0]; + const VECTOR3d e3 = x[2] - x[1]; + const VECTOR3d e4 = x[3] - x[1]; const SCALER c01 = LSL_GEO::cotTheta(e0, e1); const SCALER c02 = LSL_GEO::cotTheta(e0, e2); @@ -156,7 +158,7 @@ namespace zeno { namespace CONSTRAINT { const SCALER A0 = static_cast(0.5) * (e0.cross(e1)).norm(); const SCALER A1 = static_cast(0.5) * (e0.cross(e2)).norm(); - const SCALER coef = -3.f / (2.f * (A0 + A1)); + const SCALER coef = static_cast(-3.0 / 2.0) / (A0 + A1); const SCALER K[4] = { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 }; const SCALER K2[4] = { coef * K[0], coef * K[1], coef * K[2], coef * K[3] }; @@ -174,25 +176,27 @@ namespace zeno { namespace CONSTRAINT { // ---------------------------------------------------------------------------------------------- template constexpr bool solve_IsometricBendingConstraint( - const VECTOR3d& p0, SCALER invMass0, - const VECTOR3d& p1, SCALER invMass1, - const VECTOR3d& p2, SCALER invMass2, - const VECTOR3d& p3, SCALER invMass3, + const VECTOR3d& p0, const SCALER& invMass0, + const VECTOR3d& p1, const SCALER& invMass1, + const VECTOR3d& p2, const SCALER& invMass2, + const VECTOR3d& p3, const SCALER& invMass3, const MATRIX4d& Q, - const SCALER stiffness, - const SCALER dt, + const SCALER& stiffness, + const SCALER& dt, SCALER& lambda, VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3) { - constexpr SCALER eps = (SCALER)1e-6; - const VECTOR3d* x[4] = { &p2, &p3, &p0, &p1 }; + constexpr SCALER eps = static_cast(1e-6); + const VECTOR3d x[4] = { p2, p3, p0, p1 }; SCALER invMass[4] = { invMass2, invMass3, invMass0, invMass1 }; SCALER energy = 0.0; for (unsigned char k = 0; k < 4; k++) for (unsigned char j = 0; j < 4; j++) - energy += Q(j, k) * (x[k]->dot(*x[j])); - energy *= 0.5; + energy += Q(j, k) * (x[k].dot(x[j])); + energy *= static_cast(0.5); + + // printf("isometric_bending_energy : %f\n",(float)energy); VECTOR3d gradC[4] = {}; gradC[0] = VECTOR3d::uniform(0); @@ -201,7 +205,7 @@ namespace zeno { namespace CONSTRAINT { gradC[3] = VECTOR3d::uniform(0); for (unsigned char k = 0; k < 4; k++) for (unsigned char j = 0; j < 4; j++) - gradC[j] += Q(j, k) * *x[k]; + gradC[j] += Q(j, k) * x[k]; SCALER sum_normGradC = 0.0; @@ -220,10 +224,13 @@ namespace zeno { namespace CONSTRAINT { } // exit early if required + // for(int i = 0;i != 4;++i) + // printf("gradC[%d] : %f %f %f\n",i,(float)gradC[i][0],(float)gradC[i][1],(float)gradC[i][2]); + if (zs::abs(sum_normGradC) > eps) { // compute impulse-based scaling factor - const SCALER delta_lambda = -(energy + alpha * lambda) / sum_normGradC; + SCALER delta_lambda = -(energy + alpha * lambda) / sum_normGradC; lambda += delta_lambda; corr0 = (delta_lambda * invMass[2]) * gradC[2]; @@ -232,6 +239,11 @@ namespace zeno { namespace CONSTRAINT { corr3 = (delta_lambda * invMass[1]) * gradC[1]; return true; + }else { + corr0 = VECTOR3d::uniform(0); + corr1 = VECTOR3d::uniform(0); + corr2 = VECTOR3d::uniform(0); + corr3 = VECTOR3d::uniform(0); } return false; } @@ -245,14 +257,14 @@ namespace zeno { namespace CONSTRAINT { const VECTOR3d& p3, SCALER invMass3, const MATRIX4d& Q, VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3){ - constexpr SCALER eps = (SCALER)1e-6; - const VECTOR3d* x[4] = { &p2, &p3, &p0, &p1 }; + constexpr SCALER eps = static_cast(1e-6); + const VECTOR3d x[4] = { p2, p3, p0, p1 }; SCALER invMass[4] = { invMass2, invMass3, invMass0, invMass1 }; SCALER energy = 0.0; for (unsigned char k = 0; k < 4; k++) for (unsigned char j = 0; j < 4; j++) - energy += Q(j, k) * (x[k]->dot(*x[j])); + energy += Q(j, k) * (x[k].dot(x[j])); energy *= 0.5; VECTOR3d gradC[4] = {}; @@ -262,7 +274,7 @@ namespace zeno { namespace CONSTRAINT { gradC[3] = VECTOR3d::uniform(0); for (unsigned char k = 0; k < 4; k++) for (unsigned char j = 0; j < 4; j++) - gradC[j] += Q(j, k) * *x[k]; + gradC[j] += Q(j, k) * x[k]; SCALER sum_normGradC = 0.0; @@ -285,6 +297,11 @@ namespace zeno { namespace CONSTRAINT { corr3 = (s * invMass[1]) * gradC[1]; return true; + }else { + corr0 = VECTOR3d::uniform(0); + corr1 = VECTOR3d::uniform(0); + corr2 = VECTOR3d::uniform(0); + corr3 = VECTOR3d::uniform(0); } return false; } From e13610144f2d6ca1b2b5231a36659dfdf21f4834 Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Sat, 19 Aug 2023 15:34:48 +0800 Subject: [PATCH 11/21] fix nvcc crash with PBD --- projects/CuLagrange/pbd/Pipeline.cu | 66 ++++++++++++++--------------- 1 file changed, 31 insertions(+), 35 deletions(-) diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index f5aed55511..26bf402b0b 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -158,7 +158,7 @@ struct MakeSurfaceConstraintTopology : INode { reordered_map = proxy(reordered_map), verts = proxy({},verts)] ZS_LAMBDA(int oei) mutable { auto ei = reordered_map[oei]; - printf("bd_topos[%d] : %d %d %d %d\n",ei,bd_topos[ei][0],bd_topos[ei][1],bd_topos[ei][2],bd_topos[ei][3]); + // printf("bd_topos[%d] : %d %d %d %d\n",ei,bd_topos[ei][0],bd_topos[ei][1],bd_topos[ei][2],bd_topos[ei][3]); eles.tuple(dim_c<4>,"inds",oei) = bd_topos[ei].reinterpret_bits(float_c); vec3 x[4] = {}; for(int i = 0;i != 4;++i) @@ -166,42 +166,38 @@ struct MakeSurfaceConstraintTopology : INode { mat4 Q = mat4::uniform(0); { - // CONSTRAINT::init_IsometricBendingConstraint(x[0],x[1],x[2],x[3],Q); + CONSTRAINT::init_IsometricBendingConstraint(x[0],x[1],x[2],x[3],Q); } { - auto e0 = x[1] - x[0]; - auto e1 = x[2] - x[0]; - auto e2 = x[3] - x[0]; - auto e3 = x[2] - x[1]; - auto e4 = x[3] - x[1]; + // auto e0 = x[1] - x[0]; + // auto e1 = x[2] - x[0]; + // auto e2 = x[3] - x[0]; + // auto e3 = x[2] - x[1]; + // auto e4 = x[3] - x[1]; - auto c01 = LSL_GEO::cotTheta(e0, e1); - auto c02 = LSL_GEO::cotTheta(e0, e2); - auto c03 = LSL_GEO::cotTheta(-e0, e3); - auto c04 = LSL_GEO::cotTheta(-e0, e4); + // auto c01 = LSL_GEO::cotTheta(e0, e1); + // auto c02 = LSL_GEO::cotTheta(e0, e2); + // auto c03 = LSL_GEO::cotTheta(-e0, e3); + // auto c04 = LSL_GEO::cotTheta(-e0, e4); - auto A0 = 0.5f * (e0.cross(e1)).norm(); - auto A1 = 0.5f * (e0.cross(e2)).norm(); + // auto A0 = 0.5f * (e0.cross(e1)).norm(); + // auto A1 = 0.5f * (e0.cross(e2)).norm(); - auto coef = -3.f / (2.f * (A0 + A1)); - float K[4] = { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 }; - float K2[4] = { coef * K[0], coef * K[1], coef * K[2], coef * K[3] }; + // auto coef = -3.f / (2.f * (A0 + A1)); + // float K[4] = { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 }; + // float K2[4] = { coef * K[0], coef * K[1], coef * K[2], coef * K[3] }; - for (unsigned char j = 0; j < 4; j++) - { - for (unsigned char k = 0; k < j; k++) - { - Q(j, k) = Q(k, j) = K[j] * K2[k]; - } - Q(j, j) = K[j] * K2[j]; - } + // for (unsigned char j = 0; j < 4; j++) + // { + // for (unsigned char k = 0; k < j; k++) + // { + // Q(j, k) = Q(k, j) = K[j] * K2[k]; + // } + // Q(j, j) = K[j] * K2[j]; + // } } - - - - eles.tuple(dim_c<16>,"Q",oei) = Q; }); } @@ -565,10 +561,10 @@ struct XPBDSolve : INode { // total_ghost_impulse_X.setVal(0); // total_ghost_impulse_Y.setVal(0); // total_ghost_impulse_Z.setVal(0); - if(category == category_c::isometric_bending_constraint) - std::cout << "isometric_bending_constraint output" << std::endl; - if(category == category_c::edge_length_constraint) - std::cout << "edge_length_constraint output" << std::endl; + // if(category == category_c::isometric_bending_constraint) + // std::cout << "isometric_bending_constraint output" << std::endl; + // if(category == category_c::edge_length_constraint) + // std::cout << "edge_length_constraint output" << std::endl; // cudaPol(zs::range(verts.size()),[ // verts = proxy({},verts), @@ -1053,7 +1049,7 @@ struct XPBDDetangle2 : INode { Xtag = zs::SmallString(Xtag), pxtag = zs::SmallString(pxtag), restitution_rate = restitution_rate, - exec_tag, + exec_tag = exec_tag, eps = eps, dp_buffer = proxy(dp_buffer), dp_count = proxy(dp_count), @@ -1442,8 +1438,8 @@ struct XPBDSolveSmooth : INode { const auto& cquads = constraints->getQuadraturePoints(); auto category = constraints->readMeta(CONSTRAINT_KEY,wrapt{}); - if(category == category_c::edge_length_constraint) - continue; + // if(category == category_c::edge_length_constraint) + // continue; // std::cout << "computing smoothing for constraints" << std::endl; From fa2ee0b06488b2f375fdd5d7ddb5fa030b52d91e Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Sat, 19 Aug 2023 15:35:19 +0800 Subject: [PATCH 12/21] add dihedral angle constraint --- .../constraint_function_kernel/constraint.cuh | 111 +++++++++++++++--- 1 file changed, 94 insertions(+), 17 deletions(-) diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh index db64ef477e..7fe9d0b0cc 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh @@ -130,6 +130,76 @@ namespace zeno { namespace CONSTRAINT { return true; } + template + constexpr bool solve_DihedralConstraint( + const VECTOR3d &p0, SCALER invMass0, + const VECTOR3d &p1, SCALER invMass1, + const VECTOR3d &p2, SCALER invMass2, + const VECTOR3d &p3, SCALER invMass3, + const SCALER restAngle, + const SCALER stiffness, + VECTOR3d &corr0, VECTOR3d &corr1, VECTOR3d &corr2, VECTOR3d &corr3) + { + constexpr SCALER eps = static_cast(1e-6); + if (invMass0 == 0.0 && invMass1 == 0.0) + return false; + + VECTOR3d e = p3-p2; + SCALER elen = e.norm(); + if (elen < eps) + return false; + + SCALER invElen = static_cast(1.0) / elen; + + // auto n1 = LSL_GEO::facet_normal(p0,p2,p3); + // auto n2 = LSL_GEO::facet_normal(p1,p3,p2); + VECTOR3d n1 = (p2-p0).cross(p3-p0).normal; n1 /= n1.l2NormSqr(); + VECTOR3d n2 = (p3 - p1).cross(p2 - p1); n2 /= n2.l2NormSqr(); + + VECTOR3d d0 = elen*n1; + VECTOR3d d1 = elen*n2; + VECTOR3d d2 = (p0-p3).dot(e) * invElen * n1 + (p1-p3).dot(e) * invElen * n2; + VECTOR3d d3 = (p2-p0).dot(e) * invElen * n1 + (p2-p1).dot(e) * invElen * n2; + + n1 = n1.normalize(); + n2 = n2.normalize(); + // n1.normalize(); + // n2.normalize(); + SCALER dot = n1.dot(n2); + + if (dot < -1.0) dot = -1.0; + if (dot > 1.0) dot = 1.0; + SCALER phi = acos(dot); + + // SCALER phi = (-0.6981317 * dot * dot - 0.8726646) * dot + 1.570796; // fast approximation + + SCALER lambda = + invMass0 * d0.squaredNorm() + + invMass1 * d1.squaredNorm() + + invMass2 * d2.squaredNorm() + + invMass3 * d3.squaredNorm(); + + if (lambda == 0.0) + return false; + + // stability + // 1.5 is the largest magic number I found to be stable in all cases :-) + //if (stiffness > 0.5 && fabs(phi - b.restAngle) > 1.5) + // stiffness = 0.5; + + lambda = (phi - restAngle) / lambda * stiffness; + + if (n1.cross(n2).dot(e) > 0.0) + lambda = -lambda; + + corr0 = - invMass0 * lambda * d0; + corr1 = - invMass1 * lambda * d1; + corr2 = - invMass2 * lambda * d2; + corr3 = - invMass3 * lambda * d3; + + return true; + } + // ---------------------------------------------------------------------------------------------- template constexpr bool init_IsometricBendingConstraint( @@ -144,31 +214,37 @@ namespace zeno { namespace CONSTRAINT { const VECTOR3d x[4] = { p2, p3, p0, p1 }; // Q = MATRIX4d::uniform(0); - const VECTOR3d e0 = x[1] - x[0]; - const VECTOR3d e1 = x[2] - x[0]; - const VECTOR3d e2 = x[3] - x[0]; - const VECTOR3d e3 = x[2] - x[1]; - const VECTOR3d e4 = x[3] - x[1]; + const auto e0 = x[1].cast() - x[0].cast(); + const auto e1 = x[2].cast() - x[0].cast(); + const auto e2 = x[3].cast() - x[0].cast(); + const auto e3 = x[2].cast() - x[1].cast(); + const auto e4 = x[3].cast() - x[1].cast(); + + // auto e0 = e0_.cast(); + // auto e1 = e1_.cast(); + // auto e2 = e2_.cast(); + // auto e3 = e3_.cast(); + // auto e4 = e4_.cast(); - const SCALER c01 = LSL_GEO::cotTheta(e0, e1); - const SCALER c02 = LSL_GEO::cotTheta(e0, e2); - const SCALER c03 = LSL_GEO::cotTheta(-e0, e3); - const SCALER c04 = LSL_GEO::cotTheta(-e0, e4); + const double c01 = LSL_GEO::cotTheta(e0, e1); + const double c02 = LSL_GEO::cotTheta(e0, e2); + const double c03 = LSL_GEO::cotTheta(-e0, e3); + const double c04 = LSL_GEO::cotTheta(-e0, e4); - const SCALER A0 = static_cast(0.5) * (e0.cross(e1)).norm(); - const SCALER A1 = static_cast(0.5) * (e0.cross(e2)).norm(); + const double A0 = static_cast(0.5) * (e0.cross(e1)).norm(); + const double A1 = static_cast(0.5) * (e0.cross(e2)).norm(); - const SCALER coef = static_cast(-3.0 / 2.0) / (A0 + A1); - const SCALER K[4] = { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 }; - const SCALER K2[4] = { coef * K[0], coef * K[1], coef * K[2], coef * K[3] }; + const double coef = static_cast(-3.0 / 2.0) / (A0 + A1); + const double K[4] = { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 }; + const double K2[4] = { coef * K[0], coef * K[1], coef * K[2], coef * K[3] }; for (unsigned char j = 0; j < 4; j++) { for (unsigned char k = 0; k < j; k++) { - Q(j, k) = Q(k, j) = K[j] * K2[k]; + Q(j, k) = Q(k, j) = (SCALER)(K[j] * K2[k]); } - Q(j, j) = K[j] * K2[j]; + Q(j, j) = (SCALER)(K[j] * K2[j]); } return true; @@ -186,7 +262,7 @@ namespace zeno { namespace CONSTRAINT { SCALER& lambda, VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3) { - constexpr SCALER eps = static_cast(1e-6); + constexpr SCALER eps = static_cast(1e-4); const VECTOR3d x[4] = { p2, p3, p0, p1 }; SCALER invMass[4] = { invMass2, invMass3, invMass0, invMass1 }; @@ -237,6 +313,7 @@ namespace zeno { namespace CONSTRAINT { corr1 = (delta_lambda * invMass[3]) * gradC[3]; corr2 = (delta_lambda * invMass[0]) * gradC[0]; corr3 = (delta_lambda * invMass[1]) * gradC[1]; + // printf("update corr for iso_bending energy\n"); return true; }else { From 6887b9afa402aac7d4fe2e8f283fc348e45fceec Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Wed, 13 Sep 2023 15:37:01 +0800 Subject: [PATCH 13/21] fix reading polygon vtk --- .../geometry/file_parser/read_vtk_mesh.hpp | 39 +++++++++++-------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/projects/CuLagrange/geometry/file_parser/read_vtk_mesh.hpp b/projects/CuLagrange/geometry/file_parser/read_vtk_mesh.hpp index 00634c7f80..bc03a2fc2f 100644 --- a/projects/CuLagrange/geometry/file_parser/read_vtk_mesh.hpp +++ b/projects/CuLagrange/geometry/file_parser/read_vtk_mesh.hpp @@ -136,13 +136,13 @@ namespace zeno { return true; } - template - bool parsing_cells_topology(FILE* fp,typename zeno::AttrVector>& cells_attrv,int numberofcells,int &line_count) { + template + bool parsing_cells_topology(FILE* fp,PRIM_PTR prim,int numberofcells,int &line_count) { char *bufferp; char buffer[INPUTLINESIZE]; - cells_attrv.resize(numberofcells); - auto& cells = cells_attrv.values; + // cells_attrv.resize(numberofcells); + // auto& cells = cells_attrv.values; int nm_cells_read = 0; @@ -156,21 +156,28 @@ namespace zeno { return false; } int nn = strtol(bufferp,&bufferp,0); - if(nn != CELL_SIZE){ - printf("the size of cell(%d) does not match the target memory type(%d)\n",nn,CELL_SIZE); - printf("line : %s\n",bufferp); - fclose(fp); + if(nn == 3) { + prim->tris.resize(numberofcells); + } + else if(nn == 4) { + prim->quads.resize(numberofcells); + } + else { + printf("invalid dim of polygon %d, only tris and quads are currently supported\n",nn); return false; } - // printf("reading cell<%d> : ",nm_cells_read); - for(int j = 0;j != CELL_SIZE;++j){ + for(int j = 0;j != nn;++j){ if(*bufferp == '\0') { - printf("syntax error reading vertex coords on line %d parsing cells(%d)\n",line_count,CELL_SIZE); + printf("syntax error reading vertex coords on line %d parsing cells(%d)\n",line_count,nn); fclose(fp); return false; } bufferp = find_next_numeric(bufferp);// skip the header line idx, different index packs in different lines - cells[nm_cells_read][j] = (int)strtol(bufferp,&bufferp,0); + if(nn == 3) + prim->tris[nm_cells_read][j] = (int)strtol(bufferp,&bufferp,0); + else if(nn == 4) + prim->quads[nm_cells_read][j] = (int)strtol(bufferp,&bufferp,0); + // printf("%d\t",cells[nm_cells_read][j]); } // printf("\n"); @@ -495,7 +502,7 @@ namespace zeno { int numberoflines = 0; printf("reading lines\n"); sscanf(line,"%s %d %d",id,&numberoflines,&dummy); - if(!parsing_cells_topology<2>(fp,prim->lines,numberoflines,line_count)) + if(!parsing_cells_topology(fp,prim,numberoflines,line_count)) return false; printf("finish reading lines\n"); continue; @@ -506,7 +513,7 @@ namespace zeno { sscanf(line,"%s %d %d",id,&numberofpolys,&dummy); printf("readling polygons %s %d %d\n",id,numberofpolys,dummy); - if(!parsing_cells_topology<3>(fp,prim->tris,numberofpolys,line_count)) + if(!parsing_cells_topology(fp,prim,numberofpolys,line_count)) return false; printf("finish reading polygons\n"); continue; @@ -611,9 +618,9 @@ namespace zeno { simplex_size = numberofdofs/numberofcells - 1; printf("simplex_size %d\n",simplex_size); if(simplex_size == 4) - parsing_cells_topology<4>(fp,prim->quads,numberofcells,line_count); + parsing_cells_topology(fp,prim,numberofcells,line_count); else if(simplex_size == 3) - parsing_cells_topology<3>(fp,prim->tris,numberofcells,line_count); + parsing_cells_topology(fp,prim,numberofcells,line_count); else { printf("invalid simplex size = %d %d %d\n",simplex_size,numberofcells,numberofdofs); throw std::runtime_error("INVALID SIMPLEX SIZE"); From f3dcb9378c16d91971bfb1b95c65af2e4df8321a Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Wed, 13 Sep 2023 15:37:37 +0800 Subject: [PATCH 14/21] ccd ee and pt --- .../collision_energy/evaluate_collision.hpp | 840 ++++++++++++++++++ 1 file changed, 840 insertions(+) diff --git a/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp b/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp index 858923649d..b6c98e45d1 100644 --- a/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp +++ b/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp @@ -801,6 +801,846 @@ inline int do_tetrahedra_surface_mesh_and_kinematic_boundary_collision_detection return csPT.size(); } + + +// template +// void calc_imminent_self_PT_collision_impulse(Pol& pol, +// const PosTileVec& verts,const zs::SmallString& xtag,const zs::SmallString& vtag, +// const TriTileVec& tris, +// const HalfEdgeTileVec& halfedges, +// const REAL& thickness, +// ImpulseBuffer& impulse_buffer, +// ImpulseCount& impulse_count) { +// using namespace zs; +// constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; +// constexpr auto exec_tag = wrapv{}; +// using vec3 = zs::vec; +// using vec4 = zs::vec; +// using vec4i = zs::vec; +// constexpr auto eps = (T)1e-6; + +// auto triBvh = bvh_t{}; +// auto triBvs = retrieve_bounding_volumes(pol,verts,tris,wrapv<3>{},0,xtag); +// triBvh.build(pol,triBvs); + +// pol(zs::range(verts.size()),[ +// xtag = xtag, +// verts = proxy({},verts), +// tris = proxy({},tris), +// thickness = thickness, +// eps = eps, +// halfedges = proxy({},halfedges), +// triBvh = proxy(triBvh), +// impulse_buffer = proxy(impulse_buffer), +// impulse_count = proxy(impulse_count)] ZS_LAMBDA(int vi) mutable { +// vec3 ps[4] = {}; +// ps[3] = verts.pack(dim_c<3>,xtag,vi); +// vec3 vs[4] = {}; +// vs[3] = verts.pack(dim_c<3>,vtag,vi); +// vec4i inds{}; +// inds[3] = vi; + +// auto bv = bv_t{get_bounding_box(ps[3] - thickness,ps[3] + thickness)}; +// auto calc_impulse_for_close_proximity = [&](int ti) { +// auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); +// for(int i = 0;i != 3;++i) +// if(tri[i] == vi) +// return; + +// bool has_dynamic_points = false; +// if(verts("minv",vi) > eps) +// has_dynamic_points = true; +// for(int i = 0;i != 3;++i) +// if(verts("minv",tri[i]) > eps) +// has_dynamic_points = true; +// if(!has_dynamic_points) +// return; + +// for(int i = 0;i != 3;++i) { +// ps[i] = verts.pack(dim_c<3>,xtag,tri[i]); +// vs[i] = verts.pack(dim_c<3>,vtag,tri[i]); +// inds[i] = tri[i]; +// } + +// auto tnrm = LSL_GEO::facet_normal(ps[0],ps[1],ps[2]); +// auto seg = ps[3] - ps[0]; +// auto project_dist = zs::abs(seg.dot(tnrm)); +// if(project_dist > thickness) +// return; + +// // auto barySum = (T)1.0; +// vec3 bary_centric{}; +// auto distance = LSL_GEO::pointTriangleDistance(ps[0],ps[1],ps[2],ps[3],bary_centric); + +// if(distance > thickness) +// return; + +// auto bary_sum = zs::abs(bary_centric[0]) + zs::abs(bary_centric[1]) + zs::abs(bary_centric[2]); +// if(bary_sum > (T)(1.0 + eps * 10)) { +// vec3 bnrms[3] = {}; +// auto hi = zs::reinterpret_bits(tris("he_inds",ti)); +// for(int i = 0;i != 3;++i){ +// auto edge_normal = tnrm; +// auto opposite_hi = zs::reinterpret_bits(halfedges("opposite_he",hi)); +// if(opposite_hi >= 0){ +// auto nti = zs::reinterpret_bits(halfedges("to_face",opposite_hi)); +// auto ntri = tris.pack(dim_c<3>,"inds",nti,int_c); +// auto ntnrm = LSL_GEO::facet_normal( +// verts.pack(dim_c<3>,xtag,ntri[0]), +// verts.pack(dim_c<3>,xtag,ntri[1]), +// verts.pack(dim_c<3>,xtag,ntri[2])); +// edge_normal = tnrm + ntnrm; +// edge_normal = edge_normal/(edge_normal.norm() + eps); +// } +// auto e01 = ts[(i + 1) % 3] - ts[(i + 0) % 3]; +// bnrms[i] = edge_normal.cross(e01).normalized(); +// hi = zs::reinterpret_bits(halfedges("next_he",hi)); +// } +// for(int i = 0;i != 3;++i){ +// seg = p - ts[i]; +// if(bnrms[i].dot(seg) < 0) +// return; +// } +// } + +// vec4 bary{-bary_centric[0],-bary_centric[1],-bary_centric[2],1}; +// auto pr = vec3::zeros(); +// auto vr = vec3::zeros(); +// for(int i = 0;i != 4;++i) { +// pr += bary[i] * ps[i]; +// vr += bary[i] * vs[i]; +// } + +// auto collision_nrm = pr.normalized(); +// auto relative_normal_velocity = vr.dot(collision_nrm); + +// auto I = relative_normal_velocity < 0 ? relative_normal_velocity : (T)0; +// impulse = -collision_nrm * I; + +// auto beta = (T)0; +// for(int i = 0;i != 4;++i) +// beta += bary[i] * bary[i] * verts("minv",inds[i]); +// impulse /= beta; + +// for(int i = 0;i != 4;++i) { +// atomic_add(exec_tag,&impulse_count[inds[i]],1); +// for(int d = 0;d != 3;++d) +// atomic_add(exec_tag,&impulse_buffer[inds[i]][d],impulse[d] * bary[i]); +// } +// }; +// triBvh.iter_neighbors(bv,calc_impulse_for_close_proximity); +// }); +// } + + +template +void calc_imminent_self_PT_collision_impulse(Pol& pol, + const PosTileVec& verts,const zs::SmallString& xtag,const zs::SmallString& vtag, + const TriTileVec& tris, + const HalfEdgeTileVec& halfedges, + const REAL& thickness, + size_t buffer_offset, + CollisionBuffer& imminent_collision_buffer) { + using namespace zs; + constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; + constexpr auto exec_tag = wrapv{}; + using vec3 = zs::vec; + using vec4 = zs::vec; + using vec4i = zs::vec; + constexpr auto eps = (T)1e-6; + constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; + + zs::bht csPT{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; + csPT.reset(pol,true); + + auto triBvh = bvh_t{}; + auto triBvs = retrieve_bounding_volumes(pol,verts,tris,wrapv<3>{},thickness / (REAL)2,xtag); + triBvh.build(pol,triBvs); + + pol(zs::range(verts.size()),[ + xtag = xtag, + verts = proxy({},verts), + tris = proxy({},tris), + thickness = thickness, + eps = eps, + halfedges = proxy({},halfedges), + triBvh = proxy(triBvh), + // imminent_collision_buffer = proxy({},imminent_collision_buffer), + csPT = proxy(csPT)] ZS_LAMBDA(int vi) mutable { + auto p = verts.pack(dim_c<3>,xtag,vi); + auto bv = bv_t{get_bounding_box(p - thickness/(REAL)2,p + thickness/(REAL)2)}; + auto do_close_proximity_detection = [&](int ti) { + auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); + for(int i = 0;i != 3;++i) + if(tri[i] == vi) + return; + + bool has_dynamic_points = false; + if(verts("minv",vi) > eps) + has_dynamic_points = true; + for(int i = 0;i != 3;++i) + if(verts("minv",tri[i]) > eps) + has_dynamic_points = true; + if(!has_dynamic_points) + return; + + vec3 ts[3] = {}; + for(int i = 0;i != 3;++i) + ts[i] = verts.pack(dim_c<3>,xtag,tri[i]); + + auto tnrm = LSL_GEO::facet_normal(ts[0],ts[1],ts[2]); + auto seg = p - ts[0]; + auto project_dist = zs::abs(seg.dot(tnrm)); + if(project_dist > thickness) + return; + + // auto barySum = (T)1.0; + vec3 bary_centric{}; + auto distance = LSL_GEO::pointTriangleDistance(ts[0],ts[1],ts[2],p,bary_centric); + + if(distance > thickness) + return; + + auto bary_sum = zs::abs(bary_centric[0]) + zs::abs(bary_centric[1]) + zs::abs(bary_centric[2]); + if(bary_sum > (T)(1.0 + eps * 10)) { + return; + vec3 bnrms[3] = {}; + auto hi = zs::reinterpret_bits(tris("he_inds",ti)); + for(int i = 0;i != 3;++i){ + auto edge_normal = tnrm; + auto opposite_hi = zs::reinterpret_bits(halfedges("opposite_he",hi)); + if(opposite_hi >= 0){ + auto nti = zs::reinterpret_bits(halfedges("to_face",opposite_hi)); + auto ntri = tris.pack(dim_c<3>,"inds",nti,int_c); + auto ntnrm = LSL_GEO::facet_normal( + verts.pack(dim_c<3>,xtag,ntri[0]), + verts.pack(dim_c<3>,xtag,ntri[1]), + verts.pack(dim_c<3>,xtag,ntri[2])); + edge_normal = tnrm + ntnrm; + edge_normal = edge_normal/(edge_normal.norm() + eps); + } + auto e01 = ts[(i + 1) % 3] - ts[(i + 0) % 3]; + bnrms[i] = edge_normal.cross(e01).normalized(); + hi = zs::reinterpret_bits(halfedges("next_he",hi)); + } + for(int i = 0;i != 3;++i){ + seg = p - ts[i]; + if(bnrms[i].dot(seg) < 0) + return; + } + } + csPT.insert(zs::vec{vi,ti}); + }; + + triBvh.iter_neighbors(bv,do_close_proximity_detection); + }); + + imminent_collision_buffer.resize(csPT.size() + buffer_offset); + pol(zip(zs::range(csPT.size()),csPT._activeKeys),[ + csPT = proxy(csPT), + imminent_collision_buffer = proxy({},imminent_collision_buffer), + xtag = xtag,vtag = vtag, + buffer_offset = buffer_offset, + verts = proxy({},verts), + tris = proxy({},tris)] ZS_LAMBDA(auto id,const auto& pair) mutable { + auto vi = pair[0]; + auto ti = pair[1]; + auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); + + vec4i inds{tri[0],tri[1],tri[2],vi}; + vec3 ps[4] = {}; + for(int i = 0;i != 4;++i) + ps[i] = verts.pack(dim_c<3>,xtag,inds[i]); + vec3 vs[4] = {}; + for(int i = 0;i != 4;++i) + vs[i] = verts.pack(dim_c<3>,vtag,inds[i]); + + vec3 bary_centric{}; + LSL_GEO::pointBaryCentric(ps[0],ps[1],ps[2],ps[3],bary_centric); + + for(int i = 0;i != 3;++i) + bary_centric[i] = bary_centric[i] < 0 ? 0 : bary_centric[i]; + auto barySum = bary_centric[0] + bary_centric[1] + bary_centric[2]; + bary_centric = bary_centric / barySum; + + vec4 bary{-bary_centric[0],-bary_centric[1],-bary_centric[2],1}; + + auto pr = vec3::zeros(); + auto vr = vec3::zeros(); + for(int i = 0;i != 4;++i) { + pr += bary[i] * ps[i]; + vr += bary[i] * vs[i]; + } + + auto collision_nrm = LSL_GEO::facet_normal(ps[0],ps[1],ps[2]); + auto align = collision_nrm.dot(pr); + if(align < 0) + collision_nrm *= -1; + // LSL_GEO::facet_normal(ps[0],ps[1],ps[2]); + auto relative_normal_velocity = vr.dot(collision_nrm); + + imminent_collision_buffer.tuple(dim_c<4>,"bary",id + buffer_offset) = bary; + imminent_collision_buffer.tuple(dim_c<4>,"inds",id + buffer_offset) = inds.reinterpret_bits(float_c); + auto I = relative_normal_velocity < 0 ? relative_normal_velocity : (T)0; + imminent_collision_buffer.tuple(dim_c<3>,"impulse",id + buffer_offset) = -collision_nrm * I; + imminent_collision_buffer.tuple(dim_c<3>,"collision_normal",id + buffer_offset) = collision_nrm; + }); +} + + +template +void calc_imminent_self_EE_collision_impulse(Pol& pol, + const PosTileVec& verts,const zs::SmallString& xtag,const zs::SmallString& vtag, + const EdgeTileVec& edges, + const REAL& thickness, + size_t buffer_offset, + CollisionBuffer& imminent_collision_buffer) { + using namespace zs; + constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; + constexpr auto exec_tag = wrapv{}; + using vec2 = zs::vec; + using vec3 = zs::vec; + using vec4 = zs::vec; + using vec2i = zs::vec; + using vec4i = zs::vec; + constexpr auto eps = (T)1e-6; + constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; + zs::bht csEE{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; + csEE.reset(pol,true); + + auto edgeBvh = bvh_t{}; + auto bvs = retrieve_bounding_volumes(pol,verts,edges,wrapv<2>{},thickness / (T)2,xtag); + edgeBvh.build(pol,bvs); + + pol(zs::range(edges.size()),[ + xtag = xtag, + verts = proxy({},verts), + edges = proxy({},edges), + thickness = thickness, + eps = eps, + edgeBvh = proxy(edgeBvh), + csEE = proxy(csEE)] ZS_LAMBDA(int ei) mutable { + auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); + vec3 pas[2] = {}; + for(int i = 0;i != 2;++i) + pas[i] = verts.pack(dim_c<3>,xtag,ea[i]); + auto bv = bv_t{get_bounding_box(pas[0],pas[1])}; + bv._max += thickness/2; + bv._min -= thickness/2; + auto la = (pas[0] - pas[1]).norm(); + + auto do_close_proximity_detection = [&](int nei) mutable { + if(ei >= nei) + return; + auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); + for(int i = 0;i != 2;++i){ + if(eb[i] == ea[0] || eb[i] == ea[1]) + return; + } + + bool has_dynamic_points = false; + for(int i = 0;i != 2;++i) { + if(verts("minv",ea[i]) > eps) + has_dynamic_points = true; + if(verts("minv",eb[i]) > eps) + has_dynamic_points = true; + } + if(!has_dynamic_points) + return; + + vec3 pbs[2] = {}; + for(int i = 0;i != 2;++i) + pbs[i] = verts.pack(dim_c<3>,xtag,eb[i]); + + auto cp = (pas[0] - pas[1]).cross(pbs[0] - pbs[1]).norm(); + // if(cp < eps * 100) { + // auto ac = (pas[0] + pas[1]) / (T)2.0; + // auto bc = (pbs[0] + pbs[1]) / (T)2.0; + // if((ac - bc).norm() < thickness) + // csEE.insert(vec2i{ei,nei}); + + // return; + // } + + vec3 int_a{},int_b{}; + COLLISION_UTILS::IntersectLineSegments(pas[0],pas[1],pbs[0],pbs[1],int_a,int_b); + auto dist = (int_a - int_b).norm(); + if(dist > thickness) + return; + + auto lb = (pbs[0] - pbs[1]).norm(); + + auto ra = (pas[0] - int_a).norm() / la; + auto rb = (pbs[0] - int_b).norm() / lb; + + if(ra < 100 * eps || ra > 1 - 100 * eps) + return; + if(rb < 100 * eps || rb > 1 - 100 * eps) + return; + + csEE.insert(vec2i{ei,nei}); + }; + edgeBvh.iter_neighbors(bv,do_close_proximity_detection); + }); + + // printf("nm_EE_collision_pairs : %d %d\n",(int)csEE.size(),(int)edges.size()); + + imminent_collision_buffer.resize(csEE.size() + buffer_offset); + pol(zip(zs::range(csEE.size()),csEE._activeKeys),[ + verts = proxy({},verts), + edges = proxy({},edges), + buffer_offset = buffer_offset, + xtag = xtag, + vtag = vtag, + eps = eps, + imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(auto ci,const auto& pair) mutable { + auto ei = pair[0]; + auto nei = pair[1]; + + auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); + auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); + vec4i inds{ea[0],ea[1],eb[0],eb[1]}; + + vec3 ps[4] = {}; + vec3 vs[4] = {}; + for(int i = 0;i != 4;++i) { + ps[i] = verts.pack(dim_c<3>,xtag,inds[i]); + vs[i] = verts.pack(dim_c<3>,vtag,inds[i]); + } + + vec3 int_a{},int_b{}; + COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[2],ps[3],int_a,int_b); + + auto la = (ps[0] - ps[1]).norm(); + auto lb = (ps[2] - ps[3]).norm(); + + auto ra = (ps[0] - int_a).norm() / la; + auto rb = (ps[2] - int_b).norm() / lb; + + vec4 bary{ra - 1,-ra,1 - rb,rb}; + + auto pr = vec3::zeros(); + auto vr = vec3::zeros(); + for(int i = 0;i != 4;++i) { + pr += bary[i] * ps[i]; + vr += bary[i] * vs[i]; + } + + auto collision_nrm = (ps[0] - ps[1]).cross(ps[2] - ps[3]).normalized(); + // if(collision_nrm.norm() < eps * 100) { + + // } + auto align = collision_nrm.dot(pr); + if(align < 0) + collision_nrm *= -1; + auto relative_normal_velocity = vr.dot(collision_nrm); + + imminent_collision_buffer.tuple(dim_c<4>,"bary",ci + buffer_offset) = bary; + imminent_collision_buffer.tuple(dim_c<4>,"inds",ci + buffer_offset) = inds.reinterpret_bits(float_c); + auto I = relative_normal_velocity < 0 ? relative_normal_velocity : (T)0; + imminent_collision_buffer.tuple(dim_c<3>,"impulse",ci + buffer_offset) = -collision_nrm * I; + imminent_collision_buffer.tuple(dim_c<3>,"collision_normal",ci + buffer_offset) = collision_nrm; + }); + + // printf("Eval EE collision impulse\n"); +} + + + +// template +// void calc_continous_self_PT_collision_impulse(Pol& pol, +// const PosTileVec& verts,const zs::SmallString& xtag,const zs::SmallString& vtag, +// const TrisTileVec& tris, +// const REAL& thickness, +// TriBvh& triCCDBvh, +// bool refit_bvh, +// int buffer_offset, +// ImpulseBuffer& impulse_buffer) { +// using namespace zs; +// constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; +// constexpr auto exec_tag = wrapv{}; +// using vec2 = zs::vec; +// using vec3 = zs::vec; +// using vec4 = zs::vec; +// using vec2i = zs::vec; +// using vec4i = zs::vec; +// constexpr auto eps = (T)1e-6; +// constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; + +// zs::bht csPT{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; +// csPT.reset(pol,true); + +// // auto triCCDBvh = bvh_t{}; +// auto bvs = retrieve_bounding_volumes(pol,verts,tris,verts,wrapv<3>{},(T)1.0,thickness/(T)2,xtag,vtag); +// if(refit_bvh) +// triCCDBvh.refit(pol,bvs); +// else +// triCCDBvh.build(pol,bvs); + +// pol(zs::range(tris.size()),[ +// xtag = xtag, +// vtag = vtag, +// verts = proxy({},verts), +// tris = proxy({},tris), +// csPT = proxy(csPT), +// thickness = thickness, +// eps = eps, +// bvh = proxy(triCCDBvh)] ZS_LAMBDA(int ei) mutable { +// auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); +// vec3 pas[2] = {}; +// vec3 vas[2] = {}; +// for(int i = 0;i != 2;++i) { +// pas[i] = verts.pack(dim_c<3>,xtag,ea[i]); +// vas[i] = verts.pack(dim_c<3>,vtag,ea[i]); +// } + +// auto bv = edgeBvs[ei]; + +// auto do_close_proximity_detection = [&](int nei) mutable { +// if(ei >= nei) +// return; +// auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); +// for(int i = 0;i != 2;++i){ +// if(eb[i] == ea[0] || eb[i] == ea[1]) +// return; +// } + +// bool has_dynamic_points = false; +// for(int i = 0;i != 2;++i) { +// if(verts("minv",ea[i]) > eps) +// has_dynamic_points = true; +// if(verts("minv",eb[i]) > eps) +// has_dynamic_points = true; +// } +// if(!has_dynamic_points) +// return; + +// vec3 pbs[2] = {}; +// vec3 vbs[2] = {}; +// for(int i = 0;i != 2;++i) { +// pbs[i] = verts.pack(dim_c<3>,xtag,eb[i]); +// vbs[i] = verts.pack(dim_c<3>,vtag,eb[i]); +// } + +// auto alpha = (T)1.0; +// ee_accd(pas[0],pas[1],pbs[0],pbs[1],vas[0],vas[1],vbs[0],vbs[1],(T)0.2,thickness,alpha); + + +// vec4i inds{ea[0],ea[1],eb[0],eb[1]}; +// vec3 ps[4] = {}; +// for(int i = 0;i != 4;++i) +// ps[i] = verts.pack(dim_c<3>,xtag,inds[i]) + verts.pack(dim_c<3>,vtag,inds[i]); + +// vec3 int_a{},int_b{}; +// COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[0],ps[1],int_a,int_b); + +// auto la = (ps[0] - ps[1]).norm(); + +// auto ra = (ps[0] - int_a).norm() / (ps[0] - ps[1]).norm();; +// auto rb = (ps[2] - int_b).norm() / (ps[2] - ps[3]).norm();; + +// if(ra < 10 * eps || ra > 1 - 10 * eps) +// return; +// if(rb < 10 * eps || rb > 1 - 10 * eps) +// return; + +// csEE.insert(vec2i{ei,nei}); +// }; +// edgeBvh.iter_neighbors(bv,do_close_proximity_detection); +// }); + +// printf("nm_EE_collision_pairs : %d %d\n",(int)csEE.size(),(int)edges.size()); + +// imminent_collision_buffer.resize(csEE.size() + buffer_offset); +// pol(zip(zs::range(csEE.size()),csEE._activeKeys),[ +// verts = proxy({},verts), +// edges = proxy({},edges), +// buffer_offset = buffer_offset, +// xtag = xtag, +// vtag = vtag, +// imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(auto ci,const auto& pair) mutable { +// auto ei = pair[0]; +// auto nei = pair[1]; + +// auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); +// auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); +// vec4i inds{ea[0],ea[1],eb[0],eb[1]}; + +// vec3 ps[4] = {}; +// vec3 vs[4] = {}; +// for(int i = 0;i != 4;++i) { +// ps[i] = verts.pack(dim_c<3>,xtag,inds[i]); +// vs[i] = verts.pack(dim_c<3>,vtag,inds[i]); +// } + +// vec3 int_a{},int_b{}; +// COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[2],ps[3],int_a,int_b); + +// auto la = (ps[0] - ps[1]).norm(); +// auto lb = (ps[2] - ps[3]).norm(); + +// auto ra = (ps[0] - int_a).norm() / la; +// auto rb = (ps[2] - int_b).norm() / lb; + +// vec4 bary{ra - 1,-ra,1 - rb,rb}; + +// auto pr = vec3::zeros(); +// auto vr = vec3::zeros(); +// for(int i = 0;i != 4;++i) { +// pr += bary[i] * ps[i]; +// vr += bary[i] * vs[i]; +// } + +// auto collision_nrm = pr.normalized(); +// auto relative_normal_velocity = vr.dot(collision_nrm); + +// imminent_collision_buffer.tuple(dim_c<4>,"bary",ci + buffer_offset) = bary; +// imminent_collision_buffer.tuple(dim_c<4>,"inds",ci + buffer_offset) = inds.reinterpret_bits(float_c); +// auto I = relative_normal_velocity < 0 ? relative_normal_velocity : (T)0; +// imminent_collision_buffer.tuple(dim_c<3>,"impulse",ci + buffer_offset) = -collision_nrm * I; +// }); +// } + +// template +// void calc_continous_self_EE_collision_impulse(Pol& pol, +// const PosTileVec& verts,const zs::SmallString& xtag,const zs::SmallString& vtag, +// const EdgeTileVec& edges, +// const REAL& thickness, +// ImpulseBuffer& impulse_buffer, +// ImpulseCountBuffer& impulse_count) { +// using namespace zs; +// constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; +// constexpr auto exec_tag = wrapv{}; +// using vec2 = zs::vec; +// using vec3 = zs::vec; +// using vec4 = zs::vec; +// using vec2i = zs::vec; +// using vec4i = zs::vec; +// constexpr auto eps = (T)1e-6; +// constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; +// zs::bht csEE{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; +// csEE.reset(pol,true); + +// auto edgeCCDBvh = bvh_t{}; +// auto bvs = retrieve_bounding_volumes(pol,verts,edges,verts,wrapv<2>{},(T)1.0,thickness/(T)2,xtag,vtag); +// edgeCCDBvh.build(pol,bvs); + +// pol(zs::range(edges.size()),[ +// xtag = xtag, +// vtag = vtag, +// verts = proxy({},verts), +// edges = proxy({},edges), +// thickness = thickness, +// eps = eps, +// edgeBvs = proxy(edgeBvs), +// edgeBvh = proxy(edgeBvh)] ZS_LAMBDA(int ei) mutable { +// auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); +// vec3 pas[2] = {}; +// vec3 vas[2] = {}; +// for(int i = 0;i != 2;++i) { +// pas[i] = verts.pack(dim_c<3>,xtag,ea[i]); +// vas[i] = verts.pack(dim_c<3>,vtag,ea[i]); +// } + +// auto bv = edgeBvs[ei]; + +// auto do_close_proximity_detection = [&](int nei) mutable { +// if(ei >= nei) +// return; +// auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); +// for(int i = 0;i != 2;++i){ +// if(eb[i] == ea[0] || eb[i] == ea[1]) +// return; +// } + +// bool has_dynamic_points = false; +// for(int i = 0;i != 2;++i) { +// if(verts("minv",ea[i]) > eps) +// has_dynamic_points = true; +// if(verts("minv",eb[i]) > eps) +// has_dynamic_points = true; +// } +// if(!has_dynamic_points) +// return; + +// vec3 pbs[2] = {}; +// vec3 vbs[2] = {}; +// for(int i = 0;i != 2;++i) { +// pbs[i] = verts.pack(dim_c<3>,xtag,eb[i]); +// vbs[i] = verts.pack(dim_c<3>,vtag,eb[i]); +// } + +// auto alpha = (T)1.0; +// ee_accd(pas[0],pas[1],pbs[0],pbs[1],vas[0],vas[1],vbs[0],vbs[1],(T)0.2,thickness,alpha); + + +// vec4i inds{ea[0],ea[1],eb[0],eb[1]}; +// vec3 ps[4] = {}; +// for(int i = 0;i != 4;++i) +// ps[i] = verts.pack(dim_c<3>,xtag,inds[i]) + verts.pack(dim_c<3>,vtag,inds[i]); + +// vec3 int_a{},int_b{}; +// COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[0],ps[1],int_a,int_b); + +// auto la = (ps[0] - ps[1]).norm(); + +// auto ra = (ps[0] - int_a).norm() / (ps[0] - ps[1]).norm();; +// auto rb = (ps[2] - int_b).norm() / (ps[2] - ps[3]).norm();; + +// if(ra < 10 * eps || ra > 1 - 10 * eps) +// return; +// if(rb < 10 * eps || rb > 1 - 10 * eps) +// return; + +// csEE.insert(vec2i{ei,nei}); +// }; +// edgeBvh.iter_neighbors(bv,do_close_proximity_detection); +// }); + +// printf("nm_EE_collision_pairs : %d %d\n",(int)csEE.size(),(int)edges.size()); + +// imminent_collision_buffer.resize(csEE.size() + buffer_offset); +// pol(zip(zs::range(csEE.size()),csEE._activeKeys),[ +// verts = proxy({},verts), +// edges = proxy({},edges), +// buffer_offset = buffer_offset, +// xtag = xtag, +// vtag = vtag, +// imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(auto ci,const auto& pair) mutable { +// auto ei = pair[0]; +// auto nei = pair[1]; + +// auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); +// auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); +// vec4i inds{ea[0],ea[1],eb[0],eb[1]}; + +// vec3 ps[4] = {}; +// vec3 vs[4] = {}; +// for(int i = 0;i != 4;++i) { +// ps[i] = verts.pack(dim_c<3>,xtag,inds[i]); +// vs[i] = verts.pack(dim_c<3>,vtag,inds[i]); +// } + +// vec3 int_a{},int_b{}; +// COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[2],ps[3],int_a,int_b); + +// auto la = (ps[0] - ps[1]).norm(); +// auto lb = (ps[2] - ps[3]).norm(); + +// auto ra = (ps[0] - int_a).norm() / la; +// auto rb = (ps[2] - int_b).norm() / lb; + +// vec4 bary{ra - 1,-ra,1 - rb,rb}; + +// auto pr = vec3::zeros(); +// auto vr = vec3::zeros(); +// for(int i = 0;i != 4;++i) { +// pr += bary[i] * ps[i]; +// vr += bary[i] * vs[i]; +// } + +// auto collision_nrm = pr.normalized(); +// auto relative_normal_velocity = vr.dot(collision_nrm); + +// imminent_collision_buffer.tuple(dim_c<4>,"bary",ci + buffer_offset) = bary; +// imminent_collision_buffer.tuple(dim_c<4>,"inds",ci + buffer_offset) = inds.reinterpret_bits(float_c); +// auto I = relative_normal_velocity < 0 ? relative_normal_velocity : (T)0; +// imminent_collision_buffer.tuple(dim_c<3>,"impulse",ci + buffer_offset) = -collision_nrm * I; +// }); +// } + + +template +void apply_impulse(Pol& pol, + PosTileVec& verts,const zs::SmallString& vtag, + const T& imminent_restitution_rate, + const T& imminent_relaxation_rate, + CollisionBuffer& imminent_collision_buffer) { + using namespace zs; + constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; + constexpr auto exec_tag = wrapv{}; + using vec3 = zs::vec; + constexpr auto eps = (T)1e-6; + + zs::Vector impulse_buffer{verts.get_allocator(),verts.size()}; + pol(zs::range(impulse_buffer),[] ZS_LAMBDA(auto& imp) mutable {imp = vec3::zeros();}); + zs::Vector impulse_count{verts.get_allocator(),verts.size()}; + pol(zs::range(impulse_count),[] ZS_LAMBDA(auto& c) mutable {c = 0;}); + + pol(zs::range(imminent_collision_buffer.size()),[ + verts = proxy({},verts), + vtag = zs::SmallString(vtag), + imminent_collision_buffer = proxy({},imminent_collision_buffer), + exec_tag = exec_tag, + eps = eps, + restitution_rate = imminent_restitution_rate, + impulse_count = proxy(impulse_count), + impulse_buffer = proxy(impulse_buffer)] ZS_LAMBDA(auto ci) mutable { + auto inds = imminent_collision_buffer.pack(dim_c<4>,"inds",ci,int_c); + auto bary = imminent_collision_buffer.pack(dim_c<4>,"bary",ci); + auto impulse = imminent_collision_buffer.pack(dim_c<3>,"impulse",ci); + if(impulse.norm() < eps) + return; + + T beta = 0; + for(int i = 0;i != 4;++i) + beta += bary[i] * bary[i] * verts("minv",inds[i]); + impulse /= beta; + + for(int i = 0;i != 4;++i) { + atomic_add(exec_tag,&impulse_count[inds[i]],1); + for(int d = 0;d != 3;++d) + atomic_add(exec_tag,&impulse_buffer[inds[i]][d],impulse[d] * bary[i]); + } + + }); + pol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtag = zs::SmallString(vtag), + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count), + relaxation_rate = imminent_relaxation_rate, + eps = eps, + exec_tag = exec_tag] ZS_LAMBDA(int vi) mutable { + if(impulse_buffer[vi].norm() < eps || impulse_count[vi] == 0) + return; + auto impulse = relaxation_rate * impulse_buffer[vi] / (T)impulse_count[vi]; + auto dp = impulse * verts("minv",vi); + + for(int i = 0;i != 3;++i) + atomic_add(exec_tag,&verts(vtag,i,vi),dp[i]); + }); +} + template Date: Wed, 13 Sep 2023 15:38:41 +0800 Subject: [PATCH 15/21] XPBD fix smooth solve wip --- .../CuLagrange/geometry/kernel/geo_math.hpp | 13 + projects/CuLagrange/pbd/Pipeline.cu | 1356 +++++++++++++++-- .../constraint_function_kernel/constraint.cuh | 26 +- 3 files changed, 1294 insertions(+), 101 deletions(-) diff --git a/projects/CuLagrange/geometry/kernel/geo_math.hpp b/projects/CuLagrange/geometry/kernel/geo_math.hpp index ac800562b5..36f0349163 100644 --- a/projects/CuLagrange/geometry/kernel/geo_math.hpp +++ b/projects/CuLagrange/geometry/kernel/geo_math.hpp @@ -629,6 +629,19 @@ constexpr REAL pointTriangleDistance(const VECTOR3& v0, const VECTOR3& v1, } + constexpr void pointBaryCentric(const VECTOR3& v0, const VECTOR3& v1, + const VECTOR3& v2, const VECTOR3& v,VECTOR3& bary) { + const VECTOR3 e1 = v1 - v0; + const VECTOR3 e2 = v2 - v0; + const VECTOR3 n = e1.cross(e2); + const VECTOR3 na = (v2 - v1).cross(v - v1); + const VECTOR3 nb = (v0 - v2).cross(v - v2); + const VECTOR3 nc = (v1 - v0).cross(v - v0); + const VECTOR3 barycentric(n.dot(na) / n.l2NormSqr(), + n.dot(nb) / n.l2NormSqr(), + n.dot(nc) / n.l2NormSqr()); + bary = barycentric; + } constexpr REAL pointTriangleDistance(const VECTOR3& v0, const VECTOR3& v1, const VECTOR3& v2, const VECTOR3& v,REAL& barySum) diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index 26bf402b0b..4e9c64094f 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -1,4 +1,5 @@ #include "Structures.hpp" +// #include "../fem/SolverUtils.cuh" #include "zensim/Logger.hpp" #include "zensim/cuda/execution/ExecutionPolicy.cuh" #include "zensim/omp/execution/ExecutionPolicy.hpp" @@ -13,8 +14,10 @@ #include #include "constraint_function_kernel/constraint.cuh" +#include "../geometry/kernel/tiled_vector_ops.hpp" #include "../geometry/kernel/topology.hpp" #include "../geometry/kernel/geo_math.hpp" +#include "../fem/collision_energy/evaluate_collision.hpp" #include "constraint_function_kernel/constraint_types.hpp" namespace zeno { @@ -86,22 +89,9 @@ struct MakeSurfaceConstraintTopology : INode { if(type == "stretch") { constraint->setMeta(CONSTRAINT_KEY,category_c::edge_length_constraint); - // constraint->category = ZenoParticles::curve; - auto quads_vec = tilevec_topo_to_zsvec_topo(cudaPol,quads,wrapv<3>{}); zs::Vector> edge_topos{quads.get_allocator(),0}; retrieve_edges_topology(cudaPol,quads_vec,edge_topos); - // zs::Vector> edge_topos{quads.get_allocator(),edge_topos_broken.size()}; - // cudaPol(zs::range(edge_topos.size()),[ - // edge_topos = proxy(edge_topos), - // edge_topos_broken = proxy(edge_topos_broken)] ZS_LAMBDA(int ei) mutable { - // edge_topos[ei][0] = edge_topos_broken[ei][0]; - // edge_topos[ei][1] = edge_topos_broken[ei][1]; - // edge_topos[ei][2] = -1; - // edge_topos[ei][3] = -1; - // }); - - eles.resize(edge_topos.size()); topological_coloring(cudaPol,edge_topos,colors); @@ -120,9 +110,6 @@ struct MakeSurfaceConstraintTopology : INode { rest_scale = rest_scale, edge_topos = proxy(edge_topos)] ZS_LAMBDA(int oei) mutable { auto ei = reordered_map[oei]; - // auto edge_full = edge_topos[ei]; - // vec2i edge{edge_full[0],edge_full[1]}; - eles.tuple(dim_c<2>,"inds",oei) = edge_topos[ei].reinterpret_bits(float_c); vec3 x[2] = {}; for(int i = 0;i != 2;++i) @@ -165,39 +152,7 @@ struct MakeSurfaceConstraintTopology : INode { x[i] = verts.pack(dim_c<3>,"x",bd_topos[ei][i]); mat4 Q = mat4::uniform(0); - { - CONSTRAINT::init_IsometricBendingConstraint(x[0],x[1],x[2],x[3],Q); - } - { - // auto e0 = x[1] - x[0]; - // auto e1 = x[2] - x[0]; - // auto e2 = x[3] - x[0]; - // auto e3 = x[2] - x[1]; - // auto e4 = x[3] - x[1]; - - // auto c01 = LSL_GEO::cotTheta(e0, e1); - // auto c02 = LSL_GEO::cotTheta(e0, e2); - // auto c03 = LSL_GEO::cotTheta(-e0, e3); - // auto c04 = LSL_GEO::cotTheta(-e0, e4); - - // auto A0 = 0.5f * (e0.cross(e1)).norm(); - // auto A1 = 0.5f * (e0.cross(e2)).norm(); - - // auto coef = -3.f / (2.f * (A0 + A1)); - // float K[4] = { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 }; - // float K2[4] = { coef * K[0], coef * K[1], coef * K[2], coef * K[3] }; - - - - // for (unsigned char j = 0; j < 4; j++) - // { - // for (unsigned char k = 0; k < j; k++) - // { - // Q(j, k) = Q(k, j) = K[j] * K2[k]; - // } - // Q(j, j) = K[j] * K2[j]; - // } - } + CONSTRAINT::init_IsometricBendingConstraint(x[0],x[1],x[2],x[3],Q); eles.tuple(dim_c<16>,"Q",oei) = Q; }); } @@ -268,6 +223,11 @@ struct MakeSurfaceConstraintTopology : INode { color_offset.setVal(0); } + if(type == "attachment") { + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + + } + cudaPol(zs::range(eles.size()),[ eles = proxy({},eles), uniform_stiffness = uniform_stiffness, @@ -525,8 +485,6 @@ struct XPBDSolve : INode { lambda, dp[0],dp[1],dp[2],dp[3]); - - for(int i = 0;i != 4;++i) { // printf("dp[%d][%d] : %f %f %f %f\n",gi,i,s,(float)dp[i][0],(float)dp[i][1],(float)dp[i][2]); verts.tuple(dim_c<3>,ptag,quad[i]) = p[i] + dp[i]; @@ -606,24 +564,756 @@ ZENDEFNODE(XPBDSolve, {{{"zsparticles"},{"constraints"},{"target"},{"float","dt" {{"string","ptag","X"}}, {"PBD"}}); -// struct ParticlesColliderProject : INode { -// using T = float; -// using vec3 = zs::vec; +struct DetangleImminentCollision : INode { + using T = float; + using vec3 = zs::vec; + using dtiles_t = zs::TileVector; -// virtual void apply() override { -// using namespace zs; -// constexpr auto space = execspace_e::cuda; -// auto cudaPol = cuda_exec(); + virtual void apply() override { + using namespace zs; + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto exec_tag = wrapv{}; + constexpr auto eps = (T)1e-7; -// auto zsparticles = get_intput("zsparticles"); -// auto xtag = get_input2("xtag"); -// auto ptag = get_input2("ptag"); + auto zsparticles = get_input("zsparticles"); + auto current_x_tag = get_input2("current_x_tag"); + auto pre_x_tag = get_input2("previous_x_tag"); + auto repel_strength = get_input2("repeling_strength"); + auto imminent_collision_thickness = get_input2("immc_thickness"); + // apply impulse for imminent collision for previous configuration + auto& verts = zsparticles->getParticles(); + const auto& tris = zsparticles->getQuadraturePoints(); + const auto& halfedges = (*zsparticles)[ZenoParticles::s_surfHalfEdgeTag]; + const auto &edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + + dtiles_t vtemp{verts.get_allocator(),{ + {"x",3}, + {"v",3}, + {"minv",1} + },verts.size()}; + TILEVEC_OPS::copy(cudaPol,verts,pre_x_tag,vtemp,"x"); + TILEVEC_OPS::copy(cudaPol,verts,"minv",vtemp,"minv"); + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + current_x_tag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { + vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - vtemp.pack(dim_c<3>,"x",vi); + }); + // TILEVEC_OPS::copy(cudaPol,verts,current_x_tag,vtemp,"v"); + // TILEVEC_OPS::add(cudaPol,vtemp,"v",1,"x",-1,"v"); + dtiles_t imminent_collision_buffer(verts.get_allocator(), + { + {"inds",4}, + {"bary",4}, + {"impulse",3}, + {"collision_normal",3} + },(size_t)0); + + auto imminent_restitution_rate = get_input2("imm_restitution"); + auto imminent_relaxation_rate = get_input2("imm_relaxation"); + + auto nm_iters = get_input2("nm_imminent_iters"); + + auto do_pt_detection = get_input2("use_PT"); + auto do_ee_detection = get_input2("use_EE"); + + for(int it = 0;it != nm_iters;++it) { + // we use collision cell as the collision volume, PT collision is enough prevent penertation? + if(do_pt_detection) { + COLLISION_UTILS::calc_imminent_self_PT_collision_impulse(cudaPol, + vtemp,"x","v", + tris, + halfedges, + imminent_collision_thickness, + 0, + imminent_collision_buffer); + // std::cout << "nm_imminent_PT_collision : " << imminent_collision_buffer.size() << std::endl; + } + + if(do_ee_detection) { + COLLISION_UTILS::calc_imminent_self_EE_collision_impulse(cudaPol, + vtemp,"x","v", + edges, + imminent_collision_thickness, + imminent_collision_buffer.size(), + imminent_collision_buffer); + // std::cout << "nm_imminent_EE_collision : " << imminent_collision_buffer.size() << std::endl; + } + // resolve imminent PT collision + + // impulse_norm = TILEVEC_OPS::dot<3>(cudaPol,imminent_collision_buffer,"impulse","impulse"); + // std::cout << "EE_PT_impulse_norm : " << impulse_norm << std::endl; + + COLLISION_UTILS::apply_impulse(cudaPol, + vtemp,"v", + imminent_restitution_rate, + imminent_relaxation_rate, + imminent_collision_buffer); + } + + auto add_repulsion_force = get_input2("add_repulsion_force"); + // add an impulse to repel the pair further + if(add_repulsion_force) { + std::cout << "add imminent replering force" << std::endl; + auto max_repel_distance = get_input2("max_repel_distance"); + + cudaPol(zs::range(imminent_collision_buffer.size()),[ + imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(int ci) mutable { + imminent_collision_buffer.tuple(dim_c<3>,"impulse",ci) = vec3::zeros(); + }); + + cudaPol(zs::range(imminent_collision_buffer.size()),[ + vtemp = proxy({},vtemp), + eps = eps, + exec_tag = exec_tag, + k = repel_strength, + max_repel_distance = max_repel_distance, + thickness = imminent_collision_thickness, + imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(auto id) mutable { + auto inds = imminent_collision_buffer.pack(dim_c<4>,"inds",id,int_c); + auto bary = imminent_collision_buffer.pack(dim_c<4>,"bary",id); + + vec3 ps[4] = {}; + vec3 vs[4] = {}; + auto vr = vec3::zeros(); + auto pr = vec3::zeros(); + for(int i = 0;i != 4;++i) { + ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); + vs[i] = vtemp.pack(dim_c<3>,"v",inds[i]); + pr += bary[i] * ps[i]; + vr += bary[i] * vs[i]; + } + + auto dist = pr.norm(); + vec3 collision_normal = imminent_collision_buffer.pack(dim_c<3>,"collision_normal",id); + + if(dist > thickness) + return; + + auto d = thickness - dist; + auto vn = vr.dot(collision_normal); + if(vn > (T)max_repel_distance * d) { + // if with current velocity, the collided particles can be repeled by more than 1% of collision depth, no extra repulsion is needed + return; + } else { + // make sure the collided particles is seperated by 1% of collision depth + // assume the particles has the same velocity + auto I = k * d; + auto I_max = (max_repel_distance * d - vn); + I = I_max < I ? I_max : I; + auto impulse = (T)I * collision_normal; + + imminent_collision_buffer.tuple(dim_c<3>,"impulse",id) = impulse; + } + }); + + // auto impulse_norm = TILEVEC_OPS::dot<3>(cudaPol,imminent_collision_buffer,"impulse","impulse"); + // std::cout << "REPEL_impulse_norm : " << impulse_norm << std::endl; + + COLLISION_UTILS::apply_impulse(cudaPol, + vtemp,"v", + imminent_restitution_rate, + imminent_relaxation_rate, + imminent_collision_buffer); + } + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + current_x_tag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { + verts.tuple(dim_c<3>,current_x_tag,vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); + }); -// auto& verts = zsparticles->getParticles(); + std::cout << "finish apply imminent impulse" << std::endl; + + set_output("zsparticles",zsparticles); + } +}; + +ZENDEFNODE(DetangleImminentCollision, {{{"zsparticles"}, + {"string","current_x_tag","x"}, + {"string","previous_x_tag","px"}, + // {"string","pscaleTag","pscale"}, + {"float","repeling_strength","1.0"}, + {"float","immc_thickness","0.01"}, + {"int","nm_imminent_iters","1"}, + {"float","imm_restitution","0.1"}, + {"float","imm_relaxation","0.25"}, + {"float","max_repel_distance","0.1"}, + {"bool","add_repulsion_force","0"}, + {"bool","use_PT","1"}, + {"bool","use_EE","1"}, + }, + {{"zsparticles"}}, + {}, + {"PBD"}}); + + +struct VisualizeImminentCollision : INode { + using T = float; + using vec3 = zs::vec; + using dtiles_t = zs::TileVector; + + virtual void apply() override { + using namespace zs; + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + auto ompPol = omp_exec(); + constexpr auto omp_space = execspace_e::openmp; + + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto exec_tag = wrapv{}; + constexpr auto eps = (T)1e-7; + constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; + + auto zsparticles = get_input("zsparticles"); + auto current_x_tag = get_input2("current_x_tag"); + auto pre_x_tag = get_input2("previous_x_tag"); + auto imminent_collision_thickness = get_input2("immc_thickness"); + + // apply impulse for imminent collision for previous configuration + auto& verts = zsparticles->getParticles(); + const auto& tris = zsparticles->getQuadraturePoints(); + const auto& edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + const auto& halfedges = (*zsparticles)[ZenoParticles::s_surfHalfEdgeTag]; + + // auto nm_imminent_iters = get_input2("nm_imminent_iters"); + dtiles_t imminent_PT_collision_buffer(verts.get_allocator(), + { + {"inds",4}, + {"bary",4}, + {"impulse",3}, + {"collision_normal",3} + },(size_t)0); + + dtiles_t imminent_EE_collision_buffer(verts.get_allocator(), + { + {"inds",4}, + {"bary",4}, + {"impulse",3}, + {"collision_normal",3} + },(size_t)0); + // zs::bht csPT{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; + // csPT.reset(cudaPol,true); + + dtiles_t vtemp(verts.get_allocator(),{ + {"x",3}, + {"v",3}, + {"minv",1} + },verts.size()); + TILEVEC_OPS::copy(cudaPol,verts,pre_x_tag,vtemp,"x"); + TILEVEC_OPS::copy(cudaPol,verts,"minv",vtemp,"minv"); + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + current_x_tag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { + vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - vtemp.pack(dim_c<3>,"x",vi); + }); + // we use collision cell as the collision volume, PT collision is enough prevent penertation? + COLLISION_UTILS::calc_imminent_self_PT_collision_impulse(cudaPol, + vtemp,"x","v", + tris, + halfedges, + imminent_collision_thickness, + 0, + imminent_PT_collision_buffer); + std::cout << "nm_PT_collision : " << imminent_PT_collision_buffer.size() << std::endl; + + COLLISION_UTILS::calc_imminent_self_EE_collision_impulse(cudaPol, + vtemp,"x","v", + edges, + imminent_collision_thickness, + 0, + imminent_EE_collision_buffer); + std::cout << "nm_EE_collision : " << imminent_EE_collision_buffer.size() << std::endl; + // resolve imminent PT collision + dtiles_t imminent_PT_collision_vis(verts.get_allocator(),{ + {"collision_normal",3}, + {"pt",3}, + {"pp",3}},imminent_PT_collision_buffer.size()); + + cudaPol(zs::range(imminent_PT_collision_buffer.size()),[ + vtemp = proxy({},vtemp), + imminent_PT_collision_vis = proxy({},imminent_PT_collision_vis), + imminent_PT_collision_buffer = proxy({},imminent_PT_collision_buffer)] ZS_LAMBDA(auto ci) mutable { + auto inds = imminent_PT_collision_buffer.pack(dim_c<4>,"inds",ci,int_c); + auto bary = imminent_PT_collision_buffer.pack(dim_c<4>,"bary",ci); + + vec3 ps[4] = {}; + for(int i = 0;i != 4;++i) + ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); -// } -// }; + auto proj_t = vec3::zeros(); + auto pr = vec3::zeros(); + for(int i = 0;i != 4;++i) + pr += bary[i] * ps[i]; + + for(int i = 0;i != 3;++i) + proj_t -= bary[i] * ps[i]; + + auto collision_normal = pr.normalized(); + + imminent_PT_collision_vis.tuple(dim_c<3>,"collision_normal",ci) = collision_normal; + + imminent_PT_collision_vis.tuple(dim_c<3>,"pt",ci) = proj_t; + imminent_PT_collision_vis.tuple(dim_c<3>,"pp",ci) = vtemp.pack(dim_c<3>,"x",inds[3]); + }); + imminent_PT_collision_vis = imminent_PT_collision_vis.clone({zs::memsrc_e::host}); + + auto prim_PT = std::make_shared(); + auto& vis_verts_PT = prim_PT->verts; + auto& vis_lines_PT = prim_PT->lines; + vis_verts_PT.resize(imminent_PT_collision_buffer.size() * 2); + vis_lines_PT.resize(imminent_PT_collision_buffer.size()); + + auto nrm_prim_PT = std::make_shared(); + auto& nrm_verts_PT = nrm_prim_PT->verts; + auto& nrm_lines_PT = nrm_prim_PT->lines; + nrm_verts_PT.resize(imminent_PT_collision_buffer.size() * 2); + nrm_lines_PT.resize(imminent_PT_collision_buffer.size()); + + ompPol(zs::range(imminent_PT_collision_buffer.size()),[ + &vis_verts_PT,&vis_lines_PT, + &nrm_verts_PT,&nrm_lines_PT, + imminent_PT_collision_vis = proxy({},imminent_PT_collision_vis)] (int ci) mutable { + auto cnrm = imminent_PT_collision_vis.pack(dim_c<3>,"collision_normal",ci); + auto pt = imminent_PT_collision_vis.pack(dim_c<3>,"pt",ci); + auto pp = imminent_PT_collision_vis.pack(dim_c<3>,"pp",ci); + auto pn = pp + cnrm; + + vis_verts_PT[ci * 2 + 0] = pp.to_array(); + vis_verts_PT[ci * 2 + 1] = pt.to_array(); + vis_lines_PT[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + + nrm_verts_PT[ci * 2 + 0] = pp.to_array(); + nrm_verts_PT[ci * 2 + 1] = pn.to_array(); + nrm_lines_PT[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + }); + + set_output("prim_PT",std::move(prim_PT)); + set_output("cnrm_PT",std::move(nrm_prim_PT)); + + std::cout << "output PT VIS" << std::endl; + + + dtiles_t imminent_EE_collision_vis(verts.get_allocator(),{ + {"collision_normal",3}, + {"pt",3}, + {"pp",3}},imminent_EE_collision_buffer.size()); + + cudaPol(zs::range(imminent_EE_collision_buffer.size()),[ + vtemp = proxy({},vtemp), + imminent_EE_collision_vis = proxy({},imminent_EE_collision_vis), + imminent_EE_collision_buffer = proxy({},imminent_EE_collision_buffer)] ZS_LAMBDA(auto ci) mutable { + auto inds = imminent_EE_collision_buffer.pack(dim_c<4>,"inds",ci,int_c); + auto bary = imminent_EE_collision_buffer.pack(dim_c<4>,"bary",ci); + + vec3 ps[4] = {}; + for(int i = 0;i != 4;++i) + ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); + + auto pr = vec3::zeros(); + for(int i = 0;i != 4;++i) + pr += bary[i] * ps[i]; + + auto proja = vec3::zeros(); + auto projb = vec3::zeros(); + for(int i = 0;i != 2;++i) { + proja -= bary[i] * ps[i]; + projb += bary[i + 2] * ps[i + 2]; + } + + auto collision_normal = pr.normalized(); + + imminent_EE_collision_vis.tuple(dim_c<3>,"collision_normal",ci) = collision_normal; + imminent_EE_collision_vis.tuple(dim_c<3>,"pt",ci) = proja; + imminent_EE_collision_vis.tuple(dim_c<3>,"pp",ci) = projb; + }); + imminent_EE_collision_vis = imminent_EE_collision_vis.clone({zs::memsrc_e::host}); + + std::cout << "output EE VIS" << std::endl; + + auto prim_EE = std::make_shared(); + auto& vis_verts_EE = prim_EE->verts; + auto& vis_lines_EE = prim_EE->lines; + vis_verts_EE.resize(imminent_EE_collision_buffer.size() * 2); + vis_lines_EE.resize(imminent_EE_collision_buffer.size()); + + auto nrm_prim_EE = std::make_shared(); + auto& nrm_verts_EE = nrm_prim_EE->verts; + auto& nrm_lines_EE = nrm_prim_EE->lines; + nrm_verts_EE.resize(imminent_EE_collision_buffer.size() * 2); + nrm_lines_EE.resize(imminent_EE_collision_buffer.size()); + + ompPol(zs::range(imminent_EE_collision_buffer.size()),[ + &vis_verts_EE,&vis_lines_EE, + &nrm_verts_EE,&nrm_lines_EE, + imminent_EE_collision_vis = proxy({},imminent_EE_collision_vis)] (int ci) mutable { + auto cnrm = imminent_EE_collision_vis.pack(dim_c<3>,"collision_normal",ci); + auto pt = imminent_EE_collision_vis.pack(dim_c<3>,"pt",ci); + auto pp = imminent_EE_collision_vis.pack(dim_c<3>,"pp",ci); + auto pn = pp + cnrm; + + vis_verts_EE[ci * 2 + 0] = pp.to_array(); + vis_verts_EE[ci * 2 + 1] = pt.to_array(); + vis_lines_EE[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + + nrm_verts_EE[ci * 2 + 0] = pp.to_array(); + nrm_verts_EE[ci * 2 + 1] = pn.to_array(); + nrm_lines_EE[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + }); + + set_output("prim_EE",std::move(prim_EE)); + set_output("cnrm_EE",std::move(nrm_prim_EE)); + + } +}; + +ZENDEFNODE(VisualizeImminentCollision, {{{"zsparticles"}, + {"string","current_x_tag","x"}, + {"string","previous_x_tag","px"}, + // {"string","pscaleTag","pscale"}, + // {"float","repeling_strength","1.0"}, + {"float","immc_thickness","0.01"}, + // {"int","nm_imminent_iters","1"}, + // {"float","imm_restitution","0.1"}, + // {"float","imm_relaxation","0.25"}, + // {"bool","add_repulsion_force","0"}, + }, + {{"prim_PT"},{"cnrm_PT"},{"prim_EE"},{"cnrm_EE"}}, + {}, + {"PBD"}}); + + +struct DetangleCCDCollision : INode { + using T = float; + using vec3 = zs::vec; + using vec4 = zs::vec; + using vec4i = zs::vec; + using dtiles_t = zs::TileVector; + + virtual void apply() override { + using namespace zs; + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + using lbvh_t = typename ZenoLinearBvh::lbvh_t; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto exec_tag = wrapv{}; + constexpr auto eps = (T)1e-7; + constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; + + auto zsparticles = get_input("zsparticles"); + auto current_x_tag = get_input2("current_x_tag"); + auto pre_x_tag = get_input2("previous_x_tag"); + + auto restitution_rate = get_input2("restitution"); + auto relaxation_rate = get_input2("relaxation"); + + auto thickness = get_input2("thickness"); + + auto& verts = zsparticles->getParticles(); + const auto& tris = zsparticles->getQuadraturePoints(); + const auto &edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + + dtiles_t vtemp(verts.get_allocator(),{ + {"x",3}, + {"v",3} + },(size_t)verts.size()); + + auto nm_ccd_iters = get_input2("nm_ccd_iters"); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + current_x_tag = zs::SmallString(current_x_tag), + pre_x_tag = zs::SmallString(pre_x_tag)] ZS_LAMBDA(int vi) mutable { + vtemp.tuple(dim_c<3>,"x",vi) = verts.pack(dim_c<3>,pre_x_tag,vi); + vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - verts.pack(dim_c<3>,pre_x_tag,vi); + }); + + + lbvh_t triBvh{},eBvh{}; + + zs::Vector impulse_buffer{verts.get_allocator(),verts.size()}; + zs::Vector impulse_count{verts.get_allocator(),verts.size()}; + + auto do_ee_detection = get_input2("do_ee_detection"); + auto do_pt_detection = get_input2("do_pt_detection"); + + for(int iter = 0;iter != nm_ccd_iters;++iter) { + // std::cout << "do ccd iters : " << iter << std::endl; + + cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); + cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + + + if(do_pt_detection) { + std::cout << "do_ccd_pt_detection" << std::endl; + + auto triBvs = retrieve_bounding_volumes(cudaPol,vtemp,tris,vtemp,zs::wrapv<3>{},1.f,thickness / (T)2,"x","v"); + if(iter == 0) + triBvh.build(cudaPol,triBvs); + else + triBvh.refit(cudaPol,triBvs); + + cudaPol(zs::range(vtemp.size()),[ + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count), + verts = proxy({},verts), + vtemp = proxy({},vtemp), + tris = proxy({},tris), + exec_tag = exec_tag, + thickness = thickness, + restitution_rate = restitution_rate, + bvh = proxy(triBvh)] ZS_LAMBDA(int vi) mutable { + auto p = vtemp.pack(dim_c<3>,"x",vi); + auto v = vtemp.pack(dim_c<3>,"v",vi); + // auto np = p + v; + + bv_t bv{p,p + v}; + bv._min -= thickness / (T)2.0; + bv._max += thickness / (T)2.0; + + auto resolve_ccd_colliding_pairs = [&] (int ti) mutable { + auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); + for(int i = 0;i != 3;++i) + if(tri[i] == vi) + return; + + vec3 tp[3] = {}; + vec3 tv[3] = {}; + for(int i = 0;i != 3;++i) { + tp[i] = vtemp.pack(dim_c<3>,"x",tri[i]); + tv[i] = vtemp.pack(dim_c<3>,"v",tri[i]); + } + + auto alpha = (T)1.0; + pt_accd(p,tp[0],tp[1],tp[2],v,tv[0],tv[1],tv[2],(T)0.2,(T)0,alpha); + + if(alpha > (T)1.0 || alpha < (T)0) + return; + + auto np = p + alpha * v; + vec3 ntp[3] = {}; + for(int i = 0;i != 3;++i) + ntp[i] = tp[i] + alpha * tv[i]; + + vec3 bary_centric{}; + LSL_GEO::pointBaryCentric(ntp[0],ntp[1],ntp[2],np,bary_centric); + for(int i = 0;i != 3;++i) + bary_centric[i] = bary_centric[i] < 0 ? 0 : bary_centric[i]; + auto barySum = bary_centric[0] + bary_centric[1] + bary_centric[2]; + bary_centric = bary_centric / barySum; + + + auto pt_project = vec3::zeros(); + auto vt_project = vec3::zeros(); + for(int i = 0;i != 3;++i) { + pt_project += ntp[i] * bary_centric[i]; + vt_project += tv[i] * bary_centric[i]; + } + + auto collision_nrm = LSL_GEO::facet_normal(ntp[0],ntp[1],ntp[2]); + auto relative_velocity = v - vt_project; + if(collision_nrm.dot(relative_velocity) > 0) + collision_nrm *= -1; + + auto relative_normal_velocity = collision_nrm.dot(relative_velocity); + + auto cm = verts("minv",vi); + for(int i = 0;i != 3;++i) + cm += bary_centric[i] * bary_centric[i] * verts("minv",tri[i]); + if(cm < eps) + return; + cm = (T)1/cm; + + auto delta_rv = collision_nrm * relative_normal_velocity * (1 - alpha) * cm; + + for(int i = 0;i != 3;++i) { + auto interpI = delta_rv * bary_centric[i]; + atomic_add(exec_tag,&impulse_count[tri[i]],1); + for(int d = 0;d != 3;++d) + atomic_add(exec_tag,&impulse_buffer[tri[i]][d],interpI[d]); + } + auto interpI = -delta_rv; + atomic_add(exec_tag,&impulse_count[vi],1); + for(int d = 0;d != 3;++d) + atomic_add(exec_tag,&impulse_buffer[vi][d],interpI[d]); + }; + bvh.iter_neighbors(bv,resolve_ccd_colliding_pairs); + }); + + } + + if(do_ee_detection) { + + std::cout << "do_ccd_ee_detection" << std::endl; + + auto edgeBvs = retrieve_bounding_volumes(cudaPol,vtemp,edges,vtemp,zs::wrapv<2>{},1.f,thickness / (T)2,"x","v"); + if(iter == 0) + eBvh.build(cudaPol,edgeBvs); + else + eBvh.refit(cudaPol,edgeBvs); + + cudaPol(zs::range(edges.size()),[ + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count), + verts = proxy({},verts), + vtemp = proxy({},vtemp), + edges = proxy({},edges), + exec_tag = exec_tag, + eps = eps, + thickness = thickness, + // restitution_rate = restitution_rate, + bvs = proxy(edgeBvs), + bvh = proxy(eBvh)] ZS_LAMBDA(int ei) mutable { + auto bv = bvs[ei]; + auto edge = edges.pack(dim_c<2>,"inds",ei,int_c); + vec3 pas[2] = {}; + vec3 vas[2] = {}; + for(int i = 0;i != 2;++i) { + pas[i] = vtemp.pack(dim_c<3>,"x",edge[i]); + vas[i] = vtemp.pack(dim_c<3>,"v",edge[i]); + } + + auto resolve_ccd_collilsion_pairs = [&] (int nei) mutable { + if(nei == ei) + return; + auto nedge = edges.pack(dim_c<2>,"inds",nei,int_c); + for(int i = 0;i != 2;++i) + if(edge[i] == nedge[0] || edge[i] == nedge[1]) + return; + bool has_dynamic_vert = false; + for(int i = 0;i != 2;++i) + if(verts("minv",edge[i]) > eps || verts("minv",nedge[i]) > eps) + has_dynamic_vert = true; + if(!has_dynamic_vert) + return; + + + vec3 pbs[2] = {}; + vec3 vbs[2] = {}; + for(int i = 0;i != 2;++i) { + pbs[i] = vtemp.pack(dim_c<3>,"x",nedge[i]); + vbs[i] = vtemp.pack(dim_c<3>,"v",nedge[i]); + } + + auto alpha = (T)1.0; + ee_accd(pas[0],pas[1],pbs[0],pbs[1],vas[0],vas[1],vbs[0],vbs[1],(T)0.2,(T)0,alpha); + + if(alpha > (T)1.0 || alpha < (T)0) + return; + + vec3 npas[2] = {}; + vec3 npbs[2] = {}; + for(int i = 0;i != 2;++i) { + npas[i] = pas[i] + alpha * vas[i]; + npbs[i] = pbs[i] + alpha * vbs[i]; + } + + vec3 int_a{},int_b{}; + COLLISION_UTILS::IntersectLineSegments(npas[0],npas[1],npbs[0],npbs[1],int_a,int_b); + auto lb = (npbs[0] - npbs[1]).norm(); + auto la = (npas[0] - npas[1]).norm(); + + if(la < eps * 10 || lb < eps * 10) + return; + + auto ra = (npas[0] - int_a).norm() / la; + auto rb = (npbs[0] - int_b).norm() / lb; + + vec4 bary{ra - 1,-ra,1 - rb,rb}; + // auto pr = vec3::zeros(); + auto vr = vec3::zeros(); + auto pr = vec3::zeros(); + for(int i = 0;i != 2;++i) { + // pr += bary[i] * ps[i]; + vr += bary[i] * vas[i]; + vr += bary[i + 2] * vbs[i]; + pr += bary[i] * pas[i]; + pr += bary[i + 2] * pbs[i]; + } + + auto collision_nrm = (npbs[0] - npbs[1]).cross(npas[0] - npas[1]).normalized(); + if(collision_nrm.norm() < 100 * eps) + return; + auto align = collision_nrm.dot(pr); + if(align < 0) + collision_nrm *= -1; + auto relative_normal_velocity = vr.dot(collision_nrm); + + auto impulse = -relative_normal_velocity * collision_nrm * ((T)1 - alpha); + vec4i inds{edge[0],edge[1],nedge[0],nedge[1]}; + + T beta = 0; + for(int i = 0;i != 4;++i) + beta += bary[i] * bary[i] * verts("minv",inds[i]); + impulse /= beta; + for(int i = 0;i != 4;++i) { + atomic_add(exec_tag,&impulse_count[inds[i]],1); + for(int d = 0;d != 3;++d) + atomic_add(exec_tag,&impulse_buffer[inds[i]][d],impulse[d] * bary[i]); + } + }; + + bvh.iter_neighbors(bv,resolve_ccd_collilsion_pairs); + }); + + + } + + std::cout << "apply impulse" << std::endl; + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count), + relaxation_rate = relaxation_rate, + eps = eps, + exec_tag = exec_tag] ZS_LAMBDA(int vi) mutable { + if(impulse_count[vi] == 0) + return; + if(impulse_buffer[vi].norm() < eps) + return; + auto impulse = relaxation_rate * impulse_buffer[vi] / impulse_count[vi]; + auto dv = impulse * verts("minv",vi); + vtemp.tuple(dim_c<3>,"v",vi) = vtemp.pack(dim_c<3>,"v",vi) + dv; + // for(int i = 0;i != 3;++i) + // atomic_add(exec_tag,&vtemp("v",i,vi),dv[i]); + }); + } + + cudaPol(zs::range(verts.size()),[ + vtemp = proxy({},vtemp), + verts = proxy({},verts), + xtag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { + verts.tuple(dim_c<3>,xtag,vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); + }); + + set_output("zsparticles",zsparticles); + } +}; + +ZENDEFNODE(DetangleCCDCollision, {{{"zsparticles"}, + {"string","current_x_tag","x"}, + {"string","previous_x_tag","px"}, + {"int","nm_ccd_iters","1"}, + // {"string","pscaleTag","pscale"}, + {"float","thickness","0.1"}, + // {"float","immc_thickness","0.01"}, + // {"int","nm_imminent_iters","1"}, + {"float","restitution","0.1"}, + {"float","relaxation","1"}, + {"bool","do_ee_detection","1"}, + {"bool","do_pt_detection","1"}, + // {"bool","add_repulsion_force","0"}, + }, + {{"zsparticles"}}, + {}, + {"PBD"}}); struct SDFColliderProject : INode { @@ -1179,7 +1869,6 @@ struct XPBDDetangle2 : INode { set_output("zsparticles",zsparticles); } - }; ZENDEFNODE(XPBDDetangle2, {{{"zsparticles"}, @@ -1198,6 +1887,490 @@ ZENDEFNODE(XPBDDetangle2, {{{"zsparticles"}, }, {"PBD"}}); + + +// struct XPBDDetangle3 : INode { +// // ray trace bvh +// template +// void buildTriRayTraceBvh(zs::CudaExecutionPolicy &pol, +// const TileVecT &verts, +// const ElmTileVec& tris, +// const T& dt, +// const zs::SmallString& xTag, +// const zs::SmallString& vTag, +// const T& thickness,ZenoLinearBvh::lbvh_t &bvh) { +// using namespace zs; +// using bv_t = typename ZenoLinearBvh::lbvh_t::Box; +// constexpr auto space = execspace_e::cuda; +// Vector bvs{tris.get_allocator(), tris.size()}; +// retrieve_bounding_volumes(pol,verts,xTag,tris,verts,vTag,dt,0,bvs); +// bvh.build(pol, bvs); +// } + +// virtual void apply() override { +// using namespace zs; +// using bvh_t = typename ZenoLinearBvh::lbvh_t; +// using bv_t = typename bvh_t::Box; + +// using vec3 = zs::vec; +// using vec2i = zs::vec; +// using vec3i = zs::vec; +// using vec4i = zs::vec; +// using mat4 = zs::vec; + +// constexpr auto space = execspace_e::cuda; +// auto cudaPol = cuda_exec(); +// constexpr auto exec_tag = wrapv{}; +// constexpr auto eps = (T)1e-7; + +// auto zsparticles = get_input("zsparticles"); +// auto xtag = get_input2("xtag"); +// auto vtag = get_input2("vtag"); +// // auto pxtag = get_input2("pxtag"); +// // auto Xtag = get_input2("Xtag"); +// // auto ccdTag = get_input2("ccdTag"); +// // x +// // auto pscaleTag = get_input2("pscaleTag"); +// // auto friction = get_input2("friction"); + +// auto& verts = zsparticles->getParticles(); + +// auto restitution_rate = get_input2("restitution"); +// auto relaxation_rate = get_input2("relaxation"); + +// auto triRayBvh = bvh_t{}; +// buildTriRayTraceBvh(cudaPol,verts,pxtag,xtag,pscaleTag,triRayBvh); + +// // zs::Vector ccd_buffer{verts.get_allocator(),verts.size()}; +// // cudaPol(zs::range(ccd_buffer),[]ZS_LAMBDA(auto& t) mutable {t = (float)1;}); + +// zs::Vector dp_buffer{verts.get_allocator(),verts.size()}; +// cudaPol(zs::range(dp_buffer),[]ZS_LAMBDA(auto& dp) mutable {dp = vec3::uniform(0);}); + +// zs::Vector dp_count{verts.get_allocator(),verts.size()}; +// cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + +// cudaPol(zs::range(verts.size()),[ +// verts = proxy({},verts), +// xtag = zs::SmallString(xtag), +// Xtag = zs::SmallString(Xtag), +// pxtag = zs::SmallString(pxtag), +// restitution_rate = restitution_rate, +// exec_tag = exec_tag, +// eps = eps, +// dp_buffer = proxy(dp_buffer), +// dp_count = proxy(dp_count), +// pscaleTag = zs::SmallString(pscaleTag), +// spBvh = proxy(spBvh)] ZS_LAMBDA(int vi) mutable { +// auto dst = verts.pack(dim_c<3>,xtag,vi); +// auto src = verts.pack(dim_c<3>,pxtag,vi); +// auto vel = dst - src; +// auto rpos = verts.pack(dim_c<3>,Xtag,vi); +// // auto r = verts(ptag,vi); + +// auto pscale = verts(pscaleTag,vi); +// bv_t bv{src,dst}; +// bv._min -= pscale; +// bv._max += pscale; + +// auto find_particle_sphere_collision_pairs = [&] (int nvi) mutable { +// if(vi >= nvi) +// return; + +// auto npscale = verts(pscaleTag,nvi); +// auto thickness = pscale + npscale; + +// auto nrpos = verts.pack(dim_c<3>,Xtag,nvi); +// auto rdist = (rpos - nrpos).norm(); +// if(rdist < thickness) +// return; + +// auto ndst = verts.pack(dim_c<3>,xtag,nvi); +// auto nsrc = verts.pack(dim_c<3>,pxtag,nvi); +// auto nvel = ndst - nsrc; + +// auto t = LSL_GEO::ray_ray_intersect(src,vel,nsrc,nvel,thickness); +// if(t <= (float)1 && t >= (float)0) { +// // atomic_min(exec_tag,&ccd_buffer[vi],t); +// // atomic_min(exec_tag,&ccd_buffer[nvi],t); +// // auto rel_vel = vel - nvel; +// auto CR = restitution_rate; + +// auto minv_a = verts("minv",vi); +// auto minv_b = verts("minv",nvi); +// if(minv_a + minv_b < (T)2 * eps) +// return; + +// auto ua = vel; +// auto ub = nvel; +// auto ur = ua - ub; + +// vec3 va{},vb{}; + +// if(minv_a > (T)eps) { +// // ma / mb +// auto ratio = minv_b / minv_a; +// auto m = ratio + (T)1; +// auto momt = ratio * ua + ub; +// va = (momt - CR * ur) / m; +// vb = (momt + CR * ratio * ur) / m; + +// if(isnan(va.norm()) || isnan(vb.norm())) { +// printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); +// } +// }else if(minv_b > (T)eps) { +// // mb / ma +// auto ratio = minv_a / minv_b; +// auto m = ratio + (T)1; +// auto momt = ua + ratio * ub; +// va = (momt - CR * ratio * ur) / m; +// vb = (momt + CR * ur) / m; +// if(isnan(va.norm()) || isnan(vb.norm())) { +// printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); +// } + +// }else { +// printf("impossible reaching here\n"); +// } + +// // auto ma = verts("m",vi); +// // auto mb = verts("m",nvi); + +// // auto m = ma + mb; +// // auto momt = ma * ua + mb * ub; + + +// // auto va = (momt - CR * mb * ur) / m; +// // auto vb = (momt + CR * ma * ur) / m; + +// auto dpa = (va - ua) * (1 - t); +// auto dpb = (vb - ub) * (1 - t); + +// // auto dpa = (va - ua); +// // auto dpb = (vb - ub); +// // printf("find collision pair : %d %d\n",vi,nvi); +// atomic_add(exec_tag,&dp_count[vi],1); +// atomic_add(exec_tag,&dp_count[nvi],1); + +// for(int i = 0;i != 3;++i) { +// atomic_add(exec_tag,&dp_buffer[vi][i],dpa[i]); +// atomic_add(exec_tag,&dp_buffer[nvi][i],dpb[i]); +// } +// } +// }; + +// spBvh.iter_neighbors(bv,find_particle_sphere_collision_pairs); +// }); + +// // zs:Vector dpnorm{verts.get_allocator(),1}; +// // dpnorm.setVal(0); + +// cudaPol(zs::range(verts.size()),[ +// verts = proxy({},verts), +// xtag = zs::SmallString(xtag), +// relaxation_rate = relaxation_rate, +// pxtag = zs::SmallString(pxtag), +// exec_tag, +// // dpnorm = proxy(dpnorm), +// dp_count = proxy(dp_count), +// dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { +// if(dp_count[vi] == 0) +// return; +// auto minv = verts("minv",vi); +// auto dp = dp_buffer[vi] * relaxation_rate / (T)dp_count[vi]; +// // atomic_add(exec_tag,&dpnorm[0],dp.norm()); +// verts.tuple(dim_c<3>,xtag,vi) = verts.pack(dim_c<3>,xtag,vi) + dp; +// }); + +// // std::cout << "detangle_dp_norm : " << dpnorm.getVal(0) << std::endl; + +// set_output("zsparticles",zsparticles); +// } +// }; + +// ZENDEFNODE(XPBDDetangle3, {{{"zsparticles"}, +// // {"float","relaxation_strength","1"}, +// {"string","xtag","x"}, +// {"string","pxtag","px"}, +// {"string","Xtag","X"}, +// {"string","pscaleTag","pscale"}, +// {"float","restitution","0.1"}, +// {"float","relaxation","1"}, +// // {"float","friction","0"} +// }, +// {{"zsparticles"}}, +// { +// // {"string","ptag","x"} +// }, +// {"PBD"}}); + + +// make sure the mesh is intersection-free before using this node +// we use linear trajectory here, such that vel = (x - px) / dt +struct ExchangeLocalMomentum : INode { + // ray trace bvh + template + void buildSphereBvh(zs::CudaExecutionPolicy &pol, + TileVecT &verts, + const zs::SmallString& xtag, + const zs::SmallString& pscaleTag, + ZenoLinearBvh::lbvh_t& bvh) { + using namespace zs; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto space = execspace_e::cuda; + Vector bvs{verts.get_allocator(), verts.size()}; + + pol(range(verts.size()), + [verts = proxy({}, verts), + pscaleTag = zs::SmallString(pscaleTag), + bvs = proxy(bvs),xtag] ZS_LAMBDA(int vi) mutable { + auto pos = verts.template pack<3>(xtag, vi); + auto r = verts(pscaleTag,vi); + bv_t bv{pos - r,pos + r}; + bvs[vi] = bv; + }); + bvh.build(pol, bvs); + } + + virtual void apply() override { + using namespace zs; + using bvh_t = typename ZenoLinearBvh::lbvh_t; + using bv_t = typename bvh_t::Box; + + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + constexpr auto eps = (T)1e-7; + + auto zsparticles = get_input("zsparticles"); + auto xtag = get_input2("xtag"); + auto vtag = get_input2("vtag"); + auto Xtag = get_input2("Xtag"); + // auto ccdTag = get_input2("ccdTag"); + // x + auto pscaleTag = get_input2("pscaleTag"); + // auto thickness = get_input2("thickness"); + // auto friction = get_input2("friction"); + + auto& verts = zsparticles->getParticles(); + + auto restitution_rate = get_input2("restitution"); + auto relaxation_rate = get_input2("relaxation"); + + zs::Vector impulse_buffer{verts.get_allocator(),verts.size()}; + cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); + + zs::Vector impulse_count{verts.get_allocator(),verts.size()}; + cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + + auto spBvh = bvh_t{}; + buildSphereBvh(cudaPol,verts,xtag,pscaleTag,spBvh); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + pscaleTag = zs::SmallString(pscaleTag), + Xtag = zs::SmallString(Xtag), + vtag = zs::SmallString(vtag), + restitution_rate = restitution_rate, + exec_tag = exec_tag, + // thickness = thickness, + eps = eps, + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count), + spBvh = proxy(spBvh)] ZS_LAMBDA(int vi) mutable { + auto pos = verts.pack(dim_c<3>,xtag,vi); + auto vel = verts.pack(dim_c<3>,vtag,vi); + auto r = verts(pscaleTag,vi); + auto rpos = verts.pack(dim_c<3>,Xtag,vi); + bv_t bv{pos - r,pos + r}; + + auto find_particle_sphere_collision_pairs = [&] (int nvi) mutable { + if(vi >= nvi) + return; + + auto npos = verts.pack(dim_c<3>,xtag,nvi); + auto nvel = verts.pack(dim_c<3>,vtag,nvi); + auto nr = verts(pscaleTag,nvi); + auto nrpos = verts.pack(dim_c<3>,Xtag,nvi); + auto rdist = (nrpos - rpos).norm(); + + auto thickness = r + nr; + + if(rdist < thickness) + return; + + + auto dp = pos - npos; + auto dv = vel - nvel; + if(dp.dot(dv) > 0) + return; + + auto dist = (pos - npos).norm(); + // as the bvh is bounding box, for sphere intersection, extra distance check is needed + if(dist < (float)thickness) { + auto CR = restitution_rate; + + auto minv_a = verts("minv",vi); + auto minv_b = verts("minv",nvi); + if(minv_a + minv_b < (T)2 * eps) + return; + + auto ua = vel; + auto ub = nvel; + auto ur = ua - ub; + + vec3 va{},vb{}; + + // exchange momentum + if(minv_a > (T)eps) { + // ma / mb + auto ratio = minv_b / minv_a; + auto m = ratio + (T)1; + auto momt = ratio * ua + ub; + va = (momt - CR * ur) / m; + vb = (momt + CR * ratio * ur) / m; + + if(isnan(va.norm()) || isnan(vb.norm())) { + printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); + } + }else if(minv_b > (T)eps) { + // mb / ma + auto ratio = minv_a / minv_b; + auto m = ratio + (T)1; + auto momt = ua + ratio * ub; + va = (momt - CR * ratio * ur) / m; + vb = (momt + CR * ur) / m; + if(isnan(va.norm()) || isnan(vb.norm())) { + printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); + } + + }else { + printf("impossible reaching here\n"); + } + + auto ma = verts("m",vi); + auto mb = verts("m",nvi); + + auto dva = (va - ua) * ma; + auto dvb = (vb - ub) * mb; + + // auto dpa = (va - ua); + // auto dpb = (vb - ub); + // printf("find collision pair : %d %d\n",vi,nvi); + atomic_add(exec_tag,&impulse_count[vi],1); + atomic_add(exec_tag,&impulse_count[nvi],1); + + for(int i = 0;i != 3;++i) { + atomic_add(exec_tag,&impulse_buffer[vi][i],dva[i]); + atomic_add(exec_tag,&impulse_buffer[nvi][i],dvb[i]); + } + } + }; + + spBvh.iter_neighbors(bv,find_particle_sphere_collision_pairs); + }); + + // apply the impulse + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + // xtag = zs::SmallString(xtag), + relaxation_rate = relaxation_rate, + vtag = zs::SmallString(vtag), + exec_tag, + // dpnorm = proxy(dpnorm), + impulse_count = proxy(impulse_count), + impulse_buffer = proxy(impulse_buffer)] ZS_LAMBDA(int vi) mutable { + if(impulse_count[vi] == 0) + return; + auto minv = verts("minv",vi); + auto dv = minv * impulse_buffer[vi] * relaxation_rate / (T)impulse_count[vi]; + // atomic_add(exec_tag,&dpnorm[0],dp.norm()); + verts.tuple(dim_c<3>,vtag,vi) = verts.pack(dim_c<3>,vtag,vi) + dv; + }); + + set_output("zsparticles",zsparticles); + } + +}; + +ZENDEFNODE(ExchangeLocalMomentum, {{{"zsparticles"}, + // {"float","relaxation_strength","1"}, + {"string","xtag","x"}, + {"string","Xtag","X"}, + // {"string","pxtag","px"}, + {"string","vtag","v"}, + {"string","pscaleTag","pscale"}, + // {"float","thickness","0.0001"}, + {"float","restitution","0.1"}, + {"float","relaxation","1"}, + // {"float","friction","0"} + }, + {{"zsparticles"}}, + { + // {"string","ptag","x"} + }, + {"PBD"}}); + +struct VisualizePoints : INode { + virtual void apply() override { + using namespace zs; + using bvh_t = typename ZenoLinearBvh::lbvh_t; + using bv_t = typename bvh_t::Box; + + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + + auto zsparticles = get_input("zsparticles"); + auto xtag = get_input2("xtag"); + const auto& verts = zsparticles->getParticles(); + + zs::Vector points{verts.get_allocator(),verts.size()}; + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + points = proxy(points)] ZS_LAMBDA(int vi) mutable { + points[vi] = verts.pack(dim_c<3>,xtag,vi); + }); + + points = points.clone({zs::memsrc_e::host}); + auto points_field = std::make_shared(); + points_field->resize(verts.size()); + auto& sverts = points_field->attr("pos"); + constexpr auto omp_space = execspace_e::openmp; + auto ompPol = omp_exec(); + + ompPol(zs::range(verts.size()),[ + &sverts,points = proxy(points)] (int vi) mutable { + sverts[vi] = points[vi].to_array(); + }); + + set_output("points",std::move(points_field)); + } +}; + +ZENDEFNODE(VisualizePoints, { + {"zsparticles",{"string","xtag","x"}}, + {"points"}, + {{"float","scale","1.0"}}, + {"ZSGeometry"}, +}); + + + struct XPBDParticlesCollider : INode { // particle sphere bvh template @@ -1338,6 +2511,7 @@ struct XPBDParticlesCollider : INode { p0,minv0, p1,minv1, r, + (float)1, dp0,dp1)) { // auto rel_vel = vel0 - vel1 + dp0 - dp1; // auto tan_vel = rel_vel - nrm * rel_vel.dot(nrm); @@ -1411,23 +2585,10 @@ struct XPBDSolveSmooth : INode { auto zsparticles = get_input("zsparticles"); auto all_constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "all_constraints"); - // auto constraints = get_input("constraints"); - // auto dt = get_input2("dt"); auto ptag = get_param("ptag"); auto w = get_input2("relaxation_strength"); - // auto coffsets = constraints->readMeta("color_offset",zs::wrapt>{}); - // int nm_group = coffsets.size(); - auto& verts = zsparticles->getParticles(); - // auto& cquads = constraints->getQuadraturePoints(); - // auto category = constraints->category; - - // zs::Vector pv_buffer{verts.get_allocator(),verts.size()}; - // zs::Vector total_ghost_impulse_X{verts.get_allocator(),1}; - // zs::Vector total_ghost_impulse_Y{verts.get_allocator(),1}; - // zs::Vector total_ghost_impulse_Z{verts.get_allocator(),1}; - zs::Vector dp_buffer{verts.get_allocator(),verts.size() * 3}; cudaPol(zs::range(dp_buffer),[]ZS_LAMBDA(auto& v) {v = 0;}); @@ -1438,11 +2599,6 @@ struct XPBDSolveSmooth : INode { const auto& cquads = constraints->getQuadraturePoints(); auto category = constraints->readMeta(CONSTRAINT_KEY,wrapt{}); - // if(category == category_c::edge_length_constraint) - // continue; - - // std::cout << "computing smoothing for constraints" << std::endl; - cudaPol(zs::range(cquads.size()),[ verts = proxy({},verts), category, @@ -1470,6 +2626,7 @@ struct XPBDSolveSmooth : INode { p0,minv0, p1,minv1, r, + (float)1, dp0,dp1)) { for(int i = 0;i != 3;++i) @@ -1482,6 +2639,7 @@ struct XPBDSolveSmooth : INode { } } if(category == category_c::isometric_bending_constraint) { + return; auto quad = cquads.pack(dim_c<4>,"inds",ci,int_c); vec3 p[4] = {}; float minv[4] = {}; @@ -1493,26 +2651,44 @@ struct XPBDSolveSmooth : INode { auto Q = cquads.pack(dim_c<4,4>,"Q",ci); vec3 dp[4] = {}; + float lambda = 0; CONSTRAINT::solve_IsometricBendingConstraint( p[0],minv[0], p[1],minv[1], p[2],minv[2], p[3],minv[3], - Q,dp[0],dp[1],dp[2],dp[3]); + Q, + (float)1, + dp[0], + dp[1], + dp[2], + dp[3]); + + auto has_nan = false; + for(int i = 0;i != 4;++i) + if(zs::isnan(dp[i].norm())) + has_nan = true; + if(has_nan) { + printf("nan dp detected : %f %f %f %f %f %f %f %f\n", + (float)p[0].norm(), + (float)p[1].norm(), + (float)p[2].norm(), + (float)p[3].norm(), + (float)dp[0].norm(), + (float)dp[1].norm(), + (float)dp[2].norm(), + (float)dp[3].norm()); + return; + } for(int i = 0;i != 4;++i) for(int j = 0;j != 3;++j) atomic_add(exec_tag,&dp_buffer[quad[i] * 3 + j],dp[i][j]); for(int i = 0;i != 4;++i) - atomic_add(exec_tag,&dp_count[quad[1]],(int)1); + atomic_add(exec_tag,&dp_count[quad[i]],(int)1); } }); } - // zs::Vector avg_smooth{verts.get_allocator(),1}; - // avg_smooth.setVal(0); - // cudaPol(zs::range(dp_buffer),[avg_smooth = proxy(avg_smooth),exec_tag] ZS_LAMBDA(const auto& dp) mutable {atomic_add(exec_tag,&avg_smooth[0],dp);}); - // std::cout << "avg_smooth : " << avg_smooth.getVal(0) << std::endl; - cudaPol(zs::range(verts.size()),[ verts = proxy({},verts), ptag = zs::SmallString(ptag),w, diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh index 7fe9d0b0cc..3dda9544ba 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh @@ -8,9 +8,9 @@ namespace zeno { namespace CONSTRAINT { constexpr bool solve_DistanceConstraint( const VECTOR3d &p0, SCALER invMass0, const VECTOR3d &p1, SCALER invMass1, - const SCALER restLength, - const SCALER stiffness, - const SCALER dt, + const SCALER& restLength, + const SCALER& stiffness, + const SCALER& dt, SCALER& lambda, VECTOR3d &corr0, VECTOR3d &corr1) { @@ -58,6 +58,7 @@ namespace zeno { namespace CONSTRAINT { const VECTOR3d &p0, SCALER invMass0, const VECTOR3d &p1, SCALER invMass1, const SCALER expectedDistance, + const SCALER stiffness, VECTOR3d &corr0, VECTOR3d &corr1){ VECTOR3d diff = p0 - p1; SCALER distance = diff.norm(); @@ -66,7 +67,8 @@ namespace zeno { namespace CONSTRAINT { VECTOR3d gradient = diff / (distance + static_cast(1e-6)); SCALER denom = invMass0 + invMass1; SCALER lambda = (distance - expectedDistance) /denom; - auto common = lambda * gradient; + auto common = stiffness * lambda * gradient; + // auto common = lambda * gradient; corr0 = -invMass0 * common; corr1 = invMass1 * common; return false; @@ -303,7 +305,7 @@ namespace zeno { namespace CONSTRAINT { // for(int i = 0;i != 4;++i) // printf("gradC[%d] : %f %f %f\n",i,(float)gradC[i][0],(float)gradC[i][1],(float)gradC[i][2]); - if (zs::abs(sum_normGradC) > eps) + if (sum_normGradC > eps) { // compute impulse-based scaling factor SCALER delta_lambda = -(energy + alpha * lambda) / sum_normGradC; @@ -333,6 +335,7 @@ namespace zeno { namespace CONSTRAINT { const VECTOR3d& p2, SCALER invMass2, const VECTOR3d& p3, SCALER invMass3, const MATRIX4d& Q, + const SCALER& stiffness, VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3){ constexpr SCALER eps = static_cast(1e-6); const VECTOR3d x[4] = { p2, p3, p0, p1 }; @@ -363,15 +366,16 @@ namespace zeno { namespace CONSTRAINT { } // exit early if required - if (zs::abs(sum_normGradC) > eps) + if (sum_normGradC > eps) { // compute impulse-based scaling factor - const SCALER s = -(energy) / sum_normGradC; + const SCALER s = -(energy)/ sum_normGradC; + + corr0 = stiffness * (s * invMass[2]) * gradC[2]; + corr1 = stiffness * (s * invMass[3]) * gradC[3]; + corr2 = stiffness * (s * invMass[0]) * gradC[0]; + corr3 = stiffness * (s * invMass[1]) * gradC[1]; - corr0 = (s * invMass[2]) * gradC[2]; - corr1 = (s * invMass[3]) * gradC[3]; - corr2 = (s * invMass[0]) * gradC[0]; - corr3 = (s * invMass[1]) * gradC[1]; return true; }else { From 401b111db9d9f04574b1af659cbd9d02de4e0e76 Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Wed, 13 Sep 2023 15:39:25 +0800 Subject: [PATCH 16/21] init surf edge while building halfedge structure --- projects/CuLagrange/geometry/Topology.cu | 301 ++++++++++++++--------- 1 file changed, 191 insertions(+), 110 deletions(-) diff --git a/projects/CuLagrange/geometry/Topology.cu b/projects/CuLagrange/geometry/Topology.cu index 31325dea65..da09523af0 100644 --- a/projects/CuLagrange/geometry/Topology.cu +++ b/projects/CuLagrange/geometry/Topology.cu @@ -24,133 +24,168 @@ struct BuildSurfaceHalfEdgeStructure : zeno::INode { using namespace zs; using vec2i = zs::vec; using vec3i = zs::vec; + constexpr auto space = zs::execspace_e::cuda; auto zsparticles = get_input("zsparticles"); - if(!zsparticles->hasAuxData(ZenoParticles::s_surfTriTag) && zsparticles->category == ZenoParticles::category_e::tet) - throw std::runtime_error("the input tet zsparticles has no surface tris"); - // if(!zsparticles->hasAuxData(ZenoParticles::s_surfEdgeTag)) - // throw std::runtime_error("the input zsparticles has no surface lines"); - if(!zsparticles->hasAuxData(ZenoParticles::s_surfVertTag) && zsparticles->category == ZenoParticles::category_e::tet) - throw std::runtime_error("the input tet zsparticles has no surface points"); - - auto& tris = zsparticles->category == ZenoParticles::category_e::tet ? (*zsparticles)[ZenoParticles::s_surfTriTag] : zsparticles->getQuadraturePoints(); - // auto& lines = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; - auto& points = zsparticles->category == ZenoParticles::category_e::tet ? (*zsparticles)[ZenoParticles::s_surfVertTag] : zsparticles->getParticles(); - - auto& halfEdge = (*zsparticles)[ZenoParticles::s_surfHalfEdgeTag]; - halfEdge = typename ZenoParticles::particles_t({{"local_vertex_id",1},{"to_face",1},{"opposite_he",1},{"next_he",1}}, - tris.size() * 3,zs::memsrc_e::device,0); + if(!zsparticles->hasAuxData(ZenoParticles::s_surfTriTag) && zsparticles->category == ZenoParticles::category_e::tet) + throw std::runtime_error("the input tet zsparticles has no surface tris"); + // if(!zsparticles->hasAuxData(ZenoParticles::s_surfEdgeTag)) + // throw std::runtime_error("the input zsparticles has no surface lines"); + if(!zsparticles->hasAuxData(ZenoParticles::s_surfVertTag) && zsparticles->category == ZenoParticles::category_e::tet) + throw std::runtime_error("the input tet zsparticles has no surface points"); + + auto& tris = zsparticles->category == ZenoParticles::category_e::tet ? (*zsparticles)[ZenoParticles::s_surfTriTag] : zsparticles->getQuadraturePoints(); + // auto& lines = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + auto& points = zsparticles->category == ZenoParticles::category_e::tet ? (*zsparticles)[ZenoParticles::s_surfVertTag] : zsparticles->getParticles(); + + auto& halfEdge = (*zsparticles)[ZenoParticles::s_surfHalfEdgeTag]; + halfEdge = typename ZenoParticles::particles_t({{"local_vertex_id",1},{"to_face",1},{"opposite_he",1},{"next_he",1}}, + tris.size() * 3,zs::memsrc_e::device,0); - auto cudaPol = zs::cuda_exec(); + auto cudaPol = zs::cuda_exec(); - points.append_channels(cudaPol,{{"he_inds",1}}); - // lines.append_channels(cudaPol,{{"he_inds",1}}); - tris.append_channels(cudaPol,{{"he_inds",1}}); + points.append_channels(cudaPol,{{"he_inds",1}}); + // lines.append_channels(cudaPol,{{"he_inds",1}}); + tris.append_channels(cudaPol,{{"he_inds",1}}); #if 0 - constexpr auto space = zs::execspace_e::cuda; - - TILEVEC_OPS::fill(cudaPol,halfEdge,"to_vertex",reinterpret_bits((int)-1)); - TILEVEC_OPS::fill(cudaPol,halfEdge,"to_face",reinterpret_bits((int)-1)); - TILEVEC_OPS::fill(cudaPol,halfEdge,"to_edge",reinterpret_bits((int)-1)); - TILEVEC_OPS::fill(cudaPol,halfEdge,"opposite_he",reinterpret_bits((int)-1)); - TILEVEC_OPS::fill(cudaPol,halfEdge,"next_he",reinterpret_bits((int)-1)); - - // we might also need a space hash structure here, map from [i1,i2]->[ej] - bcht,32> de2fi{halfEdge.get_allocator(),halfEdge.size()}; - - cudaPol(zs::range(tris.size()), [ - tris = proxy({},tris),de2fi = proxy(de2fi),halfEdge = proxy({},halfEdge)] ZS_LAMBDA(int ti) mutable { - auto fe_inds = tris.pack(dim_c<3>,"fe_inds",ti).reinterpret_bits(int_c); - auto tri = tris.pack(dim_c<3>,"inds",ti).reinterpret_bits(int_c); - - vec3i nos{}; - for(int i = 0;i != 3;++i) { - if(auto no = de2fi.insert(vec2i{tri[i],tri[(i+1) % 3]});no >= 0){ - nos[i] = no; - halfEdge("to_vertex",no) = reinterpret_bits(tri[i]); - halfEdge("to_face",no) = reinterpret_bits(ti); - halfEdge("to_edge",no) = reinterpret_bits(fe_inds[i]); - // halfEdge("next_he",no) = ti * 3 + (i+1) % 3; - } else { - // some error happen - - } - } - for(int i = 0;i != 3;++i) - halfEdge("next_he",nos[i]) = reinterpret_bits(nos[(i+1) % 3]); - }); - cudaPol(zs::range(halfEdge.size()), - [halfEdge = proxy({},halfEdge),de2fi = proxy(de2fi)] ZS_LAMBDA(int hei) mutable { - auto idx0 = reinterpret_bits(halfEdge("to_vertex",hei)); - auto nexthei = reinterpret_bits(halfEdge("next_he",hei)); - auto idx1 = reinterpret_bits(halfEdge("to_vertex",nexthei)); - if(auto no = de2fi.query(vec2i{idx1,idx0});no >= 0) - halfEdge("opposite_he",hei) = reinterpret_bits(no); - else - halfEdge("opposite_he",hei) = reinterpret_bits((int)-1); - }); + // constexpr auto space = zs::execspace_e::cuda; + + TILEVEC_OPS::fill(cudaPol,halfEdge,"to_vertex",reinterpret_bits((int)-1)); + TILEVEC_OPS::fill(cudaPol,halfEdge,"to_face",reinterpret_bits((int)-1)); + TILEVEC_OPS::fill(cudaPol,halfEdge,"to_edge",reinterpret_bits((int)-1)); + TILEVEC_OPS::fill(cudaPol,halfEdge,"opposite_he",reinterpret_bits((int)-1)); + TILEVEC_OPS::fill(cudaPol,halfEdge,"next_he",reinterpret_bits((int)-1)); + + // we might also need a space hash structure here, map from [i1,i2]->[ej] + bcht,32> de2fi{halfEdge.get_allocator(),halfEdge.size()}; + + cudaPol(zs::range(tris.size()), [ + tris = proxy({},tris),de2fi = proxy(de2fi),halfEdge = proxy({},halfEdge)] ZS_LAMBDA(int ti) mutable { + auto fe_inds = tris.pack(dim_c<3>,"fe_inds",ti).reinterpret_bits(int_c); + auto tri = tris.pack(dim_c<3>,"inds",ti).reinterpret_bits(int_c); + + vec3i nos{}; + for(int i = 0;i != 3;++i) { + if(auto no = de2fi.insert(vec2i{tri[i],tri[(i+1) % 3]});no >= 0){ + nos[i] = no; + halfEdge("to_vertex",no) = reinterpret_bits(tri[i]); + halfEdge("to_face",no) = reinterpret_bits(ti); + halfEdge("to_edge",no) = reinterpret_bits(fe_inds[i]); + // halfEdge("next_he",no) = ti * 3 + (i+1) % 3; + } else { + // some error happen + + } + } + for(int i = 0;i != 3;++i) + halfEdge("next_he",nos[i]) = reinterpret_bits(nos[(i+1) % 3]); + }); + cudaPol(zs::range(halfEdge.size()), + [halfEdge = proxy({},halfEdge),de2fi = proxy(de2fi)] ZS_LAMBDA(int hei) mutable { + auto idx0 = reinterpret_bits(halfEdge("to_vertex",hei)); + auto nexthei = reinterpret_bits(halfEdge("next_he",hei)); + auto idx1 = reinterpret_bits(halfEdge("to_vertex",nexthei)); + if(auto no = de2fi.query(vec2i{idx1,idx0});no >= 0) + halfEdge("opposite_he",hei) = reinterpret_bits(no); + else + halfEdge("opposite_he",hei) = reinterpret_bits((int)-1); + }); - points.append_channels(cudaPol,{{"he_inds",1}}); - lines.append_channels(cudaPol,{{"he_inds",1}}); - tris.append_channels(cudaPol,{{"he_inds",1}}); + points.append_channels(cudaPol,{{"he_inds",1}}); + lines.append_channels(cudaPol,{{"he_inds",1}}); + tris.append_channels(cudaPol,{{"he_inds",1}}); + + cudaPol(zs::range(lines.size()),[ + lines = proxy({},lines),de2fi = proxy(de2fi)] ZS_LAMBDA(int li) mutable { + auto linds = lines.pack(dim_c<2>,"inds",li).reinterpret_bits(int_c); + if(auto no = de2fi.query(vec2i{linds[0],linds[1]});no >= 0){ + lines("he_inds",li) = reinterpret_bits((int)no); + }else { + // some algorithm bug + } + }); - cudaPol(zs::range(lines.size()),[ - lines = proxy({},lines),de2fi = proxy(de2fi)] ZS_LAMBDA(int li) mutable { - auto linds = lines.pack(dim_c<2>,"inds",li).reinterpret_bits(int_c); - if(auto no = de2fi.query(vec2i{linds[0],linds[1]});no >= 0){ - lines("he_inds",li) = reinterpret_bits((int)no); - }else { - // some algorithm bug - } - }); + if(!tris.hasProperty("fp_inds") || tris.getPropertySize("fp_inds") != 3) { + throw std::runtime_error("the tris has no fp_inds channel"); + } - if(!tris.hasProperty("fp_inds") || tris.getPropertySize("fp_inds") != 3) { - throw std::runtime_error("the tris has no fp_inds channel"); - } + cudaPol(zs::range(tris.size()),[ + tris = proxy({},tris),de2fi = proxy(de2fi)] ZS_LAMBDA(int ti) mutable { - cudaPol(zs::range(tris.size()),[ - tris = proxy({},tris),de2fi = proxy(de2fi)] ZS_LAMBDA(int ti) mutable { + auto tri = tris.pack(dim_c<3>,"inds",ti).reinterpret_bits(int_c); + if(auto no = de2fi.query(vec2i{tri[0],tri[1]});no >= 0){ + tris("he_inds",ti) = reinterpret_bits((int)no); + }else { + // some algorithm bug + printf("could not find half edge : %d %d\n",tri[0],tri[1]); + } + // auto tinds = tris.pack(dim_c<3>,"fp_inds",ti).reinterpret_bits(int_c); + // for(int i = 0;i != 3;++i) { + // if(auto no = de2fi.query(vec2i{tri[i],tri[(i+1) % 3]});no >= 0){ + // points("he_inds",tinds[i]) = reinterpret_bits((int)no); + // }else { + // // some algorithm bug + // printf("could not find half edge : %d %d\n",tri[i],tri[(i+1) % 3]); + // } + // } + }); - auto tri = tris.pack(dim_c<3>,"inds",ti).reinterpret_bits(int_c); - if(auto no = de2fi.query(vec2i{tri[0],tri[1]});no >= 0){ - tris("he_inds",ti) = reinterpret_bits((int)no); + cudaPol(zs::range(tris.size()),[ + points = proxy({},points),tris = proxy({},tris),de2fi = proxy(de2fi)] ZS_LAMBDA(int ti) mutable { + auto tinds = tris.pack(dim_c<3>,"fp_inds",ti).reinterpret_bits(int_c); + auto tri = tris.pack(dim_c<3>,"inds",ti).reinterpret_bits(int_c); + for(int i = 0;i != 3;++i) { + if(auto no = de2fi.query(vec2i{tri[i],tri[(i+1) % 3]});no >= 0){ + points("he_inds",tinds[i]) = reinterpret_bits((int)no); }else { // some algorithm bug - printf("could not find half edge : %d %d\n",tri[0],tri[1]); - } - // auto tinds = tris.pack(dim_c<3>,"fp_inds",ti).reinterpret_bits(int_c); - // for(int i = 0;i != 3;++i) { - // if(auto no = de2fi.query(vec2i{tri[i],tri[(i+1) % 3]});no >= 0){ - // points("he_inds",tinds[i]) = reinterpret_bits((int)no); - // }else { - // // some algorithm bug - // printf("could not find half edge : %d %d\n",tri[i],tri[(i+1) % 3]); - // } - // } - }); - - cudaPol(zs::range(tris.size()),[ - points = proxy({},points),tris = proxy({},tris),de2fi = proxy(de2fi)] ZS_LAMBDA(int ti) mutable { - auto tinds = tris.pack(dim_c<3>,"fp_inds",ti).reinterpret_bits(int_c); - auto tri = tris.pack(dim_c<3>,"inds",ti).reinterpret_bits(int_c); - for(int i = 0;i != 3;++i) { - if(auto no = de2fi.query(vec2i{tri[i],tri[(i+1) % 3]});no >= 0){ - points("he_inds",tinds[i]) = reinterpret_bits((int)no); - }else { - // some algorithm bug - printf("could not find half edge : %d %d\n",tri[i],tri[(i+1) % 3]); - } - } - }); + printf("could not find half edge : %d %d\n",tri[i],tri[(i+1) % 3]); + } + } + }); #else - if(!build_surf_half_edge(cudaPol,tris,points,halfEdge)) - throw std::runtime_error("fail building surf half edge"); + if(!build_surf_half_edge(cudaPol,tris,points,halfEdge)) + throw std::runtime_error("fail building surf half edge"); #endif - set_output("zsparticles",zsparticles); - // zsparticles->setMeta("de2fi",std::move()) + zs::bht edgeSet{tris.get_allocator(),tris.size() * 3}; + edgeSet.reset(cudaPol,true); + cudaPol(zs::range(halfEdge.size()),[ + halfedges = proxy({},halfEdge), + edgeSet = proxy(edgeSet), + tris = proxy({},tris)] ZS_LAMBDA(int hi) mutable { + auto ti = zs::reinterpret_bits(halfedges("to_face",hi)); + auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); + auto local_idx = zs::reinterpret_bits(halfedges("local_vertex_id",hi)); + zs::vec edge{tri[local_idx],tri[(local_idx + 1) % 3]}; + + auto ohi = zs::reinterpret_bits(halfedges("opposite_he",hi)); + if(ohi >= 0 && edge[0] > edge[1]) + return; + + edgeSet.insert(hi); + }); + + auto &surfEdges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + surfEdges = typename ZenoParticles::particles_t({{"inds", 2},{"he_inds",1}}, edgeSet.size(),zs::memsrc_e::device,0); + cudaPol(zip(zs::range(edgeSet.size()),edgeSet._activeKeys),[ + halfedges = proxy({},halfEdge), + surfEdges = proxy({},surfEdges), + tris = proxy({},tris)] ZS_LAMBDA(auto ei,const auto& hi_vec) mutable { + auto hi = hi_vec[0]; + auto ti = zs::reinterpret_bits(halfedges("to_face",hi)); + auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); + auto local_idx = zs::reinterpret_bits(halfedges("local_vertex_id",hi)); + zs::vec edge{tri[local_idx],tri[(local_idx + 1) % 3]}; + + surfEdges.tuple(dim_c<2>,"inds",ei) = edge.reinterpret_bits(float_c); + surfEdges("he_inds",ei) = reinterpret_bits((int)hi); + }); + + set_output("zsparticles",zsparticles); + // zsparticles->setMeta("de2fi",std::move()) } }; @@ -167,6 +202,7 @@ struct BuildTetrahedraHalfFacet : zeno::INode { using namespace zs; auto cudaPol = zs::cuda_exec(); + auto zsparticles = get_input("zsparticles"); auto& tets = zsparticles->getQuadraturePoints(); @@ -187,6 +223,51 @@ ZENDEFNODE(BuildTetrahedraHalfFacet, {{{"zsparticles"}}, {}, {"ZSGeometry"}}); +struct BuildSurfaceLinesStructure : zeno::INode { + using T = float; + virtual void apply() override { + using namespace zs; + using vec2i = zs::vec; + using vec3i = zs::vec; + auto cudaPol = zs::cuda_exec(); + constexpr auto space = zs::execspace_e::cuda; + + auto zsparticles = get_input("zsparticles"); + if(!zsparticles->hasAuxData(ZenoParticles::s_surfTriTag) && zsparticles->category == ZenoParticles::category_e::tet) + throw std::runtime_error("the input tet zsparticles has no surface tris"); + + auto& tris = zsparticles->category == ZenoParticles::category_e::tet ? (*zsparticles)[ZenoParticles::s_surfTriTag] : zsparticles->getQuadraturePoints(); + zs::bht edgeSet{tris.get_allocator(),tris.size() * 3}; + edgeSet.reset(cudaPol,true); + + cudaPol(zs::range(tris.size()),[ + tris = proxy({},tris), + edgeSet = proxy(edgeSet)] ZS_LAMBDA(int ti) mutable { + auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); + for(int i = 0;i != 3;++i) { + auto idx0 = tri[(i + 0) % 3]; + auto idx1 = tri[(i + 1) % 3]; + if(idx0 < idx1) + edgeSet.insert(vec2i{idx0,idx1}); + } + }); + + auto &surfEdges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + surfEdges = typename ZenoParticles::particles_t({{"inds", 2}}, edgeSet.size(),zs::memsrc_e::device,0); + cudaPol(zip(zs::range(edgeSet.size()),edgeSet._activeKeys),[ + surfEdges = proxy({},surfEdges)] ZS_LAMBDA(auto ei,const auto& pair) mutable { + surfEdges.tuple(dim_c<2>,"inds",ei) = pair.reinterpret_bits(float_c); + }); + + set_output("zsparticles",zsparticles); + } +}; + +ZENDEFNODE(BuildSurfaceLinesStructure, {{{"zsparticles"}}, + {{"zsparticles"}}, + {}, + {"ZSGeometry"}}); + struct VisualTetrahedraHalfFacet : zeno::INode { using T = float; // using dtiles_t = zs::TileVector; From 89b976ebbbf62c7f336ca119740bbef0e6ab4582 Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Thu, 14 Sep 2023 07:48:51 +0800 Subject: [PATCH 17/21] resolve precision error --- .../collision_energy/evaluate_collision.hpp | 295 +++++++++--------- projects/CuLagrange/pbd/Pipeline.cu | 186 ++++++++++- 2 files changed, 316 insertions(+), 165 deletions(-) diff --git a/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp b/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp index b6c98e45d1..7ead798641 100644 --- a/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp +++ b/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp @@ -955,7 +955,7 @@ void calc_imminent_self_PT_collision_impulse(Pol& pol, CollisionBuffer& imminent_collision_buffer) { using namespace zs; constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; - constexpr auto exec_tag = wrapv{}; + // constexpr auto exec_tag = wrapv{}; using vec3 = zs::vec; using vec4 = zs::vec; using vec4i = zs::vec; @@ -1015,7 +1015,7 @@ void calc_imminent_self_PT_collision_impulse(Pol& pol, auto bary_sum = zs::abs(bary_centric[0]) + zs::abs(bary_centric[1]) + zs::abs(bary_centric[2]); if(bary_sum > (T)(1.0 + eps * 10)) { - return; + // return; vec3 bnrms[3] = {}; auto hi = zs::reinterpret_bits(tris("he_inds",ti)); for(int i = 0;i != 3;++i){ @@ -1113,7 +1113,7 @@ void calc_imminent_self_EE_collision_impulse(Pol& pol, CollisionBuffer& imminent_collision_buffer) { using namespace zs; constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; - constexpr auto exec_tag = wrapv{}; + // constexpr auto exec_tag = wrapv{}; using vec2 = zs::vec; using vec3 = zs::vec; using vec4 = zs::vec; @@ -1263,164 +1263,148 @@ void calc_imminent_self_EE_collision_impulse(Pol& pol, -// template -// void calc_continous_self_PT_collision_impulse(Pol& pol, -// const PosTileVec& verts,const zs::SmallString& xtag,const zs::SmallString& vtag, -// const TrisTileVec& tris, -// const REAL& thickness, -// TriBvh& triCCDBvh, -// bool refit_bvh, -// int buffer_offset, -// ImpulseBuffer& impulse_buffer) { -// using namespace zs; -// constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; -// constexpr auto exec_tag = wrapv{}; -// using vec2 = zs::vec; -// using vec3 = zs::vec; -// using vec4 = zs::vec; -// using vec2i = zs::vec; -// using vec4i = zs::vec; -// constexpr auto eps = (T)1e-6; -// constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; - -// zs::bht csPT{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; -// csPT.reset(pol,true); - -// // auto triCCDBvh = bvh_t{}; -// auto bvs = retrieve_bounding_volumes(pol,verts,tris,verts,wrapv<3>{},(T)1.0,thickness/(T)2,xtag,vtag); -// if(refit_bvh) -// triCCDBvh.refit(pol,bvs); -// else -// triCCDBvh.build(pol,bvs); - -// pol(zs::range(tris.size()),[ -// xtag = xtag, -// vtag = vtag, -// verts = proxy({},verts), -// tris = proxy({},tris), -// csPT = proxy(csPT), -// thickness = thickness, -// eps = eps, -// bvh = proxy(triCCDBvh)] ZS_LAMBDA(int ei) mutable { -// auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); -// vec3 pas[2] = {}; -// vec3 vas[2] = {}; -// for(int i = 0;i != 2;++i) { -// pas[i] = verts.pack(dim_c<3>,xtag,ea[i]); -// vas[i] = verts.pack(dim_c<3>,vtag,ea[i]); -// } - -// auto bv = edgeBvs[ei]; - -// auto do_close_proximity_detection = [&](int nei) mutable { -// if(ei >= nei) -// return; -// auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); -// for(int i = 0;i != 2;++i){ -// if(eb[i] == ea[0] || eb[i] == ea[1]) -// return; -// } - -// bool has_dynamic_points = false; -// for(int i = 0;i != 2;++i) { -// if(verts("minv",ea[i]) > eps) -// has_dynamic_points = true; -// if(verts("minv",eb[i]) > eps) -// has_dynamic_points = true; -// } -// if(!has_dynamic_points) -// return; - -// vec3 pbs[2] = {}; -// vec3 vbs[2] = {}; -// for(int i = 0;i != 2;++i) { -// pbs[i] = verts.pack(dim_c<3>,xtag,eb[i]); -// vbs[i] = verts.pack(dim_c<3>,vtag,eb[i]); -// } - -// auto alpha = (T)1.0; -// ee_accd(pas[0],pas[1],pbs[0],pbs[1],vas[0],vas[1],vbs[0],vbs[1],(T)0.2,thickness,alpha); - - -// vec4i inds{ea[0],ea[1],eb[0],eb[1]}; -// vec3 ps[4] = {}; -// for(int i = 0;i != 4;++i) -// ps[i] = verts.pack(dim_c<3>,xtag,inds[i]) + verts.pack(dim_c<3>,vtag,inds[i]); - -// vec3 int_a{},int_b{}; -// COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[0],ps[1],int_a,int_b); +template +void calc_continous_self_PT_collision_impulse(Pol& pol, + const InverseMassTileVec& invMass, + const PosTileVec& verts,const zs::SmallString& xtag,const zs::SmallString& vtag, + const TrisTileVec& tris, + const THICKNESS_REAL& thickness, + TriBvh& triCCDBvh, + bool refit_bvh, + ImpulseBuffer& impulse_buffer, + ImpulseCount& impulse_count) { + using namespace zs; + constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; + // constexpr auto exec_tag = wrapv{}; -// auto la = (ps[0] - ps[1]).norm(); + using vec2 = zs::vec; + using vec3 = zs::vec; + using vec4 = zs::vec; + using vec2i = zs::vec; + using vec4i = zs::vec; + constexpr auto eps = (T)1e-6; + // constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; -// auto ra = (ps[0] - int_a).norm() / (ps[0] - ps[1]).norm();; -// auto rb = (ps[2] - int_b).norm() / (ps[2] - ps[3]).norm();; + // zs::bht csPT{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; + // csPT.reset(pol,true); -// if(ra < 10 * eps || ra > 1 - 10 * eps) -// return; -// if(rb < 10 * eps || rb > 1 - 10 * eps) -// return; + // auto triCCDBvh = bvh_t{}; + auto bvs = retrieve_bounding_volumes(pol,verts,tris,verts,wrapv<3>{},(T)1.0,(T)0,xtag,vtag); + if(refit_bvh) + triCCDBvh.refit(pol,bvs); + else + triCCDBvh.build(pol,bvs); -// csEE.insert(vec2i{ei,nei}); -// }; -// edgeBvh.iter_neighbors(bv,do_close_proximity_detection); -// }); + pol(zs::range(verts.size()),[ + invMass = proxy({},invMass), + xtag = xtag, + vtag = vtag, + verts = proxy({},verts), + tris = proxy({},tris), + // csPT = proxy(csPT), + thickness = thickness, + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count), + eps = eps, + exec_tag = wrapv{}, + bvh = proxy(triCCDBvh)] ZS_LAMBDA(int vi) mutable { + auto p = verts.pack(dim_c<3>,xtag,vi); + auto v = verts.pack(dim_c<3>,vtag,vi); -// printf("nm_EE_collision_pairs : %d %d\n",(int)csEE.size(),(int)edges.size()); + bv_t bv{p, p + v}; + // bv._min -= thickness * (T).5; + // bv._max += thickness * (T).5; -// imminent_collision_buffer.resize(csEE.size() + buffer_offset); -// pol(zip(zs::range(csEE.size()),csEE._activeKeys),[ -// verts = proxy({},verts), -// edges = proxy({},edges), -// buffer_offset = buffer_offset, -// xtag = xtag, -// vtag = vtag, -// imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(auto ci,const auto& pair) mutable { -// auto ei = pair[0]; -// auto nei = pair[1]; + vec3 ps[4] = {}; + ps[3] = p; + vec3 vs[4] = {}; + vs[3] = v; + vec4 bary{0,0,0,1}; + vec4i inds{-1,-1,-1,vi}; -// auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); -// auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); -// vec4i inds{ea[0],ea[1],eb[0],eb[1]}; + auto do_close_proximity_detection = [&](int ti) mutable { + auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); + for(int i = 0;i != 3;++i) + if(tri[i] == vi) + return; -// vec3 ps[4] = {}; -// vec3 vs[4] = {}; -// for(int i = 0;i != 4;++i) { -// ps[i] = verts.pack(dim_c<3>,xtag,inds[i]); -// vs[i] = verts.pack(dim_c<3>,vtag,inds[i]); -// } + bool has_dynamic_points = verts("minv",vi) > eps; + for(int i = 0;i != 3;++i) { + if(invMass("minv",tri[i]) > eps) + has_dynamic_points = true; + } + if(!has_dynamic_points) + return; -// vec3 int_a{},int_b{}; -// COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[2],ps[3],int_a,int_b); + for(int i = 0;i != 3;++i) { + ps[i] = verts.pack(dim_c<3>,xtag,tri[i]); + vs[i] = verts.pack(dim_c<3>,vtag,tri[i]); + inds[i] = tri[i]; + } -// auto la = (ps[0] - ps[1]).norm(); -// auto lb = (ps[2] - ps[3]).norm(); + auto alpha = (T)1.0; + if(!pt_accd(ps[3],ps[0],ps[1],ps[2],vs[3],vs[0],vs[1],vs[2],(T)0.2,(T)0,alpha)) + return; -// auto ra = (ps[0] - int_a).norm() / la; -// auto rb = (ps[2] - int_b).norm() / lb; + vec3 nps[4] = {}; + for(int i = 0;i != 4;++i) + nps[i] = ps[i] + vs[i] * alpha; -// vec4 bary{ra - 1,-ra,1 - rb,rb}; + vec3 bary_centric{}; + LSL_GEO::pointBaryCentric(nps[0],nps[1],nps[2],nps[3],bary_centric); + auto ori_bary = bary_centric; + for(int i = 0;i != 3;++i) + bary_centric[i] = bary_centric[i] < 0 ? 0 : bary_centric[i]; + auto barySum = bary_centric[0] + bary_centric[1] + bary_centric[2]; + bary_centric = bary_centric / barySum; -// auto pr = vec3::zeros(); -// auto vr = vec3::zeros(); -// for(int i = 0;i != 4;++i) { -// pr += bary[i] * ps[i]; -// vr += bary[i] * vs[i]; -// } + for(int i = 0;i != 3;++i) + bary[i] = -bary_centric[i]; -// auto collision_nrm = pr.normalized(); -// auto relative_normal_velocity = vr.dot(collision_nrm); + auto rv = vec3::zeros(); + for(int i = 0;i != 4;++i) { + rv += vs[i] * bary[i]; + } + + auto collision_nrm = LSL_GEO::facet_normal(nps[0],nps[1],nps[2]); + if(collision_nrm.dot(rv) > 0) + collision_nrm *= -1; + auto rv_nrm = collision_nrm.dot(rv); -// imminent_collision_buffer.tuple(dim_c<4>,"bary",ci + buffer_offset) = bary; -// imminent_collision_buffer.tuple(dim_c<4>,"inds",ci + buffer_offset) = inds.reinterpret_bits(float_c); -// auto I = relative_normal_velocity < 0 ? relative_normal_velocity : (T)0; -// imminent_collision_buffer.tuple(dim_c<3>,"impulse",ci + buffer_offset) = -collision_nrm * I; -// }); -// } + auto cm = (T).0; + for(int i = 0;i != 4;++i) + cm += bary[i] * bary[i] * invMass("minv",inds[i]); + if(cm < eps) + return; + + auto impulse = -collision_nrm * rv_nrm * ((T)1 - alpha); + // printf("find collision pairs[%d %d] with ccd : %f impulse : \n%f %f %f\n%f %fbary : %f %f %f %f\nminv : %f %f %f %f\n",ti,vi,(float)alpha, + // (float)impulse[0],(float)impulse[1],(float)impulse[2], + // (float)rv_nrm,(float)cm, + // (float)bary[0],(float)bary[1],(float)bary[2],(float)bary[3], + // (float)invMass("minv",inds[0]), + // (float)invMass("minv",inds[1]), + // (float)invMass("minv",inds[2]), + // (float)invMass("minv",inds[3])); + + for(int i = 0;i != 4;++i) { + auto beta = (bary[i] * invMass("minv",inds[i])) / cm; + atomic_add(exec_tag,&impulse_count[inds[i]],1); + for(int d = 0;d != 3;++d) + atomic_add(exec_tag,&impulse_buffer[inds[i]][d],impulse[d] * beta); + } + }; + bvh.iter_neighbors(bv,do_close_proximity_detection); + }); +} // template{}; + // constexpr auto exec_tag = wrapv{}; using vec3 = zs::vec; constexpr auto eps = (T)1e-6; @@ -1600,7 +1584,7 @@ void apply_impulse(Pol& pol, verts = proxy({},verts), vtag = zs::SmallString(vtag), imminent_collision_buffer = proxy({},imminent_collision_buffer), - exec_tag = exec_tag, + exec_tag = wrapv{}, eps = eps, restitution_rate = imminent_restitution_rate, impulse_count = proxy(impulse_count), @@ -1611,15 +1595,16 @@ void apply_impulse(Pol& pol, if(impulse.norm() < eps) return; - T beta = 0; + T cminv = 0; for(int i = 0;i != 4;++i) - beta += bary[i] * bary[i] * verts("minv",inds[i]); - impulse /= beta; + cminv += bary[i] * bary[i] * verts("minv",inds[i]); + // impulse /= beta; for(int i = 0;i != 4;++i) { + auto beta = verts("minv",inds[i]) * bary[i] / cminv; atomic_add(exec_tag,&impulse_count[inds[i]],1); for(int d = 0;d != 3;++d) - atomic_add(exec_tag,&impulse_buffer[inds[i]][d],impulse[d] * bary[i]); + atomic_add(exec_tag,&impulse_buffer[inds[i]][d],impulse[d] * beta); } }); @@ -1630,14 +1615,14 @@ void apply_impulse(Pol& pol, impulse_count = proxy(impulse_count), relaxation_rate = imminent_relaxation_rate, eps = eps, - exec_tag = exec_tag] ZS_LAMBDA(int vi) mutable { + exec_tag = wrapv{}] ZS_LAMBDA(int vi) mutable { if(impulse_buffer[vi].norm() < eps || impulse_count[vi] == 0) return; auto impulse = relaxation_rate * impulse_buffer[vi] / (T)impulse_count[vi]; - auto dp = impulse * verts("minv",vi); + // auto dp = impulse * verts("minv",vi); for(int i = 0;i != 3;++i) - atomic_add(exec_tag,&verts(vtag,i,vi),dp[i]); + atomic_add(exec_tag,&verts(vtag,i,vi),impulse[i]); }); } diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index 4e9c64094f..321212d206 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -1003,7 +1003,7 @@ struct DetangleCCDCollision : INode { using bv_t = typename ZenoLinearBvh::lbvh_t::Box; constexpr auto exec_tag = wrapv{}; constexpr auto eps = (T)1e-7; - constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; + // constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; auto zsparticles = get_input("zsparticles"); auto current_x_tag = get_input2("current_x_tag"); @@ -1020,7 +1020,8 @@ struct DetangleCCDCollision : INode { dtiles_t vtemp(verts.get_allocator(),{ {"x",3}, - {"v",3} + {"v",3}, + {"minv",1} },(size_t)verts.size()); auto nm_ccd_iters = get_input2("nm_ccd_iters"); @@ -1032,6 +1033,7 @@ struct DetangleCCDCollision : INode { pre_x_tag = zs::SmallString(pre_x_tag)] ZS_LAMBDA(int vi) mutable { vtemp.tuple(dim_c<3>,"x",vi) = verts.pack(dim_c<3>,pre_x_tag,vi); vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - verts.pack(dim_c<3>,pre_x_tag,vi); + vtemp("minv",vi) = verts("minv",vi); }); @@ -1044,14 +1046,26 @@ struct DetangleCCDCollision : INode { auto do_pt_detection = get_input2("do_pt_detection"); for(int iter = 0;iter != nm_ccd_iters;++iter) { - // std::cout << "do ccd iters : " << iter << std::endl; cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); - if(do_pt_detection) { - std::cout << "do_ccd_pt_detection" << std::endl; +#if 1 + auto do_bvh_refit = iter > 0; + COLLISION_UTILS::calc_continous_self_PT_collision_impulse(cudaPol, + verts, + vtemp,"x","v", + tris, + thickness, + triBvh, + do_bvh_refit, + impulse_buffer, + impulse_count); +#else + auto xnorm = TILEVEC_OPS::dot<3>(cudaPol,vtemp,"x","x"); + if(isnan(xnorm)) + printf("nan xnorm detected\n"); auto triBvs = retrieve_bounding_volumes(cudaPol,vtemp,tris,vtemp,zs::wrapv<3>{},1.f,thickness / (T)2,"x","v"); if(iter == 0) @@ -1089,22 +1103,38 @@ struct DetangleCCDCollision : INode { tp[i] = vtemp.pack(dim_c<3>,"x",tri[i]); tv[i] = vtemp.pack(dim_c<3>,"v",tri[i]); } - + for(int i = 0;i != 3;++i) + if(isnan(tp[i].norm())){ + printf("nan tp[%d] detected %d %d\n",ti,vi); + // return; + } + auto alpha = (T)1.0; pt_accd(p,tp[0],tp[1],tp[2],v,tv[0],tv[1],tv[2],(T)0.2,(T)0,alpha); + if(alpha > (T)1.0 || alpha < (T)0) return; + if(isnan(alpha)) { + printf("nan alpha %d %d\n",ti,vi); + // return; + } auto np = p + alpha * v; vec3 ntp[3] = {}; for(int i = 0;i != 3;++i) ntp[i] = tp[i] + alpha * tv[i]; - + vec3 bary_centric{}; LSL_GEO::pointBaryCentric(ntp[0],ntp[1],ntp[2],np,bary_centric); for(int i = 0;i != 3;++i) bary_centric[i] = bary_centric[i] < 0 ? 0 : bary_centric[i]; + + for(int i = 0;i != 3;++i) + if(isnan(bary_centric[i])) { + printf("nan bary_centric[%f %f %f] %d %d\n",bary_centric[0],bary_centric[1],bary_centric[2],ti,vi); + // return; + } auto barySum = bary_centric[0] + bary_centric[1] + bary_centric[2]; bary_centric = bary_centric / barySum; @@ -1117,6 +1147,10 @@ struct DetangleCCDCollision : INode { } auto collision_nrm = LSL_GEO::facet_normal(ntp[0],ntp[1],ntp[2]); + if(isnan(collision_nrm.norm())) { + printf("nan pt normal[%d %d]\n",ti,vi); + // return; + } auto relative_velocity = v - vt_project; if(collision_nrm.dot(relative_velocity) > 0) collision_nrm *= -1; @@ -1128,6 +1162,10 @@ struct DetangleCCDCollision : INode { cm += bary_centric[i] * bary_centric[i] * verts("minv",tri[i]); if(cm < eps) return; + if(isnan(cm)) { + printf("nan cm detected[%d %d]\n",ti,vi); + // return; + } cm = (T)1/cm; auto delta_rv = collision_nrm * relative_normal_velocity * (1 - alpha) * cm; @@ -1145,7 +1183,7 @@ struct DetangleCCDCollision : INode { }; bvh.iter_neighbors(bv,resolve_ccd_colliding_pairs); }); - +#endif } if(do_ee_detection) { @@ -1279,8 +1317,8 @@ struct DetangleCCDCollision : INode { if(impulse_buffer[vi].norm() < eps) return; auto impulse = relaxation_rate * impulse_buffer[vi] / impulse_count[vi]; - auto dv = impulse * verts("minv",vi); - vtemp.tuple(dim_c<3>,"v",vi) = vtemp.pack(dim_c<3>,"v",vi) + dv; + // auto dv = impulse + vtemp.tuple(dim_c<3>,"v",vi) = vtemp.pack(dim_c<3>,"v",vi) + impulse; // for(int i = 0;i != 3;++i) // atomic_add(exec_tag,&vtemp("v",i,vi),dv[i]); }); @@ -1316,6 +1354,134 @@ ZENDEFNODE(DetangleCCDCollision, {{{"zsparticles"}, {"PBD"}}); +struct VisualizeContinousCollision : INode { + using T = float; + using vec3 = zs::vec; + using dtiles_t = zs::TileVector; + + virtual void apply() override { + using namespace zs; + using lbvh_t = typename ZenoLinearBvh::lbvh_t; + constexpr auto cuda_space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + + constexpr auto omp_space = execspace_e::openmp; + auto ompPol = omp_exec(); + + auto zsparticles = get_input("zsparticles"); + auto current_x_tag = get_input2("current_x_tag"); + auto pre_x_tag = get_input2("previous_x_tag"); + + auto thickness = get_input2("thickness"); + + auto& verts = zsparticles->getParticles(); + const auto& tris = zsparticles->getQuadraturePoints(); + const auto &edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + + dtiles_t vtemp(verts.get_allocator(),{ + {"x",3}, + {"v",3}, + {"minv",1} + },(size_t)verts.size()); + + lbvh_t triBvh{},eBvh{}; + // auto nm_ccd_iters = get_input2("nm_ccd_iters"); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + current_x_tag = zs::SmallString(current_x_tag), + pre_x_tag = zs::SmallString(pre_x_tag)] ZS_LAMBDA(int vi) mutable { + vtemp.tuple(dim_c<3>,"x",vi) = verts.pack(dim_c<3>,pre_x_tag,vi); + vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - verts.pack(dim_c<3>,pre_x_tag,vi); + vtemp("minv",vi) = verts("minv",vi); + }); + + zs::Vector impulse_buffer{verts.get_allocator(),verts.size()}; + zs::Vector impulse_count{verts.get_allocator(),verts.size()}; + + cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); + cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + + COLLISION_UTILS::calc_continous_self_PT_collision_impulse(cudaPol, + verts, + vtemp,"x","v", + tris, + thickness, + triBvh, + false, + impulse_buffer, + impulse_count); + + cudaPol(zs::range(impulse_buffer.size()),[ + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count)] ZS_LAMBDA(int vi) mutable { + if(impulse_count[vi] > 0) { + impulse_buffer[vi] = impulse_buffer[vi] / (T)impulse_count[vi]; + // printf("impulse[%d] : %f %f %f\n",vi + // ,(float)impulse_buffer[vi][0] + // ,(float)impulse_buffer[vi][1] + // ,(float)impulse_buffer[vi][2]); + } + }); + + dtiles_t ccd_PT_collision_vis(verts.get_allocator(),{ + {"impulse",3}, + {"p0",3}, + {"p1",3}, + {"pp",3} + },verts.size()); + + cudaPol(zs::range(verts.size()),[ + ccd_PT_collision_vis = proxy({},ccd_PT_collision_vis), + impulse_buffer = proxy(impulse_buffer), + vtemp = proxy({},vtemp)] ZS_LAMBDA(int vi) mutable { + ccd_PT_collision_vis.tuple(dim_c<3>,"impulse",vi) = impulse_buffer[vi]; + ccd_PT_collision_vis.tuple(dim_c<3>,"p0",vi) = vtemp.pack(dim_c<3>,"x",vi); + ccd_PT_collision_vis.tuple(dim_c<3>,"p1",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); + ccd_PT_collision_vis.tuple(dim_c<3>,"pp",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi) + impulse_buffer[vi]; + }); + + ccd_PT_collision_vis = ccd_PT_collision_vis.clone({zs::memsrc_e::host}); + auto prim_PT = std::make_shared(); + auto& vis_verts_PT = prim_PT->verts; + auto& vis_lines_PT = prim_PT->lines; + vis_verts_PT.resize(verts.size() * 2); + vis_lines_PT.resize(verts.size()); + + auto scale = get_input2("scale"); + + ompPol(zs::range(verts.size()),[ + &vis_verts_PT,&vis_lines_PT, + scale = scale, + ccd_PT_collision_vis = proxy({},ccd_PT_collision_vis)] (int vi) mutable { + auto p = ccd_PT_collision_vis.pack(dim_c<3>,"p0",vi); + auto impulse = ccd_PT_collision_vis.pack(dim_c<3>,"impulse",vi); + auto pc = p + impulse * scale; + vis_verts_PT[vi * 2 + 0] = p.to_array(); + vis_verts_PT[vi * 2 + 1] = pc.to_array(); + vis_lines_PT[vi] = zeno::vec2i{vi * 2 + 0,vi * 2 + 1}; + }); + + set_output("impulse_PT",std::move(prim_PT)); + } +}; + +ZENDEFNODE(VisualizeContinousCollision, {{{"zsparticles"}, + {"string","current_x_tag","x"}, + {"string","previous_x_tag","px"}, + // {"string","pscaleTag","pscale"}, + // {"float","repeling_strength","1.0"}, + {"float","thickness","0.01"}, + // {"int","nm_imminent_iters","1"}, + // {"float","imm_restitution","0.1"}, + {"float","scale","0.25"}, + // {"bool","add_repulsion_force","0"}, + }, + {{"impulse_PT"}}, + {}, + {"PBD"}}); + struct SDFColliderProject : INode { using T = float; using vec3 = zs::vec; From 09c4d4e891ce842ae3c9f6ff11e6706ecd82db58 Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Thu, 14 Sep 2023 22:58:17 +0800 Subject: [PATCH 18/21] robust nm of collision iters --- .../collision_energy/evaluate_collision.hpp | 266 +++++----- projects/CuLagrange/pbd/Pipeline.cu | 485 +++++++----------- 2 files changed, 306 insertions(+), 445 deletions(-) diff --git a/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp b/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp index 7ead798641..64c01a3d01 100644 --- a/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp +++ b/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp @@ -1265,7 +1265,7 @@ void calc_imminent_self_EE_collision_impulse(Pol& pol, template; using vec4i = zs::vec; constexpr auto eps = (T)1e-6; - // constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; - // zs::bht csPT{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; - // csPT.reset(pol,true); - - // auto triCCDBvh = bvh_t{}; auto bvs = retrieve_bounding_volumes(pol,verts,tris,verts,wrapv<3>{},(T)1.0,(T)0,xtag,vtag); if(refit_bvh) triCCDBvh.refit(pol,bvs); @@ -1310,7 +1305,7 @@ void calc_continous_self_PT_collision_impulse(Pol& pol, verts = proxy({},verts), tris = proxy({},tris), // csPT = proxy(csPT), - thickness = thickness, + // thickness = thickness, impulse_buffer = proxy(impulse_buffer), impulse_count = proxy(impulse_count), eps = eps, @@ -1406,158 +1401,143 @@ void calc_continous_self_PT_collision_impulse(Pol& pol, }); } -// template -// void calc_continous_self_EE_collision_impulse(Pol& pol, -// const PosTileVec& verts,const zs::SmallString& xtag,const zs::SmallString& vtag, -// const EdgeTileVec& edges, -// const REAL& thickness, -// ImpulseBuffer& impulse_buffer, -// ImpulseCountBuffer& impulse_count) { -// using namespace zs; -// constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; -// constexpr auto exec_tag = wrapv{}; -// using vec2 = zs::vec; -// using vec3 = zs::vec; -// using vec4 = zs::vec; -// using vec2i = zs::vec; -// using vec4i = zs::vec; -// constexpr auto eps = (T)1e-6; -// constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; -// zs::bht csEE{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; -// csEE.reset(pol,true); - -// auto edgeCCDBvh = bvh_t{}; -// auto bvs = retrieve_bounding_volumes(pol,verts,edges,verts,wrapv<2>{},(T)1.0,thickness/(T)2,xtag,vtag); -// edgeCCDBvh.build(pol,bvs); - -// pol(zs::range(edges.size()),[ -// xtag = xtag, -// vtag = vtag, -// verts = proxy({},verts), -// edges = proxy({},edges), -// thickness = thickness, -// eps = eps, -// edgeBvs = proxy(edgeBvs), -// edgeBvh = proxy(edgeBvh)] ZS_LAMBDA(int ei) mutable { -// auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); -// vec3 pas[2] = {}; -// vec3 vas[2] = {}; -// for(int i = 0;i != 2;++i) { -// pas[i] = verts.pack(dim_c<3>,xtag,ea[i]); -// vas[i] = verts.pack(dim_c<3>,vtag,ea[i]); -// } - -// auto bv = edgeBvs[ei]; - -// auto do_close_proximity_detection = [&](int nei) mutable { -// if(ei >= nei) -// return; -// auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); -// for(int i = 0;i != 2;++i){ -// if(eb[i] == ea[0] || eb[i] == ea[1]) -// return; -// } - -// bool has_dynamic_points = false; -// for(int i = 0;i != 2;++i) { -// if(verts("minv",ea[i]) > eps) -// has_dynamic_points = true; -// if(verts("minv",eb[i]) > eps) -// has_dynamic_points = true; -// } -// if(!has_dynamic_points) -// return; - -// vec3 pbs[2] = {}; -// vec3 vbs[2] = {}; -// for(int i = 0;i != 2;++i) { -// pbs[i] = verts.pack(dim_c<3>,xtag,eb[i]); -// vbs[i] = verts.pack(dim_c<3>,vtag,eb[i]); -// } - -// auto alpha = (T)1.0; -// ee_accd(pas[0],pas[1],pbs[0],pbs[1],vas[0],vas[1],vbs[0],vbs[1],(T)0.2,thickness,alpha); +template +void calc_continous_self_EE_collision_impulse(Pol& pol, + const InverseMassTileVec& invMass, + const PosTileVec& verts,const zs::SmallString& xtag,const zs::SmallString& vtag, + const EdgeTileVec& edges, + EdgeBvh& edgeCCDBvh, + bool refit_bvh, + ImpulseBuffer& impulse_buffer, + ImpulseCountBuffer& impulse_count) { + using namespace zs; + constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; + // constexpr auto exec_tag = wrapv{}; + using vec2 = zs::vec; + using vec3 = zs::vec; + using vec4 = zs::vec; + using vec2i = zs::vec; + using vec4i = zs::vec; + constexpr auto eps = (T)1e-6; -// vec4i inds{ea[0],ea[1],eb[0],eb[1]}; -// vec3 ps[4] = {}; -// for(int i = 0;i != 4;++i) -// ps[i] = verts.pack(dim_c<3>,xtag,inds[i]) + verts.pack(dim_c<3>,vtag,inds[i]); - -// vec3 int_a{},int_b{}; -// COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[0],ps[1],int_a,int_b); + // auto edgeCCDBvh = bvh_t{}; + auto edgeBvs = retrieve_bounding_volumes(pol,verts,edges,verts,wrapv<2>{},(T)1.0,(T)0,xtag,vtag); + if(refit_bvh) + edgeCCDBvh.refit(pol,edgeBvs); + else + edgeCCDBvh.build(pol,edgeBvs); -// auto la = (ps[0] - ps[1]).norm(); + pol(zs::range(edges.size()),[ + xtag = xtag, + vtag = vtag, + verts = proxy({},verts), + edges = proxy({},edges), + invMass = proxy({},invMass), + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count), + // thickness = thickness, + eps = eps, + exec_tag = wrapv{}, + edgeBvs = proxy(edgeBvs), + bvh = proxy(edgeCCDBvh)] ZS_LAMBDA(int ei) mutable { + auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); + vec3 ps[4] = {}; + ps[0] = verts.pack(dim_c<3>,xtag,ea[0]); + ps[1] = verts.pack(dim_c<3>,xtag,ea[1]); + vec3 vs[4] = {}; + vs[0] = verts.pack(dim_c<3>,vtag,ea[0]); + vs[1] = verts.pack(dim_c<3>,vtag,ea[1]); -// auto ra = (ps[0] - int_a).norm() / (ps[0] - ps[1]).norm();; -// auto rb = (ps[2] - int_b).norm() / (ps[2] - ps[3]).norm();; + vec4i inds{ea[0],ea[1],-1,-1}; -// if(ra < 10 * eps || ra > 1 - 10 * eps) -// return; -// if(rb < 10 * eps || rb > 1 - 10 * eps) -// return; + auto bv = edgeBvs[ei]; -// csEE.insert(vec2i{ei,nei}); -// }; -// edgeBvh.iter_neighbors(bv,do_close_proximity_detection); -// }); + auto do_close_proximity_detection = [&](int nei) mutable { + if(ei >= nei) + return; + auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); + for(int i = 0;i != 2;++i){ + if(eb[i] == ea[0] || eb[i] == ea[1]) + return; + } -// printf("nm_EE_collision_pairs : %d %d\n",(int)csEE.size(),(int)edges.size()); + bool has_dynamic_points = false; + for(int i = 0;i != 2;++i) { + if(verts("minv",ea[i]) > eps) + has_dynamic_points = true; + if(verts("minv",eb[i]) > eps) + has_dynamic_points = true; + } + if(!has_dynamic_points) + return; -// imminent_collision_buffer.resize(csEE.size() + buffer_offset); -// pol(zip(zs::range(csEE.size()),csEE._activeKeys),[ -// verts = proxy({},verts), -// edges = proxy({},edges), -// buffer_offset = buffer_offset, -// xtag = xtag, -// vtag = vtag, -// imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(auto ci,const auto& pair) mutable { -// auto ei = pair[0]; -// auto nei = pair[1]; + for(int i = 0;i != 2;++i) { + ps[i + 2] = verts.pack(dim_c<3>,xtag,eb[i]); + vs[i + 2] = verts.pack(dim_c<3>,vtag,eb[i]); + inds[i + 2] = eb[i]; + } -// auto ea = edges.pack(dim_c<2>,"inds",ei,int_c); -// auto eb = edges.pack(dim_c<2>,"inds",nei,int_c); -// vec4i inds{ea[0],ea[1],eb[0],eb[1]}; + auto alpha = (T)1.0; + if(!ee_accd(ps[0],ps[1],ps[2],ps[3],vs[0],vs[1],vs[2],vs[3],(T)0.2,(T)0.0,alpha)) + return; -// vec3 ps[4] = {}; -// vec3 vs[4] = {}; -// for(int i = 0;i != 4;++i) { -// ps[i] = verts.pack(dim_c<3>,xtag,inds[i]); -// vs[i] = verts.pack(dim_c<3>,vtag,inds[i]); -// } + vec3 nps[4] = {}; + for(int i = 0;i != 4;++i) + nps[i] = ps[i] + alpha * vs[i]; + vec3 int_a{},int_b{}; + COLLISION_UTILS::IntersectLineSegments(nps[0],nps[1],nps[2],nps[3],int_a,int_b); -// vec3 int_a{},int_b{}; -// COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[2],ps[3],int_a,int_b); + auto ra = (nps[0] - int_a).norm() / (nps[0] - nps[1]).norm();; + auto rb = (nps[2] - int_b).norm() / (nps[2] - nps[3]).norm();; -// auto la = (ps[0] - ps[1]).norm(); -// auto lb = (ps[2] - ps[3]).norm(); + // if(ra < 10 * eps || ra > 1 - 10 * eps) + // return; + // if(rb < 10 * eps || rb > 1 - 10 * eps) + // return; -// auto ra = (ps[0] - int_a).norm() / la; -// auto rb = (ps[2] - int_b).norm() / lb; + vec4 bary{ra - 1,-ra,1 - rb,rb}; + auto pr = vec3::zeros(); + auto vr = vec3::zeros(); + for(int i = 0;i != 4;++i) { + pr += bary[i] * nps[i]; + vr += bary[i] * vs[i]; + } -// vec4 bary{ra - 1,-ra,1 - rb,rb}; + vec3 collision_nrm = {}; + if(pr.norm() < 100 * eps) + collision_nrm = (nps[0] - nps[1]).cross(nps[2] - nps[3]).normalized(); + else + collision_nrm = pr.normalized(); + if(collision_nrm.dot(vr) > 0) + collision_nrm = (T)-1 * collision_nrm; -// auto pr = vec3::zeros(); -// auto vr = vec3::zeros(); -// for(int i = 0;i != 4;++i) { -// pr += bary[i] * ps[i]; -// vr += bary[i] * vs[i]; -// } + auto vr_nrm = collision_nrm.dot(vr); -// auto collision_nrm = pr.normalized(); -// auto relative_normal_velocity = vr.dot(collision_nrm); + auto cm = (T).0; + for(int i = 0;i != 4;++i) + cm += bary[i] * bary[i] * invMass("minv",inds[i]); + if(cm < eps) + return; -// imminent_collision_buffer.tuple(dim_c<4>,"bary",ci + buffer_offset) = bary; -// imminent_collision_buffer.tuple(dim_c<4>,"inds",ci + buffer_offset) = inds.reinterpret_bits(float_c); -// auto I = relative_normal_velocity < 0 ? relative_normal_velocity : (T)0; -// imminent_collision_buffer.tuple(dim_c<3>,"impulse",ci + buffer_offset) = -collision_nrm * I; -// }); -// } + auto impulse = -collision_nrm * vr_nrm * ((T)1 - alpha); + for(int i = 0;i != 4;++i) { + auto beta = (bary[i] * invMass("minv",inds[i])) / cm; + atomic_add(exec_tag,&impulse_count[inds[i]],1); + for(int d = 0;d != 3;++d) + atomic_add(exec_tag,&impulse_buffer[inds[i]][d],impulse[d] * beta); + } + }; + bvh.iter_neighbors(bv,do_close_proximity_detection); + }); +} template{}; + // constexpr auto exec_tag = wrapv{}; constexpr auto eps = (T)1e-7; auto zsparticles = get_input("zsparticles"); @@ -588,6 +588,11 @@ struct DetangleImminentCollision : INode { const auto& halfedges = (*zsparticles)[ZenoParticles::s_surfHalfEdgeTag]; const auto &edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + if(!verts.hasProperty("imminent_fail")) + verts.append_channels(cudaPol,{{"imminent_fail",1}}); + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts)] ZS_LAMBDA(int vi) mutable {verts("imminent_fail",vi) = (T)0;}); + dtiles_t vtemp{verts.get_allocator(),{ {"x",3}, {"v",3}, @@ -619,7 +624,14 @@ struct DetangleImminentCollision : INode { auto do_pt_detection = get_input2("use_PT"); auto do_ee_detection = get_input2("use_EE"); + zs::Vector nm_imminent_collision{verts.get_allocator(),(size_t)1}; + + std::cout << "do imminent detangle" << std::endl; for(int it = 0;it != nm_iters;++it) { + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts)] ZS_LAMBDA(int vi) mutable {verts("imminent_fail",vi) = (T)0;}); + // we use collision cell as the collision volume, PT collision is enough prevent penertation? if(do_pt_detection) { COLLISION_UTILS::calc_imminent_self_PT_collision_impulse(cudaPol, @@ -651,74 +663,90 @@ struct DetangleImminentCollision : INode { imminent_restitution_rate, imminent_relaxation_rate, imminent_collision_buffer); - } + - auto add_repulsion_force = get_input2("add_repulsion_force"); - // add an impulse to repel the pair further - if(add_repulsion_force) { - std::cout << "add imminent replering force" << std::endl; - auto max_repel_distance = get_input2("max_repel_distance"); - - cudaPol(zs::range(imminent_collision_buffer.size()),[ - imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(int ci) mutable { - imminent_collision_buffer.tuple(dim_c<3>,"impulse",ci) = vec3::zeros(); - }); + auto add_repulsion_force = get_input2("add_repulsion_force"); + // add an impulse to repel the pair further - cudaPol(zs::range(imminent_collision_buffer.size()),[ - vtemp = proxy({},vtemp), - eps = eps, - exec_tag = exec_tag, - k = repel_strength, - max_repel_distance = max_repel_distance, - thickness = imminent_collision_thickness, - imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(auto id) mutable { - auto inds = imminent_collision_buffer.pack(dim_c<4>,"inds",id,int_c); - auto bary = imminent_collision_buffer.pack(dim_c<4>,"bary",id); - - vec3 ps[4] = {}; - vec3 vs[4] = {}; - auto vr = vec3::zeros(); - auto pr = vec3::zeros(); - for(int i = 0;i != 4;++i) { - ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); - vs[i] = vtemp.pack(dim_c<3>,"v",inds[i]); - pr += bary[i] * ps[i]; - vr += bary[i] * vs[i]; - } + nm_imminent_collision.setVal(0); + // if(add_repulsion_force) { + // std::cout << "add imminent replering force" << std::endl; + auto max_repel_distance = get_input2("max_repel_distance"); - auto dist = pr.norm(); - vec3 collision_normal = imminent_collision_buffer.pack(dim_c<3>,"collision_normal",id); + cudaPol(zs::range(imminent_collision_buffer.size()),[ + imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(int ci) mutable { + imminent_collision_buffer.tuple(dim_c<3>,"impulse",ci) = vec3::zeros(); + }); - if(dist > thickness) - return; + cudaPol(zs::range(imminent_collision_buffer.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + eps = eps, + exec_tag = wrapv{}, + k = repel_strength, + max_repel_distance = max_repel_distance, + thickness = imminent_collision_thickness, + nm_imminent_collision = proxy(nm_imminent_collision), + imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(auto id) mutable { + auto inds = imminent_collision_buffer.pack(dim_c<4>,"inds",id,int_c); + auto bary = imminent_collision_buffer.pack(dim_c<4>,"bary",id); + + vec3 ps[4] = {}; + vec3 vs[4] = {}; + auto vr = vec3::zeros(); + auto pr = vec3::zeros(); + for(int i = 0;i != 4;++i) { + ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); + vs[i] = vtemp.pack(dim_c<3>,"v",inds[i]); + pr += bary[i] * ps[i]; + vr += bary[i] * vs[i]; + } - auto d = thickness - dist; - auto vn = vr.dot(collision_normal); - if(vn > (T)max_repel_distance * d) { - // if with current velocity, the collided particles can be repeled by more than 1% of collision depth, no extra repulsion is needed - return; - } else { - // make sure the collided particles is seperated by 1% of collision depth - // assume the particles has the same velocity - auto I = k * d; - auto I_max = (max_repel_distance * d - vn); - I = I_max < I ? I_max : I; - auto impulse = (T)I * collision_normal; - - imminent_collision_buffer.tuple(dim_c<3>,"impulse",id) = impulse; - } - }); + auto dist = pr.norm(); + vec3 collision_normal = imminent_collision_buffer.pack(dim_c<3>,"collision_normal",id); - // auto impulse_norm = TILEVEC_OPS::dot<3>(cudaPol,imminent_collision_buffer,"impulse","impulse"); - // std::cout << "REPEL_impulse_norm : " << impulse_norm << std::endl; + if(dist > thickness) + return; - COLLISION_UTILS::apply_impulse(cudaPol, - vtemp,"v", - imminent_restitution_rate, - imminent_relaxation_rate, - imminent_collision_buffer); + auto d = thickness - dist; + auto vn = vr.dot(collision_normal); + if(vn < -thickness * 0.05) { + atomic_add(exec_tag,&nm_imminent_collision[0],1); + for(int i = 0;i != 4;++i) + verts("imminent_fail",inds[i]) = (T)1.0; + } + if(vn > (T)max_repel_distance * d || d < 0) { + // if with current velocity, the collided particles can be repeled by more than 1% of collision depth, no extra repulsion is needed + return; + } else { + // make sure the collided particles is seperated by 1% of collision depth + // assume the particles has the same velocity + auto I = k * d; + auto I_max = (max_repel_distance * d - vn); + I = I_max < I ? I_max : I; + auto impulse = (T)I * collision_normal; + + imminent_collision_buffer.tuple(dim_c<3>,"impulse",id) = impulse; + } + }); + std::cout << "nm_imminent_collision : " << nm_imminent_collision.getVal(0) << std::endl; + if(nm_imminent_collision.getVal(0) == 0) + break; + + + // auto impulse_norm = TILEVEC_OPS::dot<3>(cudaPol,imminent_collision_buffer,"impulse","impulse"); + // std::cout << "REPEL_impulse_norm : " << impulse_norm << std::endl; + + COLLISION_UTILS::apply_impulse(cudaPol, + vtemp,"v", + imminent_restitution_rate, + imminent_relaxation_rate, + imminent_collision_buffer); + // } } + std::cout << "finish imminent collision" << std::endl; + cudaPol(zs::range(verts.size()),[ verts = proxy({},verts), vtemp = proxy({},vtemp), @@ -726,7 +754,7 @@ struct DetangleImminentCollision : INode { verts.tuple(dim_c<3>,current_x_tag,vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); }); - std::cout << "finish apply imminent impulse" << std::endl; + // std::cout << "finish apply imminent impulse" << std::endl; set_output("zsparticles",zsparticles); } @@ -1045,285 +1073,76 @@ struct DetangleCCDCollision : INode { auto do_ee_detection = get_input2("do_ee_detection"); auto do_pt_detection = get_input2("do_pt_detection"); + zs::Vector nm_ccd_collision{verts.get_allocator(),1}; + + std::cout << "resolve continous collision " << std::endl; + for(int iter = 0;iter != nm_ccd_iters;++iter) { cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + nm_ccd_collision.setVal(0); + if(do_pt_detection) { -#if 1 auto do_bvh_refit = iter > 0; COLLISION_UTILS::calc_continous_self_PT_collision_impulse(cudaPol, verts, vtemp,"x","v", tris, - thickness, + // thickness, triBvh, do_bvh_refit, impulse_buffer, impulse_count); -#else - auto xnorm = TILEVEC_OPS::dot<3>(cudaPol,vtemp,"x","x"); - if(isnan(xnorm)) - printf("nan xnorm detected\n"); - - auto triBvs = retrieve_bounding_volumes(cudaPol,vtemp,tris,vtemp,zs::wrapv<3>{},1.f,thickness / (T)2,"x","v"); - if(iter == 0) - triBvh.build(cudaPol,triBvs); - else - triBvh.refit(cudaPol,triBvs); - - cudaPol(zs::range(vtemp.size()),[ - impulse_buffer = proxy(impulse_buffer), - impulse_count = proxy(impulse_count), - verts = proxy({},verts), - vtemp = proxy({},vtemp), - tris = proxy({},tris), - exec_tag = exec_tag, - thickness = thickness, - restitution_rate = restitution_rate, - bvh = proxy(triBvh)] ZS_LAMBDA(int vi) mutable { - auto p = vtemp.pack(dim_c<3>,"x",vi); - auto v = vtemp.pack(dim_c<3>,"v",vi); - // auto np = p + v; - - bv_t bv{p,p + v}; - bv._min -= thickness / (T)2.0; - bv._max += thickness / (T)2.0; - - auto resolve_ccd_colliding_pairs = [&] (int ti) mutable { - auto tri = tris.pack(dim_c<3>,"inds",ti,int_c); - for(int i = 0;i != 3;++i) - if(tri[i] == vi) - return; - - vec3 tp[3] = {}; - vec3 tv[3] = {}; - for(int i = 0;i != 3;++i) { - tp[i] = vtemp.pack(dim_c<3>,"x",tri[i]); - tv[i] = vtemp.pack(dim_c<3>,"v",tri[i]); - } - for(int i = 0;i != 3;++i) - if(isnan(tp[i].norm())){ - printf("nan tp[%d] detected %d %d\n",ti,vi); - // return; - } - - auto alpha = (T)1.0; - pt_accd(p,tp[0],tp[1],tp[2],v,tv[0],tv[1],tv[2],(T)0.2,(T)0,alpha); - - - if(alpha > (T)1.0 || alpha < (T)0) - return; - if(isnan(alpha)) { - printf("nan alpha %d %d\n",ti,vi); - // return; - } - - auto np = p + alpha * v; - vec3 ntp[3] = {}; - for(int i = 0;i != 3;++i) - ntp[i] = tp[i] + alpha * tv[i]; - - vec3 bary_centric{}; - LSL_GEO::pointBaryCentric(ntp[0],ntp[1],ntp[2],np,bary_centric); - for(int i = 0;i != 3;++i) - bary_centric[i] = bary_centric[i] < 0 ? 0 : bary_centric[i]; - - for(int i = 0;i != 3;++i) - if(isnan(bary_centric[i])) { - printf("nan bary_centric[%f %f %f] %d %d\n",bary_centric[0],bary_centric[1],bary_centric[2],ti,vi); - // return; - } - auto barySum = bary_centric[0] + bary_centric[1] + bary_centric[2]; - bary_centric = bary_centric / barySum; - - - auto pt_project = vec3::zeros(); - auto vt_project = vec3::zeros(); - for(int i = 0;i != 3;++i) { - pt_project += ntp[i] * bary_centric[i]; - vt_project += tv[i] * bary_centric[i]; - } - - auto collision_nrm = LSL_GEO::facet_normal(ntp[0],ntp[1],ntp[2]); - if(isnan(collision_nrm.norm())) { - printf("nan pt normal[%d %d]\n",ti,vi); - // return; - } - auto relative_velocity = v - vt_project; - if(collision_nrm.dot(relative_velocity) > 0) - collision_nrm *= -1; - - auto relative_normal_velocity = collision_nrm.dot(relative_velocity); - - auto cm = verts("minv",vi); - for(int i = 0;i != 3;++i) - cm += bary_centric[i] * bary_centric[i] * verts("minv",tri[i]); - if(cm < eps) - return; - if(isnan(cm)) { - printf("nan cm detected[%d %d]\n",ti,vi); - // return; - } - cm = (T)1/cm; - - auto delta_rv = collision_nrm * relative_normal_velocity * (1 - alpha) * cm; - - for(int i = 0;i != 3;++i) { - auto interpI = delta_rv * bary_centric[i]; - atomic_add(exec_tag,&impulse_count[tri[i]],1); - for(int d = 0;d != 3;++d) - atomic_add(exec_tag,&impulse_buffer[tri[i]][d],interpI[d]); - } - auto interpI = -delta_rv; - atomic_add(exec_tag,&impulse_count[vi],1); - for(int d = 0;d != 3;++d) - atomic_add(exec_tag,&impulse_buffer[vi][d],interpI[d]); - }; - bvh.iter_neighbors(bv,resolve_ccd_colliding_pairs); - }); -#endif } if(do_ee_detection) { - - std::cout << "do_ccd_ee_detection" << std::endl; - - auto edgeBvs = retrieve_bounding_volumes(cudaPol,vtemp,edges,vtemp,zs::wrapv<2>{},1.f,thickness / (T)2,"x","v"); - if(iter == 0) - eBvh.build(cudaPol,edgeBvs); - else - eBvh.refit(cudaPol,edgeBvs); - - cudaPol(zs::range(edges.size()),[ - impulse_buffer = proxy(impulse_buffer), - impulse_count = proxy(impulse_count), - verts = proxy({},verts), - vtemp = proxy({},vtemp), - edges = proxy({},edges), - exec_tag = exec_tag, - eps = eps, - thickness = thickness, - // restitution_rate = restitution_rate, - bvs = proxy(edgeBvs), - bvh = proxy(eBvh)] ZS_LAMBDA(int ei) mutable { - auto bv = bvs[ei]; - auto edge = edges.pack(dim_c<2>,"inds",ei,int_c); - vec3 pas[2] = {}; - vec3 vas[2] = {}; - for(int i = 0;i != 2;++i) { - pas[i] = vtemp.pack(dim_c<3>,"x",edge[i]); - vas[i] = vtemp.pack(dim_c<3>,"v",edge[i]); - } - - auto resolve_ccd_collilsion_pairs = [&] (int nei) mutable { - if(nei == ei) - return; - auto nedge = edges.pack(dim_c<2>,"inds",nei,int_c); - for(int i = 0;i != 2;++i) - if(edge[i] == nedge[0] || edge[i] == nedge[1]) - return; - bool has_dynamic_vert = false; - for(int i = 0;i != 2;++i) - if(verts("minv",edge[i]) > eps || verts("minv",nedge[i]) > eps) - has_dynamic_vert = true; - if(!has_dynamic_vert) - return; - - - vec3 pbs[2] = {}; - vec3 vbs[2] = {}; - for(int i = 0;i != 2;++i) { - pbs[i] = vtemp.pack(dim_c<3>,"x",nedge[i]); - vbs[i] = vtemp.pack(dim_c<3>,"v",nedge[i]); - } - - auto alpha = (T)1.0; - ee_accd(pas[0],pas[1],pbs[0],pbs[1],vas[0],vas[1],vbs[0],vbs[1],(T)0.2,(T)0,alpha); - - if(alpha > (T)1.0 || alpha < (T)0) - return; - - vec3 npas[2] = {}; - vec3 npbs[2] = {}; - for(int i = 0;i != 2;++i) { - npas[i] = pas[i] + alpha * vas[i]; - npbs[i] = pbs[i] + alpha * vbs[i]; - } - - vec3 int_a{},int_b{}; - COLLISION_UTILS::IntersectLineSegments(npas[0],npas[1],npbs[0],npbs[1],int_a,int_b); - auto lb = (npbs[0] - npbs[1]).norm(); - auto la = (npas[0] - npas[1]).norm(); - - if(la < eps * 10 || lb < eps * 10) - return; - - auto ra = (npas[0] - int_a).norm() / la; - auto rb = (npbs[0] - int_b).norm() / lb; - - vec4 bary{ra - 1,-ra,1 - rb,rb}; - // auto pr = vec3::zeros(); - auto vr = vec3::zeros(); - auto pr = vec3::zeros(); - for(int i = 0;i != 2;++i) { - // pr += bary[i] * ps[i]; - vr += bary[i] * vas[i]; - vr += bary[i + 2] * vbs[i]; - pr += bary[i] * pas[i]; - pr += bary[i + 2] * pbs[i]; - } - - auto collision_nrm = (npbs[0] - npbs[1]).cross(npas[0] - npas[1]).normalized(); - if(collision_nrm.norm() < 100 * eps) - return; - auto align = collision_nrm.dot(pr); - if(align < 0) - collision_nrm *= -1; - auto relative_normal_velocity = vr.dot(collision_nrm); - - auto impulse = -relative_normal_velocity * collision_nrm * ((T)1 - alpha); - vec4i inds{edge[0],edge[1],nedge[0],nedge[1]}; - - T beta = 0; - for(int i = 0;i != 4;++i) - beta += bary[i] * bary[i] * verts("minv",inds[i]); - impulse /= beta; - for(int i = 0;i != 4;++i) { - atomic_add(exec_tag,&impulse_count[inds[i]],1); - for(int d = 0;d != 3;++d) - atomic_add(exec_tag,&impulse_buffer[inds[i]][d],impulse[d] * bary[i]); - } - }; - - bvh.iter_neighbors(bv,resolve_ccd_collilsion_pairs); - }); - - + auto do_bvh_refit = iter > 0; + COLLISION_UTILS::calc_continous_self_EE_collision_impulse(cudaPol, + verts, + vtemp,"x","v", + edges, + eBvh, + do_bvh_refit, + impulse_buffer, + impulse_count); } - std::cout << "apply impulse" << std::endl; + // std::cout << "apply impulse" << std::endl; cudaPol(zs::range(verts.size()),[ verts = proxy({},verts), vtemp = proxy({},vtemp), impulse_buffer = proxy(impulse_buffer), impulse_count = proxy(impulse_count), relaxation_rate = relaxation_rate, + nm_ccd_collision = proxy(nm_ccd_collision), eps = eps, + thickness = thickness, exec_tag = exec_tag] ZS_LAMBDA(int vi) mutable { if(impulse_count[vi] == 0) return; if(impulse_buffer[vi].norm() < eps) return; + auto impulse = relaxation_rate * impulse_buffer[vi] / impulse_count[vi]; + if(impulse.norm() > thickness * 0.01) + atomic_add(exec_tag,&nm_ccd_collision[0],1); + // auto dv = impulse vtemp.tuple(dim_c<3>,"v",vi) = vtemp.pack(dim_c<3>,"v",vi) + impulse; // for(int i = 0;i != 3;++i) // atomic_add(exec_tag,&vtemp("v",i,vi),dv[i]); }); + + std::cout << "nm_ccd_collision : " << nm_ccd_collision.getVal() << std::endl; + if(nm_ccd_collision.getVal() == 0) + break; } + + std::cout << "finish solving continous collision " << std::endl; + cudaPol(zs::range(verts.size()),[ vtemp = proxy({},vtemp), verts = proxy({},verts), @@ -1407,7 +1226,7 @@ struct VisualizeContinousCollision : INode { verts, vtemp,"x","v", tris, - thickness, + // thickness, triBvh, false, impulse_buffer, @@ -1464,6 +1283,68 @@ struct VisualizeContinousCollision : INode { }); set_output("impulse_PT",std::move(prim_PT)); + + cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); + cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + + COLLISION_UTILS::calc_continous_self_EE_collision_impulse(cudaPol, + verts, + vtemp,"x","v", + edges, + eBvh, + false, + impulse_buffer, + impulse_count); + + cudaPol(zs::range(impulse_buffer.size()),[ + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count)] ZS_LAMBDA(int vi) mutable { + if(impulse_count[vi] > 0) { + impulse_buffer[vi] = impulse_buffer[vi] / (T)impulse_count[vi]; + // printf("impulse[%d] : %f %f %f\n",vi + // ,(float)impulse_buffer[vi][0] + // ,(float)impulse_buffer[vi][1] + // ,(float)impulse_buffer[vi][2]); + } + }); + + dtiles_t ccd_EE_collision_vis(verts.get_allocator(),{ + {"impulse",3}, + {"p0",3}, + {"p1",3}, + {"pp",3} + },verts.size()); + + cudaPol(zs::range(verts.size()),[ + ccd_EE_collision_vis = proxy({},ccd_EE_collision_vis), + impulse_buffer = proxy(impulse_buffer), + vtemp = proxy({},vtemp)] ZS_LAMBDA(int vi) mutable { + ccd_EE_collision_vis.tuple(dim_c<3>,"impulse",vi) = impulse_buffer[vi]; + ccd_EE_collision_vis.tuple(dim_c<3>,"p0",vi) = vtemp.pack(dim_c<3>,"x",vi); + ccd_EE_collision_vis.tuple(dim_c<3>,"p1",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); + ccd_EE_collision_vis.tuple(dim_c<3>,"pp",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi) + impulse_buffer[vi]; + }); + + ccd_EE_collision_vis = ccd_EE_collision_vis.clone({zs::memsrc_e::host}); + auto prim_EE = std::make_shared(); + auto& vis_verts_EE = prim_EE->verts; + auto& vis_lines_EE = prim_EE->lines; + vis_verts_EE.resize(verts.size() * 2); + vis_lines_EE.resize(verts.size()); + + ompPol(zs::range(verts.size()),[ + &vis_verts_EE,&vis_lines_EE, + scale = scale, + ccd_EE_collision_vis = proxy({},ccd_EE_collision_vis)] (int vi) mutable { + auto p = ccd_EE_collision_vis.pack(dim_c<3>,"p0",vi); + auto impulse = ccd_EE_collision_vis.pack(dim_c<3>,"impulse",vi); + auto pc = p + impulse * scale; + vis_verts_EE[vi * 2 + 0] = p.to_array(); + vis_verts_EE[vi * 2 + 1] = pc.to_array(); + vis_lines_EE[vi] = zeno::vec2i{vi * 2 + 0,vi * 2 + 1}; + }); + + set_output("impulse_EE",std::move(prim_EE)); } }; @@ -1478,7 +1359,7 @@ ZENDEFNODE(VisualizeContinousCollision, {{{"zsparticles"}, {"float","scale","0.25"}, // {"bool","add_repulsion_force","0"}, }, - {{"impulse_PT"}}, + {{"impulse_PT"},{"impulse_EE"}}, {}, {"PBD"}}); From 8b6b07e3e061456d230c96c4ae05f295d7d1bdf6 Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Fri, 15 Sep 2023 16:35:02 +0800 Subject: [PATCH 19/21] reorganize pbd collision code --- .../fem/collision_energy/collision_utils.hpp | 134 ++++++++++++++++++ .../collision_energy/evaluate_collision.hpp | 54 ++++--- projects/CuLagrange/pbd/Pipeline.cu | 3 - 3 files changed, 167 insertions(+), 24 deletions(-) diff --git a/projects/CuLagrange/fem/collision_energy/collision_utils.hpp b/projects/CuLagrange/fem/collision_energy/collision_utils.hpp index df631817d7..149f603885 100644 --- a/projects/CuLagrange/fem/collision_energy/collision_utils.hpp +++ b/projects/CuLagrange/fem/collision_energy/collision_utils.hpp @@ -21,6 +21,8 @@ namespace COLLISION_UTILS { using MATRIX3x12 = typename zs::vec; using MATRIX12 = typename zs::vec; + + /////////////////////////////////////////////////////////////////////// // should we reverse the direction of the force? /////////////////////////////////////////////////////////////////////// @@ -981,5 +983,137 @@ namespace COLLISION_UTILS { normal[0], normal[1], normal[2], intersect); } +// ps = [p_ea_0,p_ea_1,p_eb_0,p_eb_1] + constexpr bool compute_imminent_EE_collision_impulse(const VECTOR3 ps[4], + const VECTOR3 vs[4], + const REAL& scale, + const REAL& imminent_thickness, + const REAL minv[4], + VECTOR3 imps[4]) { + constexpr auto eps = 1e-7; + + VECTOR3 int_a{},int_b{}; + COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[2],ps[3],int_a,int_b); + auto ra = (ps[0] - int_a).norm() / (ps[0] - ps[1]).norm(); + auto rb = (ps[2] - int_b).norm() / (ps[2] - ps[3]).norm(); + + VECTOR4 bary{}; + bary[0] = ra - 1; + bary[1] = -ra; + bary[2] = 1 - rb; + bary[3] = rb; + + auto cm = (REAL).0; + for(int i = 0;i != 4;++i) + cm += bary[i] * bary[i] * minv[i]; + if(cm < eps){ + // for(int i = 0;i != 4;++i) + // imps[i] = VECTOR3::zeros(); + return false;; + } + + auto pr = VECTOR3::zeros(); + auto vr = VECTOR3::zeros(); + for(int i = 0;i != 4;++i) { + pr += bary[i] * ps[i]; + vr += bary[i] * vs[i]; + } + + if(pr.norm() > imminent_thickness) { + return false; + } + + VECTOR3 collision_nrm{}; + if(pr.norm() < 100 * eps) + collision_nrm = (ps[0] - ps[1]).cross(ps[2] - ps[3]).normalized(); + else + collision_nrm = pr.normalized(); + + if(collision_nrm.dot(vr) > 0) + collision_nrm = (REAL)-1 * collision_nrm; + + auto vr_nrm = collision_nrm.dot(vr); + auto impulse = -collision_nrm * vr_nrm * scale; + + for(int i = 0;i != 4;++i) + imps[i] = bary[i] * (minv[i] / cm) * impulse; + + return true; + } + +// ps = [p_ea_0,p_ea_1,p_eb_0,p_eb_1] + constexpr bool compute_continous_EE_collision_impulse( + const VECTOR3 ps[4], + const VECTOR3 vs[4], + const REAL minv[4], + VECTOR3 imps[4]) { + constexpr auto eps = 1e-7; + + auto alpha = (REAL)1.0; + + auto has_dynamic_points = false; + for(int i = 0;i != 4;++i) + if(minv[i] > eps) + has_dynamic_points = true; + + + if(!has_dynamic_points || !ee_accd(ps[0],ps[1],ps[2],ps[3],vs[0],vs[1],vs[2],vs[3],(REAL)0.2,(REAL)0.0,alpha)) { + for(int i = 0;i != 4;++i) + imps[i] = VECTOR3::zeros(); + return false; + } + + VECTOR3 nps[4] = {}; + for(int i = 0;i != 4;++i) + nps[i] = ps[i] + alpha * vs[i]; + +// there will surely be an imminent collision + return compute_imminent_EE_collision_impulse(nps,vs,(REAL)1 - alpha,std::numeric_limits::max(),minv,imps); + } + +// ps = [t0,t1,t2,p] + constexpr bool compute_imminent_PT_collision_impulse(const VECTOR3 ps[4], + const VECTOR3 vs[4], + const REAL& scale, + const REAL& imminent_thickness, + const REAL minv[4], + VECTOR3 imps[4]) { + constexpr auto eps = 1e-7; + auto tnrm = LSL_GEO::facet_normal(ps[0],ps[1],ps[2]); + + auto seg = ps[3] - ps[0]; + auto project_dist = zs::abs(seg.dot(tnrm)); + if(project_dist > imminent_thickness) { + return false; + } + + VECTOR3 bary_centric{}; + LSL_GEO::pointBaryCentric(ps[0],ps[1],ps[2],ps[3],bary_centric); + + if(distance > imminent_thickness) + return false; + + auto bary_sum = zs::abs(bary_centric[0]) + zs::abs(bary_centric[1]) + zs::abs(bary_centric[2]); + if(bary_sum > (REAL)(1.0 + eps * 1000)) { + return false; + } + + VECTOR4 bary{-bary_centric[0],-bary_centric[1],-bary_centric[2],1}; + + auto pr = VECTOR3::zeros(); + auto vr = VECTOR3::zeros(); + for(int i = 0;i != 4;++i) { + pr += bary[i] * ps[i]; + vr += bary[i] * vs[i]; + } + + auto collision_nrm = LSL_GEO::facet_normal(ps[0],ps[1],ps[2]); + auto align = collision_nrm.dot(pr); + if(align < 0) + collision_nrm *= -1; + + auto relative_normal_velocity = vr.dot(collision_nrm); + } + }; }; \ No newline at end of file diff --git a/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp b/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp index 64c01a3d01..2c2edab20d 100644 --- a/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp +++ b/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp @@ -1014,8 +1014,8 @@ void calc_imminent_self_PT_collision_impulse(Pol& pol, return; auto bary_sum = zs::abs(bary_centric[0]) + zs::abs(bary_centric[1]) + zs::abs(bary_centric[2]); - if(bary_sum > (T)(1.0 + eps * 10)) { - // return; + if(bary_sum > (T)(1.0 + eps * 100)) { + return; vec3 bnrms[3] = {}; auto hi = zs::reinterpret_bits(tris("he_inds",ti)); for(int i = 0;i != 3;++i){ @@ -1458,6 +1458,12 @@ void calc_continous_self_EE_collision_impulse(Pol& pol, vec4i inds{ea[0],ea[1],-1,-1}; + T minvs[4] = {}; + minvs[0] = verts("minv",ea[0]); + minvs[1] = verts("minv",ea[1]); + + vec3 imps[4] = {}; + auto bv = edgeBvs[ei]; auto do_close_proximity_detection = [&](int nei) mutable { @@ -1469,21 +1475,20 @@ void calc_continous_self_EE_collision_impulse(Pol& pol, return; } - bool has_dynamic_points = false; - for(int i = 0;i != 2;++i) { - if(verts("minv",ea[i]) > eps) - has_dynamic_points = true; - if(verts("minv",eb[i]) > eps) - has_dynamic_points = true; - } - if(!has_dynamic_points) - return; - for(int i = 0;i != 2;++i) { ps[i + 2] = verts.pack(dim_c<3>,xtag,eb[i]); vs[i + 2] = verts.pack(dim_c<3>,vtag,eb[i]); inds[i + 2] = eb[i]; + minvs[i + 2] = verts("minv",eb[i]); } +// The orginal reference code +#if 0 + auto has_dynamic_points = false; + for(int i = 0;i != 4;++i) + if(minvs[i] > eps) + has_dynamic_points = true; + if(!has_dynamic_points) + return; auto alpha = (T)1.0; if(!ee_accd(ps[0],ps[1],ps[2],ps[3],vs[0],vs[1],vs[2],vs[3],(T)0.2,(T)0.0,alpha)) @@ -1492,16 +1497,13 @@ void calc_continous_self_EE_collision_impulse(Pol& pol, vec3 nps[4] = {}; for(int i = 0;i != 4;++i) nps[i] = ps[i] + alpha * vs[i]; + + vec3 int_a{},int_b{}; COLLISION_UTILS::IntersectLineSegments(nps[0],nps[1],nps[2],nps[3],int_a,int_b); - auto ra = (nps[0] - int_a).norm() / (nps[0] - nps[1]).norm();; - auto rb = (nps[2] - int_b).norm() / (nps[2] - nps[3]).norm();; - - // if(ra < 10 * eps || ra > 1 - 10 * eps) - // return; - // if(rb < 10 * eps || rb > 1 - 10 * eps) - // return; + auto ra = (nps[0] - int_a).norm() / (nps[0] - nps[1]).norm(); + auto rb = (nps[2] - int_b).norm() / (nps[2] - nps[3]).norm(); vec4 bary{ra - 1,-ra,1 - rb,rb}; auto pr = vec3::zeros(); @@ -1527,13 +1529,23 @@ void calc_continous_self_EE_collision_impulse(Pol& pol, if(cm < eps) return; - auto impulse = -collision_nrm * vr_nrm * ((T)1 - alpha); + auto impulse = -collision_nrm * vr_nrm * ((T)1 - alpha); + for(int i = 0;i != 4;++i) { auto beta = (bary[i] * invMass("minv",inds[i])) / cm; atomic_add(exec_tag,&impulse_count[inds[i]],1); for(int d = 0;d != 3;++d) atomic_add(exec_tag,&impulse_buffer[inds[i]][d],impulse[d] * beta); - } + } +#else + if(!compute_continous_EE_collision_impulse(ps,vs,minvs,imps)) + return; + for(int i = 0;i != 4;++i) { + atomic_add(exec_tag,&impulse_count[inds[i]],1); + for(int d = 0;d != 3;++d) + atomic_add(exec_tag,&impulse_buffer[inds[i]][d],imps[i][d]); + } +#endif }; bvh.iter_neighbors(bv,do_close_proximity_detection); }); diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu index 0c4f42c911..20b230a7bd 100644 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ b/projects/CuLagrange/pbd/Pipeline.cu @@ -1139,10 +1139,7 @@ struct DetangleCCDCollision : INode { if(nm_ccd_collision.getVal() == 0) break; } - - std::cout << "finish solving continous collision " << std::endl; - cudaPol(zs::range(verts.size()),[ vtemp = proxy({},vtemp), verts = proxy({},verts), From 12f746bd9d5b91371274dfb1df88ffb4c16c82f0 Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Thu, 21 Sep 2023 21:34:21 +0800 Subject: [PATCH 20/21] tidy up pbd code --- projects/CuLagrange/CMakeLists.txt | 7 +- .../fem/collision_energy/collision_utils.hpp | 4 +- .../CuLagrange/geometry/kernel/geo_math.hpp | 4 +- .../CuLagrange/geometry/kernel/topology.hpp | 2 +- projects/CuLagrange/pbd/Constraints.cu | 681 ++++ projects/CuLagrange/pbd/KinematicCollision.cu | 122 + projects/CuLagrange/pbd/PBD.cuh | 117 - projects/CuLagrange/pbd/PBDInit.cu | 227 -- projects/CuLagrange/pbd/PBDPipeline.cu | 152 - projects/CuLagrange/pbd/Pipeline.cu | 2762 ----------------- projects/CuLagrange/pbd/SelfCollision.cu | 820 +++++ .../constraint_function_kernel/constraint.cuh | 19 +- 12 files changed, 1649 insertions(+), 3268 deletions(-) create mode 100644 projects/CuLagrange/pbd/Constraints.cu create mode 100644 projects/CuLagrange/pbd/KinematicCollision.cu delete mode 100644 projects/CuLagrange/pbd/PBD.cuh delete mode 100644 projects/CuLagrange/pbd/PBDInit.cu delete mode 100644 projects/CuLagrange/pbd/PBDPipeline.cu delete mode 100644 projects/CuLagrange/pbd/Pipeline.cu create mode 100644 projects/CuLagrange/pbd/SelfCollision.cu diff --git a/projects/CuLagrange/CMakeLists.txt b/projects/CuLagrange/CMakeLists.txt index cf4371fb40..fa05caca3f 100644 --- a/projects/CuLagrange/CMakeLists.txt +++ b/projects/CuLagrange/CMakeLists.txt @@ -35,9 +35,10 @@ target_sources(zeno PRIVATE # pbd target_sources(zeno PRIVATE - pbd/PBDInit.cu - pbd/PBDPipeline.cu - pbd/Pipeline.cu + pbd/Constraints.cu + pbd/SelfCollision.cu + pbd/KinematicCollision.cu + # pbd/Pipeline.cu ) # fem-cloth diff --git a/projects/CuLagrange/fem/collision_energy/collision_utils.hpp b/projects/CuLagrange/fem/collision_energy/collision_utils.hpp index 149f603885..17561c59e6 100644 --- a/projects/CuLagrange/fem/collision_energy/collision_utils.hpp +++ b/projects/CuLagrange/fem/collision_energy/collision_utils.hpp @@ -1090,8 +1090,8 @@ namespace COLLISION_UTILS { VECTOR3 bary_centric{}; LSL_GEO::pointBaryCentric(ps[0],ps[1],ps[2],ps[3],bary_centric); - if(distance > imminent_thickness) - return false; + // if(distance > imminent_thickness) + // return false; auto bary_sum = zs::abs(bary_centric[0]) + zs::abs(bary_centric[1]) + zs::abs(bary_centric[2]); if(bary_sum > (REAL)(1.0 + eps * 1000)) { diff --git a/projects/CuLagrange/geometry/kernel/geo_math.hpp b/projects/CuLagrange/geometry/kernel/geo_math.hpp index 36f0349163..3c0513ad58 100644 --- a/projects/CuLagrange/geometry/kernel/geo_math.hpp +++ b/projects/CuLagrange/geometry/kernel/geo_math.hpp @@ -54,8 +54,8 @@ namespace zeno { namespace LSL_GEO { constexpr auto cotTheta(const zs::VecInterface& e0,const zs::VecInterface& e1){ auto de0 = e0.cast(); auto de1 = e1.cast(); - auto costheta = e0.dot(e1); - auto sintheta = e0.cross(e1).norm(); + auto costheta = de0.dot(de1); + auto sintheta = de0.cross(de1).norm(); return (REAL)(costheta / sintheta); } diff --git a/projects/CuLagrange/geometry/kernel/topology.hpp b/projects/CuLagrange/geometry/kernel/topology.hpp index 4b380566b2..883952e991 100644 --- a/projects/CuLagrange/geometry/kernel/topology.hpp +++ b/projects/CuLagrange/geometry/kernel/topology.hpp @@ -1411,7 +1411,7 @@ namespace zeno { auto otri = tris.pack(dim_c<3>,"inds",oti,int_c); // tb_topos[id] = zs::vec(tri[(vid + 0) % 3],tri[(vid + 1) % 3],tri[(vid + 2) % 3],otri[(ovid + 2) % 3]); - tb_topos[id] = zs::vec(tri[(vid + 2) % 3],otri[(ovid + 2) % 3],tri[(vid + 1) % 3],tri[(vid + 0) % 3]); + tb_topos[id] = zs::vec(tri[(vid + 2) % 3],otri[(ovid + 2) % 3],tri[(vid + 0) % 3],tri[(vid + 1) % 3]); }); } diff --git a/projects/CuLagrange/pbd/Constraints.cu b/projects/CuLagrange/pbd/Constraints.cu new file mode 100644 index 0000000000..79b6baea4a --- /dev/null +++ b/projects/CuLagrange/pbd/Constraints.cu @@ -0,0 +1,681 @@ +#include "Structures.hpp" +#include "zensim/Logger.hpp" +#include "zensim/cuda/execution/ExecutionPolicy.cuh" +#include "zensim/omp/execution/ExecutionPolicy.hpp" +#include "zensim/io/MeshIO.hpp" +#include "zensim/math/bit/Bits.h" +#include "zensim/types/Property.h" +#include +#include +#include +#include +#include +#include + +#include "constraint_function_kernel/constraint.cuh" +#include "../geometry/kernel/tiled_vector_ops.hpp" +#include "../geometry/kernel/topology.hpp" +#include "../geometry/kernel/geo_math.hpp" +// #include "../fem/collision_energy/evaluate_collision.hpp" +#include "constraint_function_kernel/constraint_types.hpp" + +namespace zeno { + + +// we only need to record the topo here +// serve triangulate mesh or strands only currently +struct MakeSurfaceConstraintTopology : INode { + template + void buildBvh(zs::CudaExecutionPolicy &pol, + TileVecT &verts, + const zs::SmallString& srcTag, + const zs::SmallString& dstTag, + const zs::SmallString& pscaleTag, + ZenoLinearBvh::lbvh_t &bvh) { + using namespace zs; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto space = execspace_e::cuda; + Vector bvs{verts.get_allocator(), verts.size()}; + pol(range(verts.size()), + [verts = proxy({}, verts), + bvs = proxy(bvs), + pscaleTag,srcTag,dstTag] ZS_LAMBDA(int vi) mutable { + auto src = verts.template pack<3>(srcTag, vi); + auto dst = verts.template pack<3>(dstTag, vi); + auto pscale = verts(pscaleTag,vi); + + bv_t bv{src,dst}; + bv._min -= pscale; + bv._max += pscale; + bvs[vi] = bv; + }); + bvh.build(pol, bvs); + } + + virtual void apply() override { + using namespace zs; + using namespace PBD_CONSTRAINT; + + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = zs::cuda_exec(); + + auto source = get_input("source"); + auto constraint = std::make_shared(); + + auto type = get_input2("topo_type"); + + if(source->category != ZenoParticles::surface) + throw std::runtime_error("Try adding Constraint topology to non-surface ZenoParticles"); + + const auto& verts = source->getParticles(); + const auto& quads = source->getQuadraturePoints(); + + auto uniform_stiffness = get_input2("stiffness"); + + zs::Vector colors{quads.get_allocator(),0}; + zs::Vector reordered_map{quads.get_allocator(),0}; + zs::Vector color_offset{quads.get_allocator(),0}; + + constraint->sprayedOffset = 0; + constraint->elements = typename ZenoParticles::particles_t({{"stiffness",1},{"lambda",1},{"tclr",1}}, 0, zs::memsrc_e::device,0); + auto &eles = constraint->getQuadraturePoints(); + + if(type == "stretch") { + constraint->setMeta(CONSTRAINT_KEY,category_c::edge_length_constraint); + auto quads_vec = tilevec_topo_to_zsvec_topo(cudaPol,quads,wrapv<3>{}); + zs::Vector> edge_topos{quads.get_allocator(),0}; + retrieve_edges_topology(cudaPol,quads_vec,edge_topos); + eles.resize(edge_topos.size()); + + topological_coloring(cudaPol,edge_topos,colors); + sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); + // std::cout << "quads.size() = " << quads.size() << "\t" << "edge_topos.size() = " << edge_topos.size() << std::endl; + eles.append_channels(cudaPol,{{"inds",2},{"r",1}}); + + auto rest_scale = get_input2("rest_scale"); + + cudaPol(zs::range(eles.size()),[ + verts = proxy({},verts), + eles = proxy({},eles), + reordered_map = proxy(reordered_map), + uniform_stiffness = uniform_stiffness, + colors = proxy(colors), + rest_scale = rest_scale, + edge_topos = proxy(edge_topos)] ZS_LAMBDA(int oei) mutable { + auto ei = reordered_map[oei]; + eles.tuple(dim_c<2>,"inds",oei) = edge_topos[ei].reinterpret_bits(float_c); + vec3 x[2] = {}; + for(int i = 0;i != 2;++i) + x[i] = verts.pack(dim_c<3>,"x",edge_topos[ei][i]); + eles("r",oei) = (x[0] - x[1]).norm() * rest_scale; + }); + + } + + if(type == "bending") { + constraint->setMeta(CONSTRAINT_KEY,category_c::isometric_bending_constraint); + // constraint->category = ZenoParticles::tri_bending_spring; + // constraint->sprayedOffset = 0; + + const auto& halfedges = (*source)[ZenoParticles::s_surfHalfEdgeTag]; + + zs::Vector> bd_topos{quads.get_allocator(),0}; + retrieve_tri_bending_topology(cudaPol,quads,halfedges,bd_topos); + + eles.resize(bd_topos.size()); + + topological_coloring(cudaPol,bd_topos,colors); + sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); + // std::cout << "quads.size() = " << quads.size() << "\t" << "edge_topos.size() = " << edge_topos.size() << std::endl; + + eles.append_channels(cudaPol,{{"inds",4},{"Q",4 * 4}}); + + // std::cout << "halfedges.size() = " << halfedges.size() << "\t" << "bd_topos.size() = " << bd_topos.size() << std::endl; + + cudaPol(zs::range(eles.size()),[ + eles = proxy({},eles), + bd_topos = proxy(bd_topos), + reordered_map = proxy(reordered_map), + verts = proxy({},verts)] ZS_LAMBDA(int oei) mutable { + auto ei = reordered_map[oei]; + // printf("bd_topos[%d] : %d %d %d %d\n",ei,bd_topos[ei][0],bd_topos[ei][1],bd_topos[ei][2],bd_topos[ei][3]); + eles.tuple(dim_c<4>,"inds",oei) = bd_topos[ei].reinterpret_bits(float_c); + vec3 x[4] = {}; + for(int i = 0;i != 4;++i) + x[i] = verts.pack(dim_c<3>,"x",bd_topos[ei][i]); + + mat4 Q = mat4::uniform(0); + CONSTRAINT::init_IsometricBendingConstraint(x[0],x[1],x[2],x[3],Q); + eles.tuple(dim_c<16>,"Q",oei) = Q; + }); + } + if(type == "kcollision") { + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + + constraint->setMeta(CONSTRAINT_KEY,category_c::p_kp_collision_constraint); + auto target = get_input("target"); + + const auto& kverts = target->getParticles(); + ZenoLinearBvh::lbvh_t kbvh{}; + buildBvh(cudaPol,kverts,"px","x","pscale",kbvh); + + zs::bht csPP{verts.get_allocator(),verts.size()}; + csPP.reset(cudaPol,true); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + kverts = proxy({},kverts), + kbvh = proxy(kbvh), + csPP = proxy(csPP)] ZS_LAMBDA(int vi) mutable { + auto x = verts.pack(dim_c<3>,"x",vi); + auto px = verts.pack(dim_c<3>,"px",vi); + auto mx = (x + px) / (T)2.0; + auto pscale = verts("pscale",vi); + + auto radius = (mx - px).norm() + pscale * (T)2.0; + auto bv = bv_t{mx - radius,mx + radius}; + + int contact_kvi = -1; + T min_contact_time = std::numeric_limits::max(); + + auto process_ccd_collision = [&](int kvi) { + auto kpscale = kverts("pscale",kvi); + auto kx = kverts.pack(dim_c<3>,"x",kvi); + auto pkx = kx; + if(kverts.hasProperty("px")) + pkx = kverts.pack(dim_c<3>,"px",kvi); + + auto t = LSL_GEO::ray_ray_intersect(px,x - px,pkx,kx - pkx,(pscale + kpscale) * (T)2); + if(t < min_contact_time) { + min_contact_time = t; + contact_kvi = kvi; + } + }; + kbvh.iter_neighbors(bv,process_ccd_collision); + + if(contact_kvi >= 0) { + csPP.insert(vec2i{vi,contact_kvi}); + } + }); + + eles.resize(csPP.size()); + colors.resize(csPP.size()); + reordered_map.resize(csPP.size()); + + eles.append_channels(cudaPol,{{"inds",2}}); + cudaPol(zip(zs::range(csPP.size()),csPP._activeKeys),[ + eles = proxy({},eles), + colors = proxy(colors), + reordered_map = proxy(reordered_map)] ZS_LAMBDA(auto ei,const auto& pair) mutable { + eles.tuple(dim_c<2>,"inds",ei) = pair.reinterpret_bits(float_c); + colors[ei] = (T)0; + reordered_map[ei] = ei; + }); + + color_offset.resize(1); + color_offset.setVal(0); + } + + if(type == "attachment") { + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + + } + + cudaPol(zs::range(eles.size()),[ + eles = proxy({},eles), + uniform_stiffness = uniform_stiffness, + colors = proxy(colors), + // exec_tag, + reordered_map = proxy(reordered_map)] ZS_LAMBDA(int oei) mutable { + auto ei = reordered_map[oei]; + eles("lambda",oei) = 0.0; + eles("stiffness",oei) = uniform_stiffness; + eles("tclr",oei) = colors[ei]; + // auto + }); + + constraint->setMeta("color_offset",color_offset); + + // set_output("source",source); + set_output("constraint",constraint); + }; +}; + +ZENDEFNODE(MakeSurfaceConstraintTopology, {{ + {"source"}, + {"target"}, + {"float","stiffness","0.5"}, + {"string","topo_type","stretch"}, + {"float","rest_scale","1.0"} + }, + {{"constraint"}}, + { + // {"string","groupID",""}, + }, + {"PBD"}}); + + +struct VisualizePBDConstraint : INode { + using T = float; + using vec3 = zs::vec; + // using tiles_t = typename ZenoParticles::particles_t; + // using dtiles_t = zs::TileVector; + + virtual void apply() override { + using namespace zs; + using namespace PBD_CONSTRAINT; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + + auto zsparticles = get_input("zsparticles"); + auto constraints_ptr = get_input("constraints"); + + const auto& geo_verts = zsparticles->getParticles(); + const auto& constraints = constraints_ptr->getQuadraturePoints(); + + auto tclr_tag = get_param("tclrTag"); + + zs::Vector cvis{geo_verts.get_allocator(),constraints.getChannelSize("inds") * constraints.size()}; + zs::Vector cclrs{constraints.get_allocator(),constraints.size()}; + int cdim = constraints.getChannelSize("inds"); + cudaPol(zs::range(constraints.size()),[ + constraints = proxy({},constraints), + geo_verts = proxy({},geo_verts), + cclrs = proxy(cclrs), + tclr_tag = zs::SmallString(tclr_tag), + cdim = cdim, + cvis = proxy(cvis)] ZS_LAMBDA(int ci) mutable { + // auto cdim = constraints.propertySize("inds"); + for(int i = 0;i != cdim;++i) { + auto vi = zs::reinterpret_bits(constraints("inds",i,ci)); + cvis[ci * cdim + i] = geo_verts.pack(dim_c<3>,"x",vi); + } + cclrs[ci] = (int)constraints(tclr_tag,ci); + }); + + constexpr auto omp_space = execspace_e::openmp; + auto ompPol = omp_exec(); + + cvis = cvis.clone({zs::memsrc_e::host}); + cclrs = cclrs.clone({zs::memsrc_e::host}); + auto prim = std::make_shared(); + auto& pverts = prim->verts; + + auto constraint_type = constraints_ptr->readMeta(CONSTRAINT_KEY,wrapt{}); + + if(constraint_type == category_c::edge_length_constraint) { + pverts.resize(constraints.size() * 2); + auto& plines = prim->lines; + plines.resize(constraints.size()); + auto& tclrs = pverts.add_attr(tclr_tag); + auto& ltclrs = plines.add_attr(tclr_tag); + + ompPol(zs::range(constraints.size()),[ + <clrs,&pverts,&plines,&tclrs,cvis = proxy(cvis),cclrs = proxy(cclrs)] (int ci) mutable { + pverts[ci * 2 + 0] = cvis[ci * 2 + 0].to_array(); + pverts[ci * 2 + 1] = cvis[ci * 2 + 1].to_array(); + tclrs[ci * 2 + 0] = cclrs[ci]; + tclrs[ci * 2 + 1] = cclrs[ci]; + plines[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + ltclrs[ci] = cclrs[ci]; + }); + }else if(constraint_type == category_c::isometric_bending_constraint) { + pverts.resize(constraints.size() * 2); + auto& plines = prim->lines; + plines.resize(constraints.size()); + auto& tclrs = pverts.add_attr(tclr_tag); + auto& ltclrs = plines.add_attr(tclr_tag); + + ompPol(zs::range(constraints.size()),[ + <clrs,&pverts,&plines,&tclrs,cvis = proxy(cvis),cclrs = proxy(cclrs)] (int ci) mutable { + zeno::vec3f cverts[4] = {}; + for(int i = 0;i != 4;++i) + cverts[i] = cvis[ci * 4 + i].to_array(); + + pverts[ci * 2 + 0] = (cverts[0] + cverts[2] + cverts[3]) / (T)3.0; + pverts[ci * 2 + 1] = (cverts[1] + cverts[2] + cverts[3]) / (T)3.0; + tclrs[ci * 2 + 0] = cclrs[ci]; + tclrs[ci * 2 + 1] = cclrs[ci]; + ltclrs[ci] = cclrs[ci]; + + plines[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + }); + } + + set_output("prim",std::move(prim)); + } +}; + +ZENDEFNODE(VisualizePBDConstraint, {{{"zsparticles"},{"constraints"}}, + {{"prim"}}, + { + {"string","tclrTag","tclrTag"}, + }, + {"PBD"}}); + +// solve a specific type of constraint for one iterations +struct XPBDSolve : INode { + + virtual void apply() override { + using namespace zs; + using namespace PBD_CONSTRAINT; + + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + + auto zsparticles = get_input("zsparticles"); + auto constraints = get_input("constraints"); + + auto dt = get_input2("dt"); + auto ptag = get_param("ptag"); + + auto coffsets = constraints->readMeta("color_offset",zs::wrapt>{}); + int nm_group = coffsets.size(); + + auto& verts = zsparticles->getParticles(); + auto& cquads = constraints->getQuadraturePoints(); + auto category = constraints->readMeta(CONSTRAINT_KEY,wrapt{}); + auto target = get_input("target"); + const auto& kverts = target->getParticles(); + // zs::Vector pv_buffer{verts.get_allocator(),verts.size()}; + // zs::Vector total_ghost_impulse_X{verts.get_allocator(),1}; + // zs::Vector total_ghost_impulse_Y{verts.get_allocator(),1}; + // zs::Vector total_ghost_impulse_Z{verts.get_allocator(),1}; + + // std::cout << "SOVLE CONSTRAINT WITH GROUP : " << nm_group << "\t" << cquads.size() << std::endl; + + + + for(int g = 0;g != nm_group;++g) { + + // if(category == category_c::isometric_bending_constraint) + // break; + + auto coffset = coffsets.getVal(g); + int group_size = 0; + if(g == nm_group - 1) + group_size = cquads.size() - coffsets.getVal(g); + else + group_size = coffsets.getVal(g + 1) - coffsets.getVal(g); + + // cudaPol(zs::range(verts.size()),[ + // ptag = zs::SmallString(ptag), + // verts = proxy({},verts), + // pv_buffer = proxy(pv_buffer)] ZS_LAMBDA(int vi) mutable { + // pv_buffer[vi] = verts.pack(dim_c<3>,ptag,vi); + // }); + + + + cudaPol(zs::range(group_size),[ + coffset, + verts = proxy({},verts), + category, + dt, + ptag = zs::SmallString(ptag), + kverts = proxy({},kverts), + cquads = proxy({},cquads)] ZS_LAMBDA(int gi) mutable { + float s = cquads("stiffness",coffset + gi); + float lambda = cquads("lambda",coffset + gi); + + if(category == category_c::edge_length_constraint) { + auto edge = cquads.pack(dim_c<2>,"inds",coffset + gi,int_c); + vec3 p0{},p1{}; + p0 = verts.pack(dim_c<3>,ptag,edge[0]); + p1 = verts.pack(dim_c<3>,ptag,edge[1]); + float minv0 = verts("minv",edge[0]); + float minv1 = verts("minv",edge[1]); + float r = cquads("r",coffset + gi); + + vec3 dp0{},dp1{}; + CONSTRAINT::solve_DistanceConstraint( + p0,minv0, + p1,minv1, + r, + s, + dt, + lambda, + dp0,dp1); + + + verts.tuple(dim_c<3>,ptag,edge[0]) = p0 + dp0; + verts.tuple(dim_c<3>,ptag,edge[1]) = p1 + dp1; + + // float m0 = verts("m",edge[0]); + // float m1 = verts("m",edge[1]); + // auto ghost_impulse = (dp0 * m0 + dp1 * m1).norm(); + // if(ghost_impulse > 1e-6) + // printf("dmomentum : %f\n",(float)ghost_impulse); + } + if(category == category_c::isometric_bending_constraint) { + auto quad = cquads.pack(dim_c<4>,"inds",coffset + gi,int_c); + vec3 p[4] = {}; + float minv[4] = {}; + for(int i = 0;i != 4;++i) { + p[i] = verts.pack(dim_c<3>,ptag,quad[i]); + minv[i] = verts("minv",quad[i]); + } + + auto Q = cquads.pack(dim_c<4,4>,"Q",coffset + gi); + + vec3 dp[4] = {}; + CONSTRAINT::solve_IsometricBendingConstraint( + p[0],minv[0], + p[1],minv[1], + p[2],minv[2], + p[3],minv[3], + Q, + s, + dt, + lambda, + dp[0],dp[1],dp[2],dp[3]); + + for(int i = 0;i != 4;++i) { + // printf("dp[%d][%d] : %f %f %f %f\n",gi,i,s,(float)dp[i][0],(float)dp[i][1],(float)dp[i][2]); + verts.tuple(dim_c<3>,ptag,quad[i]) = p[i] + dp[i]; + } + + + } + + if(category == category_c::p_kp_collision_constraint) { + auto quad = cquads.pack(dim_c<2>,"inds",coffset + gi,int_c); + + vec3 p = verts.pack(dim_c<3>,ptag,quad[0]); + vec3 kp = kverts.pack(dim_c<3>,"x",quad[1]); + vec3 knrm = kverts.pack(dim_c<3>,"nrm",quad[1]); + + auto pscale = verts("pscale",quad[0]); + auto kpscale = kverts("pscale",quad[1]); + + T minv = verts("minv",quad[0]); + // T kminv = kverts("minv",quad[1]); + + vec3 dp = {}; + auto thickness = pscale + kpscale; + thickness *= (float)1.01; + CONSTRAINT::solve_PlaneConstraint(p,minv,kp,knrm,thickness,s,dt,lambda,dp); + verts.tuple(dim_c<3>,ptag,quad[0]) = p + dp; + } + + cquads("lambda",coffset + gi) = lambda; + }); + + } + + // cudaPol(zs::range(verts.size())) + + set_output("constraints",constraints); + set_output("zsparticles",zsparticles); + set_output("target",target); + }; +}; + +ZENDEFNODE(XPBDSolve, {{{"zsparticles"},{"constraints"},{"target"},{"float","dt","0.5"}}, + {{"zsparticles"},{"constraints"},{"target"}}, + {{"string","ptag","X"}}, + {"PBD"}}); + +struct XPBDSolveSmooth : INode { + + virtual void apply() override { + using namespace zs; + using namespace PBD_CONSTRAINT; + + using vec3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + + auto zsparticles = get_input("zsparticles"); + + auto all_constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "all_constraints"); + auto ptag = get_param("ptag"); + auto w = get_input2("relaxation_strength"); + + auto& verts = zsparticles->getParticles(); + + zs::Vector dp_buffer{verts.get_allocator(),verts.size() * 3}; + cudaPol(zs::range(dp_buffer),[]ZS_LAMBDA(auto& v) {v = 0;}); + zs::Vector dp_count{verts.get_allocator(),verts.size()}; + cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) {c = 0;}); + + for(auto &&constraints : all_constraints) { + const auto& cquads = constraints->getQuadraturePoints(); + auto category = constraints->readMeta(CONSTRAINT_KEY,wrapt{}); + + cudaPol(zs::range(cquads.size()),[ + verts = proxy({},verts), + category, + // dt, + // w, + exec_tag, + dp_buffer = proxy(dp_buffer), + dp_count = proxy(dp_count), + ptag = zs::SmallString(ptag), + cquads = proxy({},cquads)] ZS_LAMBDA(int ci) mutable { + float s = cquads("stiffness",ci); + float lambda = cquads("lambda",ci); + + if(category == category_c::edge_length_constraint) { + auto edge = cquads.pack(dim_c<2>,"inds",ci,int_c); + vec3 p0{},p1{}; + p0 = verts.pack(dim_c<3>,ptag,edge[0]); + p1 = verts.pack(dim_c<3>,ptag,edge[1]); + float minv0 = verts("minv",edge[0]); + float minv1 = verts("minv",edge[1]); + float r = cquads("r",ci); + + vec3 dp0{},dp1{}; + if(!CONSTRAINT::solve_DistanceConstraint( + p0,minv0, + p1,minv1, + r, + (float)1, + dp0,dp1)) { + + for(int i = 0;i != 3;++i) + atomic_add(exec_tag,&dp_buffer[edge[0] * 3 + i],dp0[i]); + for(int i = 0;i != 3;++i) + atomic_add(exec_tag,&dp_buffer[edge[1] * 3 + i],dp1[i]); + + atomic_add(exec_tag,&dp_count[edge[0]],(int)1); + atomic_add(exec_tag,&dp_count[edge[1]],(int)1); + } + } + if(category == category_c::isometric_bending_constraint) { + return; + auto quad = cquads.pack(dim_c<4>,"inds",ci,int_c); + vec3 p[4] = {}; + float minv[4] = {}; + for(int i = 0;i != 4;++i) { + p[i] = verts.pack(dim_c<3>,ptag,quad[i]); + minv[i] = verts("minv",quad[i]); + } + + auto Q = cquads.pack(dim_c<4,4>,"Q",ci); + + vec3 dp[4] = {}; + float lambda = 0; + CONSTRAINT::solve_IsometricBendingConstraint( + p[0],minv[0], + p[1],minv[1], + p[2],minv[2], + p[3],minv[3], + Q, + (float)1, + dp[0], + dp[1], + dp[2], + dp[3]); + + auto has_nan = false; + for(int i = 0;i != 4;++i) + if(zs::isnan(dp[i].norm())) + has_nan = true; + if(has_nan) { + printf("nan dp detected : %f %f %f %f %f %f %f %f\n", + (float)p[0].norm(), + (float)p[1].norm(), + (float)p[2].norm(), + (float)p[3].norm(), + (float)dp[0].norm(), + (float)dp[1].norm(), + (float)dp[2].norm(), + (float)dp[3].norm()); + return; + } + for(int i = 0;i != 4;++i) + for(int j = 0;j != 3;++j) + atomic_add(exec_tag,&dp_buffer[quad[i] * 3 + j],dp[i][j]); + for(int i = 0;i != 4;++i) + atomic_add(exec_tag,&dp_count[quad[i]],(int)1); + } + }); + } + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + ptag = zs::SmallString(ptag),w, + dp_count = proxy(dp_count), + dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { + if(dp_count[vi] > 0) { + auto dp = w * vec3{dp_buffer[vi * 3 + 0],dp_buffer[vi * 3 + 1],dp_buffer[vi * 3 + 2]}; + verts.tuple(dim_c<3>,ptag,vi) = verts.pack(dim_c<3>,ptag,vi) + dp / (T)dp_count[vi]; + } + }); + + // set_output("all_constraints",all_constraints); + set_output("zsparticles",zsparticles); + }; +}; + +ZENDEFNODE(XPBDSolveSmooth, {{{"zsparticles"},{"all_constraints"},{"float","relaxation_strength","1"}}, + {{"zsparticles"}}, + {{"string","ptag","x"}}, + {"PBD"}}); + + + + + +}; diff --git a/projects/CuLagrange/pbd/KinematicCollision.cu b/projects/CuLagrange/pbd/KinematicCollision.cu new file mode 100644 index 0000000000..475386644c --- /dev/null +++ b/projects/CuLagrange/pbd/KinematicCollision.cu @@ -0,0 +1,122 @@ +#include "Structures.hpp" +#include "zensim/Logger.hpp" +#include "zensim/cuda/execution/ExecutionPolicy.cuh" +#include "zensim/omp/execution/ExecutionPolicy.hpp" +#include "zensim/io/MeshIO.hpp" +#include "zensim/math/bit/Bits.h" +#include "zensim/types/Property.h" +#include +#include +#include +#include +#include +#include + +#include "../geometry/kernel/tiled_vector_ops.hpp" +#include "../geometry/kernel/topology.hpp" +#include "../geometry/kernel/geo_math.hpp" + +namespace zeno { + + +struct SDFColliderProject : INode { + using T = float; + using vec3 = zs::vec; + + virtual void apply() override { + using namespace zs; + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + + auto zsparticles = get_input("zsparticles"); + + auto radius = get_input2("radius"); + auto center = get_input2("center"); + auto cv = get_input2("center_velocity"); + auto w = get_input2("angular_velocity"); + + // prex + auto xtag = get_input2("xtag"); + // x + auto ptag = get_input2("ptag"); + auto friction = get_input2("friction"); + + // auto do_stablize = get_input2("do_stablize"); + + auto& verts = zsparticles->getParticles(); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + xtag = zs::SmallString(xtag), + ptag = zs::SmallString(ptag), + friction, + radius, + center, + // do_stablize, + cv,w] ZS_LAMBDA(int vi) mutable { + if(verts("minv",vi) < (T)1e-6) + return; + + auto pred = verts.pack(dim_c<3>,ptag,vi); + auto pos = verts.pack(dim_c<3>,xtag,vi); + + auto center_vel = vec3::from_array(cv); + auto center_pos = vec3::from_array(center); + auto angular_velocity = vec3::from_array(w); + + auto disp = pred - center_pos; + auto dist = radius - disp.norm() + verts("pscale",vi); + + if(dist < 0) + return; + + auto nrm = disp.normalized(); + + auto dp = dist * nrm; + if(dp.norm() < (T)1e-6) + return; + + pred += dp; + + // if(do_stablize) { + // pos += dp; + // verts.tuple(dim_c<3>,xtag,vi) = pos; + // } + + auto collider_velocity_at_p = center_vel + angular_velocity.cross(pred - center_pos); + auto rel_vel = pred - pos - center_vel; + + auto tan_vel = rel_vel - nrm * rel_vel.dot(nrm); + auto tan_len = tan_vel.norm(); + auto max_tan_len = dp.norm() * friction; + + if(tan_len > (T)1e-6) { + auto alpha = (T)max_tan_len / (T)tan_len; + dp = -tan_vel * zs::min(alpha,(T)1.0); + pred += dp; + } + + // dp = dp * verts("m",vi) * verts("minv",vi); + + verts.tuple(dim_c<3>,ptag,vi) = pred; + }); + set_output("zsparticles",zsparticles); + } + +}; + +ZENDEFNODE(SDFColliderProject, {{{"zsparticles"}, + {"float","radius","1"}, + {"center"}, + {"center_velocity"}, + {"angular_velocity"}, + {"string","xtag","x"}, + {"string","ptag","x"}, + {"float","friction","0"} + // {"bool","do_stablize","0"} + }, + {{"zsparticles"}}, + {}, + {"PBD"}}); + +}; \ No newline at end of file diff --git a/projects/CuLagrange/pbd/PBD.cuh b/projects/CuLagrange/pbd/PBD.cuh deleted file mode 100644 index 6bbae36a99..0000000000 --- a/projects/CuLagrange/pbd/PBD.cuh +++ /dev/null @@ -1,117 +0,0 @@ -#pragma once -#include "Structures.hpp" -#include "zensim/container/Bvh.hpp" -#include "zensim/container/Bvs.hpp" -#include "zensim/container/HashTable.hpp" -#include "zensim/container/Vector.hpp" -#include "zensim/cuda/Cuda.h" -#include "zensim/cuda/execution/ExecutionPolicy.cuh" -#include "zensim/math/Vec.h" -#include -#include -#include - -namespace zeno { - -struct PBDSystem : IObject { - using T = float; - using Ti = zs::conditional_t, zs::i64, zs::i32>; - using dtiles_t = zs::TileVector; - using tiles_t = typename ZenoParticles::particles_t; - using vec3 = zs::vec; - using mat3 = zs::vec; - using ivec3 = zs::vec; - using ivec2 = zs::vec; - using bvh_t = zs::LBvh<3, int, T>; - using bv_t = zs::AABBBox<3, T>; - - struct PrimitiveHandle { - PrimitiveHandle(ZenoParticles &zsprim, std::size_t &vOffset, std::size_t &sfOffset, std::size_t &seOffset, - std::size_t &svOffset, zs::wrapv<4>); - - void initGeo(); - - auto getModelLameParams() const { - T mu, lam; - zs::match([&](const auto &model) { - mu = model.mu; - lam = model.lam; - })(models.getElasticModel()); - return zs::make_tuple(mu, lam); - } - - decltype(auto) getVerts() const { - return *vertsPtr; - } - decltype(auto) getEles() const { - return *elesPtr; - } - decltype(auto) getEdges() const { - return *edgesPtr; - } - decltype(auto) getSurfTris() const { - return *surfTrisPtr; - } - decltype(auto) getSurfEdges() const { - return *surfEdgesPtr; - } - decltype(auto) getSurfVerts() const { - return *surfVertsPtr; - } - bool isBoundary() const noexcept { - return zsprimPtr->asBoundary; - } - - std::shared_ptr zsprimPtr{}; // nullptr if it is an auxiliary object - const ZenoConstitutiveModel ⊧ - std::shared_ptr vertsPtr; - std::shared_ptr elesPtr; - std::shared_ptr edgesPtr; - std::shared_ptr surfTrisPtr; - std::shared_ptr surfEdgesPtr; - std::shared_ptr surfVertsPtr; - // typename ZenoParticles::dtiles_t etemp; - // typename ZenoParticles::dtiles_t svtemp; - const std::size_t vOffset, sfOffset, seOffset, svOffset; - ZenoParticles::category_e category; - }; - - void initialize(zs::CudaExecutionPolicy &pol); - PBDSystem(std::vector zsprims, vec3 extForce, T dt, int numSolveIters, T ec, T vc); - - void reinitialize(zs::CudaExecutionPolicy &pol, T framedt); - void writebackPositionsAndVelocities(zs::CudaExecutionPolicy &pol); - - /// pipeline - void preSolve(zs::CudaExecutionPolicy &pol); - void solveEdge(zs::CudaExecutionPolicy &pol); - void solveVolume(zs::CudaExecutionPolicy &pol); - void postSolve(zs::CudaExecutionPolicy &pol); - - // sim params - vec3 extForce; - int solveIterCap = 50; - T edgeCompliance = 0.001, volumeCompliance = 0.001; - - // - std::vector prims; - std::size_t coOffset, numDofs; - std::size_t sfOffset, seOffset, svOffset; - - dtiles_t vtemp; - - zs::Vector temp; - - // boundary contacts - // auxiliary data (spatial acceleration) - tiles_t stInds, seInds, svInds; - // using bvs_t = zs::LBvs<3, int, T>; - bvh_t stBvh, seBvh; // for simulated objects - // bvs_t stBvs, seBvs; // STQ - // bvh_t bouStBvh, bouSeBvh; // for collision objects - // bvs_t bouStBvs, bouSeBvs; // STQ - T dt, framedt; -}; -// config compliance, num solve iters, edge/volume extforce, dt - -} // namespace zeno \ No newline at end of file diff --git a/projects/CuLagrange/pbd/PBDInit.cu b/projects/CuLagrange/pbd/PBDInit.cu deleted file mode 100644 index 4f47fbb3c3..0000000000 --- a/projects/CuLagrange/pbd/PBDInit.cu +++ /dev/null @@ -1,227 +0,0 @@ -#include "PBD.cuh" -#include "Utils.hpp" - -namespace zeno { - -void PBDSystem::PrimitiveHandle::initGeo() { - // init rest volumes & edge lengths - auto cudaPol = zs::cuda_exec(); - using namespace zs; - constexpr auto space = zs::execspace_e::cuda; - elesPtr->append_channels(cudaPol, {{"rv", 1}}); - cudaPol(zs::Collapse{elesPtr->size()}, - [eles = proxy({}, *elesPtr), verts = proxy({}, *vertsPtr)] ZS_LAMBDA(int ei) mutable { - auto quad = eles.template pack<4>("inds", ei).template reinterpret_bits(); - vec3 xs[4]; - for (int d = 0; d != 4; ++d) - xs[d] = verts.template pack<3>("x", quad[d]); - vec3 ds[3] = {xs[1] - xs[0], xs[2] - xs[0], xs[3] - xs[0]}; -#if 0 - mat3 D{}; - for (int d = 0; d != 3; ++d) - for (int i = 0; i != 3; ++i) - D(d, i) = ds[i][d]; - T vol = zs::abs(zs::determinant(D)) / 6; -#else - T vol = zs::abs((ds[0]).cross(ds[1]).dot(ds[2])) / 6; -#endif - eles("rv", ei) = vol; - }); - edgesPtr->append_channels(cudaPol, {{"rl", 1}}); - cudaPol(zs::Collapse{edgesPtr->size()}, - [ses = proxy({}, *edgesPtr), verts = proxy({}, *vertsPtr)] ZS_LAMBDA(int sei) mutable { - auto line = ses.template pack<2>("inds", sei).template reinterpret_bits(); - vec3 xs[2]; - for (int d = 0; d != 2; ++d) - xs[d] = verts.template pack<3>("x", line[d]); - ses("rl", sei) = (xs[1] - xs[0]).length(); - }); -} - -PBDSystem::PrimitiveHandle::PrimitiveHandle(ZenoParticles &zsprim, std::size_t &vOffset, std::size_t &sfOffset, - std::size_t &seOffset, std::size_t &svOffset, zs::wrapv<4>) - : zsprimPtr{&zsprim, [](void *) {}}, models{zsprim.getModel()}, vertsPtr{&zsprim.getParticles(), [](void *) {}}, - elesPtr{&zsprim.getQuadraturePoints(), [](void *) {}}, edgesPtr{&zsprim[ZenoParticles::s_edgeTag], [](void *) {}}, - surfTrisPtr{&zsprim[ZenoParticles::s_surfTriTag], [](void *) {}}, - surfEdgesPtr{&zsprim[ZenoParticles::s_surfEdgeTag], [](void *) {}}, - surfVertsPtr{&zsprim[ZenoParticles::s_surfVertTag], [](void *) {}}, vOffset{vOffset}, sfOffset{sfOffset}, - seOffset{seOffset}, svOffset{svOffset}, category{zsprim.category} { - if (category != ZenoParticles::tet) - throw std::runtime_error("dimension of 4 but is not tetrahedra"); - initGeo(); - vOffset += getVerts().size(); - sfOffset += getSurfTris().size(); - seOffset += getSurfEdges().size(); - svOffset += getSurfVerts().size(); -} - -void PBDSystem::initialize(zs::CudaExecutionPolicy &pol) { - using namespace zs; - constexpr auto space = execspace_e::cuda; - stInds = tiles_t{vtemp.get_allocator(), {{"inds", 3}}, sfOffset}; - seInds = tiles_t{vtemp.get_allocator(), {{"inds", 2}}, seOffset}; - svInds = tiles_t{vtemp.get_allocator(), {{"inds", 1}}, svOffset}; - for (auto &primHandle : prims) { - auto &verts = primHandle.getVerts(); - // record surface (tri) indices - if (primHandle.category != ZenoParticles::category_e::curve) { - auto &tris = primHandle.getSurfTris(); - pol(Collapse(tris.size()), - [stInds = proxy({}, stInds), tris = proxy({}, tris), voffset = primHandle.vOffset, - sfoffset = primHandle.sfOffset] __device__(int i) mutable { - stInds.template tuple<3>("inds", sfoffset + i) = - (tris.template pack<3>("inds", i).template reinterpret_bits() + (int)voffset) - .template reinterpret_bits(); - }); - } - auto &edges = primHandle.getSurfEdges(); - pol(Collapse(edges.size()), - [seInds = proxy({}, seInds), edges = proxy({}, edges), voffset = primHandle.vOffset, - seoffset = primHandle.seOffset] __device__(int i) mutable { - seInds.template tuple<2>("inds", seoffset + i) = - (edges.template pack<2>("inds", i).template reinterpret_bits() + (int)voffset) - .template reinterpret_bits(); - }); - auto &points = primHandle.getSurfVerts(); - pol(Collapse(points.size()), - [svInds = proxy({}, svInds), points = proxy({}, points), voffset = primHandle.vOffset, - svoffset = primHandle.svOffset] __device__(int i) mutable { - svInds("inds", svoffset + i) = - reinterpret_bits(reinterpret_bits(points("inds", i)) + (int)voffset); - }); - } - // init mass - pol(Collapse(numDofs), [vtemp = proxy({}, vtemp)] __device__(int i) mutable { vtemp("m", i) = 0; }); - for (auto &primHandle : prims) { - auto density = primHandle.models.density; - auto &eles = primHandle.getEles(); - pol(Collapse(eles.size()), [vtemp = proxy({}, vtemp), eles = proxy({}, eles), - vOffset = primHandle.vOffset, density] __device__(int ei) mutable { - auto m = eles("rv", ei) * density; - auto inds = eles.template pack<4>("inds", ei).template reinterpret_bits() + vOffset; - for (int d = 0; d != 4; ++d) - atomic_add(exec_cuda, &vtemp("m", inds[d]), m / 4); - }); - } - reinitialize(pol, dt); -} - -void PBDSystem::reinitialize(zs::CudaExecutionPolicy &pol, T framedt) { - using namespace zs; - constexpr auto space = execspace_e::cuda; - this->framedt = framedt; - this->dt = framedt / solveIterCap; // substep dt - for (auto &primHandle : prims) { - auto &verts = primHandle.getVerts(); - // initialize BC info - // predict pos, initialize augmented lagrangian, constrain weights - pol(Collapse(verts.size()), [vtemp = proxy({}, vtemp), verts = proxy({}, verts), - voffset = primHandle.vOffset, dt = dt] __device__(int i) mutable { - auto x = verts.pack<3>("x", i); - auto v = verts.pack<3>("v", i); - vtemp.tuple<3>("x", voffset + i) = x; - vtemp.tuple<3>("xpre", voffset + i) = x; - vtemp.tuple<3>("v", voffset + i) = v; - }); - } -} - -PBDSystem::PBDSystem(std::vector zsprims, vec3 extForce, T dt, int numSolveIters, T ec, T vc) - : extForce{extForce}, solveIterCap{numSolveIters}, edgeCompliance{ec}, volumeCompliance{vc}, prims{}, coOffset{0}, - numDofs{0}, sfOffset{0}, seOffset{0}, svOffset{0}, vtemp{}, temp{}, stInds{}, seInds{}, svInds{}, dt{dt} { - for (auto primPtr : zsprims) { - if (primPtr->category == ZenoParticles::category_e::tet) { - prims.emplace_back(*primPtr, coOffset, sfOffset, seOffset, svOffset, zs::wrapv<4>{}); - } - } - zeno::log_info("num total obj : {}, {}, {}, {}\n", coOffset, svOffset, seOffset, - sfOffset); - numDofs = coOffset; // if there are boundaries, then updated - vtemp = dtiles_t{zsprims[0]->getParticles().get_allocator(), {{"m", 1}, {"x", 3}, {"xpre", 3}, {"v", 3}}, numDofs}; - - auto cudaPol = zs::cuda_exec(); - initialize(cudaPol); -} - -struct ExtractTetEdges : INode { - void apply() override { - using namespace zs; - - auto zstets = RETRIEVE_OBJECT_PTRS(ZenoParticles, "ZSParticles"); - for (auto tet : zstets) { - using ivec2 = zs::vec; - auto comp_v2 = [](const ivec2 &x, const ivec2 &y) { - return x[0] < y[0] ? 1 : (x[0] == y[0] && x[1] < y[1] ? 1 : 0); - }; - std::set sedges(comp_v2); - std::vector lines(0); - auto ist2 = [&sedges, &lines](int i, int j) { - if (sedges.find(ivec2{i, j}) == sedges.end() && sedges.find(ivec2{j, i}) == sedges.end()) { - sedges.insert(ivec2{i, j}); - lines.push_back(ivec2{i, j}); - } - }; - const auto &elements = tet->getQuadraturePoints().clone({memsrc_e::host, -1}); - constexpr auto space = execspace_e::host; - auto eles = proxy({}, elements); - for (int ei = 0; ei != eles.size(); ++ei) { - auto inds = eles.template pack<4>("inds", ei).template reinterpret_bits(); - ist2(inds[0], inds[1]); - ist2(inds[0], inds[2]); - ist2(inds[0], inds[3]); - ist2(inds[1], inds[2]); - ist2(inds[1], inds[3]); - ist2(inds[2], inds[3]); - } - auto &edges = (*tet)[ZenoParticles::s_edgeTag]; - edges = typename ZenoParticles::particles_t{{{"inds", 2}}, lines.size(), memsrc_e::host, -1}; - auto ev = proxy({}, edges); - for (int i = 0; i != lines.size(); ++i) { - ev("inds", 0, i) = reinterpret_bits((int)lines[i][0]); - ev("inds", 1, i) = reinterpret_bits((int)lines[i][1]); - } - edges = edges.clone(tet->getParticles().get_allocator()); - } - - set_output("ZSParticles", get_input("ZSParticles")); - } -}; - -ZENDEFNODE(ExtractTetEdges, {{ - "ZSParticles", - }, - {"ZSParticles"}, - {}, - {"PBD"}}); - -struct MakePBDSystem : INode { - void apply() override { - using namespace zs; - auto zstets = RETRIEVE_OBJECT_PTRS(ZenoParticles, "ZSParticles"); - /// solver parameters - auto input_cap = get_input2("iter_cap"); - auto dt = get_input2("dt"); - auto ec = get_input2("edge_compliance"); - auto vc = get_input2("edge_compliance"); - auto extForce = get_input("ext_force")->get(); - - auto A = std::make_shared(zstets, zs::vec{extForce[0], extForce[1], extForce[2]}, dt, - input_cap, ec, vc); - - set_output("ZSPBDSystem", A); - } -}; - -ZENDEFNODE(MakePBDSystem, {{ - "ZSParticles", - {"float", "dt", "0.01"}, - {"vec3f", "ext_force", "0,-9,0"}, - {"int", "iter_cap", "100"}, - {"float", "edge_compliance", "0.001"}, - {"float", "volume_compliance", "0.001"}, - }, - {"ZSPBDSystem"}, - {}, - {"PBD"}}); - -} // namespace zeno \ No newline at end of file diff --git a/projects/CuLagrange/pbd/PBDPipeline.cu b/projects/CuLagrange/pbd/PBDPipeline.cu deleted file mode 100644 index 32e347b247..0000000000 --- a/projects/CuLagrange/pbd/PBDPipeline.cu +++ /dev/null @@ -1,152 +0,0 @@ -#include "PBD.cuh" -#include "zensim/execution/ExecutionPolicy.hpp" - -namespace zeno { - -void PBDSystem::preSolve(zs::CudaExecutionPolicy &pol) { - using namespace zs; - constexpr auto space = execspace_e::cuda; - pol(range(numDofs), [vtemp = proxy({}, vtemp), extForce = extForce, dt = dt] ZS_LAMBDA(int i) mutable { - auto x = vtemp.template pack<3>("x", i); - auto v = vtemp.template pack<3>("v", i); - auto xpre = x; - v += extForce * dt; //extForce here is actually accel - x += v * dt; - // project - if (x[1] < 0) { - x = xpre; - x[1] = 0; - } - vtemp.template tuple<3>("xpre", i) = xpre; - vtemp.template tuple<3>("x", i) = x; - vtemp.template tuple<3>("v", i) = v; - }); -} - -void PBDSystem::solveEdge(zs::CudaExecutionPolicy &pol) { - using namespace zs; - constexpr auto space = execspace_e::cuda; - T alpha = edgeCompliance / dt / dt; - zeno::vec3f grads{0, 0, 0}; - for (auto &&prim : prims) { - auto &edges = prim.getEdges(); - pol(range(edges.size()), [vtemp = proxy({}, vtemp), edges = proxy({}, edges), - vOffset = prim.vOffset, alpha] ZS_LAMBDA(int ei) mutable { - auto id = edges.template pack<2>("inds", ei).template reinterpret_bits() + vOffset; - T ms[2]; - ms[0] = 1 / vtemp("m", id[0]); - ms[1] = 1 / vtemp("m", id[1]); - vec3 grads; - auto x0 = vtemp.template pack<3>("x", id[0]); - auto x1 = vtemp.template pack<3>("x", id[1]); - grads = x0 - x1; - auto len = grads.length(); - grads /= len; - T C = len - edges("rl", ei); - T w = ms[0] + ms[1]; - T s = -C / (w + alpha); - - for (int d = 0; d != 3; ++d) { - atomic_add(exec_cuda, &vtemp("x", d, id[0]), grads[d] * s * ms[0]); - atomic_add(exec_cuda, &vtemp("x", d, id[1]), grads[d] * -s * ms[1]); - } - }); - } -} -void PBDSystem::solveVolume(zs::CudaExecutionPolicy &pol) { - using namespace zs; - constexpr auto space = execspace_e::cuda; - float alphaVol = volumeCompliance / dt / dt; - - for (auto &&prim : prims) { - auto &eles = prim.getEles(); - pol(range(eles.size()), [vtemp = proxy({}, vtemp), eles = proxy({}, eles), vOffset = prim.vOffset, - alphaVol] ZS_LAMBDA(int ei) mutable { - auto id = eles.template pack<4>("inds", ei).template reinterpret_bits() + vOffset; - vec3 gradsVol[4]; - T ms[4]; - auto x0 = vtemp.template pack<3>("x", id[0]); - auto x1 = vtemp.template pack<3>("x", id[1]); - auto x2 = vtemp.template pack<3>("x", id[2]); - auto x3 = vtemp.template pack<3>("x", id[3]); - ms[0] = 1 / vtemp("m", id[0]); - ms[1] = 1 / vtemp("m", id[1]); - ms[2] = 1 / vtemp("m", id[2]); - ms[3] = 1 / vtemp("m", id[3]); - - // ref: https://github.com/InteractiveComputerGraphics/PositionBasedDynamics - // XPBD.cpp:solve_VolumeConstraint - gradsVol[0] = (x1 - x2).cross(x3 - x2); - gradsVol[1] = (x2 - x0).cross(x3 - x0); - gradsVol[2] = (x0 - x1).cross(x3 - x1); - gradsVol[3] = (x1 - x0).cross(x2 - x0); - - T w = 0; - for (int j = 0; j != 4; ++j) - w += gradsVol[j].l2NormSqr() * ms[j]; - - T vol = zs::abs((x1 - x0).cross(x2 - x0).dot(x3 - x0)) / 6; - T C = (vol - eles("rv", ei)) * 6; - T s = -C / (w + alphaVol); - - for (int j = 0; j != 4; ++j) - for (int d = 0; d != 3; ++d) - atomic_add(exec_cuda, &vtemp("x", d, id[j]), gradsVol[j][d] * s * ms[j]); - }); - } -} - -void PBDSystem::postSolve(zs::CudaExecutionPolicy &pol) { - using namespace zs; - constexpr auto space = execspace_e::cuda; - pol(range(numDofs), [vtemp = proxy({}, vtemp), dt = dt] ZS_LAMBDA(int i) mutable { - auto x = vtemp.template pack<3>("x", i); - auto xpre = vtemp.template pack<3>("xpre", i); - auto v = (x - xpre) / dt; - vtemp.template tuple<3>("v", i) = v; - }); -} - -void PBDSystem::writebackPositionsAndVelocities(zs::CudaExecutionPolicy &pol) { - using namespace zs; - constexpr auto space = execspace_e::cuda; - for (auto &primHandle : prims) { - auto &verts = primHandle.getVerts(); - // update velocity and positions - pol(zs::range(verts.size()), [vtemp = proxy({}, vtemp), verts = proxy({}, verts), dt = dt, - vOffset = primHandle.vOffset] __device__(int vi) mutable { - verts.tuple<3>("x", vi) = vtemp.pack<3>("x", vOffset + vi); - verts.tuple<3>("v", vi) = vtemp.pack<3>("v", vOffset + vi); - }); - } -} - -struct StepPBDSystem : INode { - void apply() override { - using namespace zs; - auto A = get_input("ZSPBDSystem"); - auto dt = get_input2("dt"); - - auto cudaPol = cuda_exec(); - A->reinitialize(cudaPol, dt); - for (int steps = 0; steps != A->solveIterCap; ++steps) { - A->preSolve(cudaPol); - A->solveEdge(cudaPol); - A->solveVolume(cudaPol); - A->postSolve(cudaPol); - } - A->writebackPositionsAndVelocities(cudaPol); - - set_output("ZSPBDSystem", A); - } -}; - -ZENDEFNODE(StepPBDSystem, {{ - "ZSPBDSystem", - {"float", "dt", "0.01"}, - }, - {"ZSPBDSystem"}, - {}, - {"PBD"}}); - -} // namespace zeno \ No newline at end of file diff --git a/projects/CuLagrange/pbd/Pipeline.cu b/projects/CuLagrange/pbd/Pipeline.cu deleted file mode 100644 index 20b230a7bd..0000000000 --- a/projects/CuLagrange/pbd/Pipeline.cu +++ /dev/null @@ -1,2762 +0,0 @@ -#include "Structures.hpp" -// #include "../fem/SolverUtils.cuh" -#include "zensim/Logger.hpp" -#include "zensim/cuda/execution/ExecutionPolicy.cuh" -#include "zensim/omp/execution/ExecutionPolicy.hpp" -#include "zensim/io/MeshIO.hpp" -#include "zensim/math/bit/Bits.h" -#include "zensim/types/Property.h" -#include -#include -#include -#include -#include -#include - -#include "constraint_function_kernel/constraint.cuh" -#include "../geometry/kernel/tiled_vector_ops.hpp" -#include "../geometry/kernel/topology.hpp" -#include "../geometry/kernel/geo_math.hpp" -#include "../fem/collision_energy/evaluate_collision.hpp" -#include "constraint_function_kernel/constraint_types.hpp" - -namespace zeno { - -// we only need to record the topo here -// serve triangulate mesh or strands only currently -struct MakeSurfaceConstraintTopology : INode { - - template - void buildBvh(zs::CudaExecutionPolicy &pol, - TileVecT &verts, - const zs::SmallString& srcTag, - const zs::SmallString& dstTag, - const zs::SmallString& pscaleTag, - ZenoLinearBvh::lbvh_t &bvh) { - using namespace zs; - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto space = execspace_e::cuda; - Vector bvs{verts.get_allocator(), verts.size()}; - pol(range(verts.size()), - [verts = proxy({}, verts), - bvs = proxy(bvs), - pscaleTag,srcTag,dstTag] ZS_LAMBDA(int vi) mutable { - auto src = verts.template pack<3>(srcTag, vi); - auto dst = verts.template pack<3>(dstTag, vi); - auto pscale = verts(pscaleTag,vi); - - bv_t bv{src,dst}; - bv._min -= pscale; - bv._max += pscale; - bvs[vi] = bv; - }); - bvh.build(pol, bvs); - } - - virtual void apply() override { - using namespace zs; - using namespace PBD_CONSTRAINT; - - using vec3 = zs::vec; - using vec2i = zs::vec; - using vec3i = zs::vec; - using vec4i = zs::vec; - using mat4 = zs::vec; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = zs::cuda_exec(); - - auto source = get_input("source"); - auto constraint = std::make_shared(); - - auto type = get_input2("topo_type"); - - if(source->category != ZenoParticles::surface) - throw std::runtime_error("Try adding Constraint topology to non-surface ZenoParticles"); - - const auto& verts = source->getParticles(); - const auto& quads = source->getQuadraturePoints(); - - auto uniform_stiffness = get_input2("stiffness"); - - zs::Vector colors{quads.get_allocator(),0}; - zs::Vector reordered_map{quads.get_allocator(),0}; - zs::Vector color_offset{quads.get_allocator(),0}; - - constraint->sprayedOffset = 0; - constraint->elements = typename ZenoParticles::particles_t({{"stiffness",1},{"lambda",1},{"tclr",1}}, 0, zs::memsrc_e::device,0); - auto &eles = constraint->getQuadraturePoints(); - - if(type == "stretch") { - constraint->setMeta(CONSTRAINT_KEY,category_c::edge_length_constraint); - auto quads_vec = tilevec_topo_to_zsvec_topo(cudaPol,quads,wrapv<3>{}); - zs::Vector> edge_topos{quads.get_allocator(),0}; - retrieve_edges_topology(cudaPol,quads_vec,edge_topos); - eles.resize(edge_topos.size()); - - topological_coloring(cudaPol,edge_topos,colors); - sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); - // std::cout << "quads.size() = " << quads.size() << "\t" << "edge_topos.size() = " << edge_topos.size() << std::endl; - eles.append_channels(cudaPol,{{"inds",2},{"r",1}}); - - auto rest_scale = get_input2("rest_scale"); - - cudaPol(zs::range(eles.size()),[ - verts = proxy({},verts), - eles = proxy({},eles), - reordered_map = proxy(reordered_map), - uniform_stiffness = uniform_stiffness, - colors = proxy(colors), - rest_scale = rest_scale, - edge_topos = proxy(edge_topos)] ZS_LAMBDA(int oei) mutable { - auto ei = reordered_map[oei]; - eles.tuple(dim_c<2>,"inds",oei) = edge_topos[ei].reinterpret_bits(float_c); - vec3 x[2] = {}; - for(int i = 0;i != 2;++i) - x[i] = verts.pack(dim_c<3>,"x",edge_topos[ei][i]); - eles("r",oei) = (x[0] - x[1]).norm() * rest_scale; - }); - - } - - if(type == "bending") { - constraint->setMeta(CONSTRAINT_KEY,category_c::isometric_bending_constraint); - // constraint->category = ZenoParticles::tri_bending_spring; - // constraint->sprayedOffset = 0; - - const auto& halfedges = (*source)[ZenoParticles::s_surfHalfEdgeTag]; - - zs::Vector> bd_topos{quads.get_allocator(),0}; - retrieve_tri_bending_topology(cudaPol,quads,halfedges,bd_topos); - - eles.resize(bd_topos.size()); - - topological_coloring(cudaPol,bd_topos,colors); - sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); - // std::cout << "quads.size() = " << quads.size() << "\t" << "edge_topos.size() = " << edge_topos.size() << std::endl; - - eles.append_channels(cudaPol,{{"inds",4},{"Q",4 * 4}}); - - // std::cout << "halfedges.size() = " << halfedges.size() << "\t" << "bd_topos.size() = " << bd_topos.size() << std::endl; - - cudaPol(zs::range(eles.size()),[ - eles = proxy({},eles), - bd_topos = proxy(bd_topos), - reordered_map = proxy(reordered_map), - verts = proxy({},verts)] ZS_LAMBDA(int oei) mutable { - auto ei = reordered_map[oei]; - // printf("bd_topos[%d] : %d %d %d %d\n",ei,bd_topos[ei][0],bd_topos[ei][1],bd_topos[ei][2],bd_topos[ei][3]); - eles.tuple(dim_c<4>,"inds",oei) = bd_topos[ei].reinterpret_bits(float_c); - vec3 x[4] = {}; - for(int i = 0;i != 4;++i) - x[i] = verts.pack(dim_c<3>,"x",bd_topos[ei][i]); - - mat4 Q = mat4::uniform(0); - CONSTRAINT::init_IsometricBendingConstraint(x[0],x[1],x[2],x[3],Q); - eles.tuple(dim_c<16>,"Q",oei) = Q; - }); - } - if(type == "kcollision") { - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - - constraint->setMeta(CONSTRAINT_KEY,category_c::p_kp_collision_constraint); - auto target = get_input("target"); - - const auto& kverts = target->getParticles(); - ZenoLinearBvh::lbvh_t kbvh{}; - buildBvh(cudaPol,kverts,"px","x","pscale",kbvh); - - zs::bht csPP{verts.get_allocator(),verts.size()}; - csPP.reset(cudaPol,true); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - kverts = proxy({},kverts), - kbvh = proxy(kbvh), - csPP = proxy(csPP)] ZS_LAMBDA(int vi) mutable { - auto x = verts.pack(dim_c<3>,"x",vi); - auto px = verts.pack(dim_c<3>,"px",vi); - auto mx = (x + px) / (T)2.0; - auto pscale = verts("pscale",vi); - - auto radius = (mx - px).norm() + pscale * (T)2.0; - auto bv = bv_t{mx - radius,mx + radius}; - - int contact_kvi = -1; - T min_contact_time = std::numeric_limits::max(); - - auto process_ccd_collision = [&](int kvi) { - auto kpscale = kverts("pscale",kvi); - auto kx = kverts.pack(dim_c<3>,"x",kvi); - auto pkx = kx; - if(kverts.hasProperty("px")) - pkx = kverts.pack(dim_c<3>,"px",kvi); - - auto t = LSL_GEO::ray_ray_intersect(px,x - px,pkx,kx - pkx,(pscale + kpscale) * (T)2); - if(t < min_contact_time) { - min_contact_time = t; - contact_kvi = kvi; - } - }; - kbvh.iter_neighbors(bv,process_ccd_collision); - - if(contact_kvi >= 0) { - csPP.insert(vec2i{vi,contact_kvi}); - } - }); - - eles.resize(csPP.size()); - colors.resize(csPP.size()); - reordered_map.resize(csPP.size()); - - eles.append_channels(cudaPol,{{"inds",2}}); - cudaPol(zip(zs::range(csPP.size()),csPP._activeKeys),[ - eles = proxy({},eles), - colors = proxy(colors), - reordered_map = proxy(reordered_map)] ZS_LAMBDA(auto ei,const auto& pair) mutable { - eles.tuple(dim_c<2>,"inds",ei) = pair.reinterpret_bits(float_c); - colors[ei] = (T)0; - reordered_map[ei] = ei; - }); - - color_offset.resize(1); - color_offset.setVal(0); - } - - if(type == "attachment") { - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - - } - - cudaPol(zs::range(eles.size()),[ - eles = proxy({},eles), - uniform_stiffness = uniform_stiffness, - colors = proxy(colors), - // exec_tag, - reordered_map = proxy(reordered_map)] ZS_LAMBDA(int oei) mutable { - auto ei = reordered_map[oei]; - eles("lambda",oei) = 0.0; - eles("stiffness",oei) = uniform_stiffness; - eles("tclr",oei) = colors[ei]; - // auto - }); - - constraint->setMeta("color_offset",color_offset); - - // set_output("source",source); - set_output("constraint",constraint); - }; -}; - -ZENDEFNODE(MakeSurfaceConstraintTopology, {{ - {"source"}, - {"target"}, - {"float","stiffness","0.5"}, - {"string","topo_type","stretch"}, - {"float","rest_scale","1.0"} - }, - {{"constraint"}}, - { - // {"string","groupID",""}, - }, - {"PBD"}}); - - -struct VisualizePBDConstraint : INode { - using T = float; - using vec3 = zs::vec; - // using tiles_t = typename ZenoParticles::particles_t; - // using dtiles_t = zs::TileVector; - - virtual void apply() override { - using namespace zs; - using namespace PBD_CONSTRAINT; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - - auto zsparticles = get_input("zsparticles"); - auto constraints_ptr = get_input("constraints"); - - const auto& geo_verts = zsparticles->getParticles(); - const auto& constraints = constraints_ptr->getQuadraturePoints(); - - auto tclr_tag = get_param("tclrTag"); - - zs::Vector cvis{geo_verts.get_allocator(),constraints.getChannelSize("inds") * constraints.size()}; - zs::Vector cclrs{constraints.get_allocator(),constraints.size()}; - int cdim = constraints.getChannelSize("inds"); - cudaPol(zs::range(constraints.size()),[ - constraints = proxy({},constraints), - geo_verts = proxy({},geo_verts), - cclrs = proxy(cclrs), - tclr_tag = zs::SmallString(tclr_tag), - cdim = cdim, - cvis = proxy(cvis)] ZS_LAMBDA(int ci) mutable { - // auto cdim = constraints.propertySize("inds"); - for(int i = 0;i != cdim;++i) { - auto vi = zs::reinterpret_bits(constraints("inds",i,ci)); - cvis[ci * cdim + i] = geo_verts.pack(dim_c<3>,"x",vi); - } - cclrs[ci] = (int)constraints(tclr_tag,ci); - }); - - constexpr auto omp_space = execspace_e::openmp; - auto ompPol = omp_exec(); - - cvis = cvis.clone({zs::memsrc_e::host}); - cclrs = cclrs.clone({zs::memsrc_e::host}); - auto prim = std::make_shared(); - auto& pverts = prim->verts; - - auto constraint_type = constraints_ptr->readMeta(CONSTRAINT_KEY,wrapt{}); - - if(constraint_type == category_c::edge_length_constraint) { - pverts.resize(constraints.size() * 2); - auto& plines = prim->lines; - plines.resize(constraints.size()); - auto& tclrs = pverts.add_attr(tclr_tag); - auto& ltclrs = plines.add_attr(tclr_tag); - - ompPol(zs::range(constraints.size()),[ - <clrs,&pverts,&plines,&tclrs,cvis = proxy(cvis),cclrs = proxy(cclrs)] (int ci) mutable { - pverts[ci * 2 + 0] = cvis[ci * 2 + 0].to_array(); - pverts[ci * 2 + 1] = cvis[ci * 2 + 1].to_array(); - tclrs[ci * 2 + 0] = cclrs[ci]; - tclrs[ci * 2 + 1] = cclrs[ci]; - plines[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; - ltclrs[ci] = cclrs[ci]; - }); - }else if(constraint_type == category_c::isometric_bending_constraint) { - pverts.resize(constraints.size() * 2); - auto& plines = prim->lines; - plines.resize(constraints.size()); - auto& tclrs = pverts.add_attr(tclr_tag); - auto& ltclrs = plines.add_attr(tclr_tag); - - ompPol(zs::range(constraints.size()),[ - <clrs,&pverts,&plines,&tclrs,cvis = proxy(cvis),cclrs = proxy(cclrs)] (int ci) mutable { - zeno::vec3f cverts[4] = {}; - for(int i = 0;i != 4;++i) - cverts[i] = cvis[ci * 4 + i].to_array(); - - pverts[ci * 2 + 0] = (cverts[0] + cverts[2] + cverts[3]) / (T)3.0; - pverts[ci * 2 + 1] = (cverts[1] + cverts[2] + cverts[3]) / (T)3.0; - tclrs[ci * 2 + 0] = cclrs[ci]; - tclrs[ci * 2 + 1] = cclrs[ci]; - ltclrs[ci] = cclrs[ci]; - - plines[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; - }); - } - - set_output("prim",std::move(prim)); - } -}; - -ZENDEFNODE(VisualizePBDConstraint, {{{"zsparticles"},{"constraints"}}, - {{"prim"}}, - { - {"string","tclrTag","tclrTag"}, - }, - {"PBD"}}); - -// solve a specific type of constraint for one iterations -struct XPBDSolve : INode { - - virtual void apply() override { - using namespace zs; - using namespace PBD_CONSTRAINT; - - using vec3 = zs::vec; - using vec2i = zs::vec; - using vec3i = zs::vec; - using vec4i = zs::vec; - using mat4 = zs::vec; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - constexpr auto exec_tag = wrapv{}; - - auto zsparticles = get_input("zsparticles"); - auto constraints = get_input("constraints"); - - auto dt = get_input2("dt"); - auto ptag = get_param("ptag"); - - auto coffsets = constraints->readMeta("color_offset",zs::wrapt>{}); - int nm_group = coffsets.size(); - - auto& verts = zsparticles->getParticles(); - auto& cquads = constraints->getQuadraturePoints(); - auto category = constraints->readMeta(CONSTRAINT_KEY,wrapt{}); - auto target = get_input("target"); - const auto& kverts = target->getParticles(); - // zs::Vector pv_buffer{verts.get_allocator(),verts.size()}; - // zs::Vector total_ghost_impulse_X{verts.get_allocator(),1}; - // zs::Vector total_ghost_impulse_Y{verts.get_allocator(),1}; - // zs::Vector total_ghost_impulse_Z{verts.get_allocator(),1}; - - // std::cout << "SOVLE CONSTRAINT WITH GROUP : " << nm_group << "\t" << cquads.size() << std::endl; - - - - for(int g = 0;g != nm_group;++g) { - - // if(category == category_c::isometric_bending_constraint) - // break; - - auto coffset = coffsets.getVal(g); - int group_size = 0; - if(g == nm_group - 1) - group_size = cquads.size() - coffsets.getVal(g); - else - group_size = coffsets.getVal(g + 1) - coffsets.getVal(g); - - // cudaPol(zs::range(verts.size()),[ - // ptag = zs::SmallString(ptag), - // verts = proxy({},verts), - // pv_buffer = proxy(pv_buffer)] ZS_LAMBDA(int vi) mutable { - // pv_buffer[vi] = verts.pack(dim_c<3>,ptag,vi); - // }); - - - - cudaPol(zs::range(group_size),[ - coffset, - verts = proxy({},verts), - category, - dt, - ptag = zs::SmallString(ptag), - kverts = proxy({},kverts), - cquads = proxy({},cquads)] ZS_LAMBDA(int gi) mutable { - float s = cquads("stiffness",coffset + gi); - float lambda = cquads("lambda",coffset + gi); - - if(category == category_c::edge_length_constraint) { - auto edge = cquads.pack(dim_c<2>,"inds",coffset + gi,int_c); - vec3 p0{},p1{}; - p0 = verts.pack(dim_c<3>,ptag,edge[0]); - p1 = verts.pack(dim_c<3>,ptag,edge[1]); - float minv0 = verts("minv",edge[0]); - float minv1 = verts("minv",edge[1]); - float r = cquads("r",coffset + gi); - - vec3 dp0{},dp1{}; - CONSTRAINT::solve_DistanceConstraint( - p0,minv0, - p1,minv1, - r, - s, - dt, - lambda, - dp0,dp1); - - - verts.tuple(dim_c<3>,ptag,edge[0]) = p0 + dp0; - verts.tuple(dim_c<3>,ptag,edge[1]) = p1 + dp1; - - // float m0 = verts("m",edge[0]); - // float m1 = verts("m",edge[1]); - // auto ghost_impulse = (dp0 * m0 + dp1 * m1).norm(); - // if(ghost_impulse > 1e-6) - // printf("dmomentum : %f\n",(float)ghost_impulse); - } - if(category == category_c::isometric_bending_constraint) { - auto quad = cquads.pack(dim_c<4>,"inds",coffset + gi,int_c); - vec3 p[4] = {}; - float minv[4] = {}; - for(int i = 0;i != 4;++i) { - p[i] = verts.pack(dim_c<3>,ptag,quad[i]); - minv[i] = verts("minv",quad[i]); - } - - auto Q = cquads.pack(dim_c<4,4>,"Q",coffset + gi); - - vec3 dp[4] = {}; - CONSTRAINT::solve_IsometricBendingConstraint( - p[0],minv[0], - p[1],minv[1], - p[2],minv[2], - p[3],minv[3], - Q, - s, - dt, - lambda, - dp[0],dp[1],dp[2],dp[3]); - - for(int i = 0;i != 4;++i) { - // printf("dp[%d][%d] : %f %f %f %f\n",gi,i,s,(float)dp[i][0],(float)dp[i][1],(float)dp[i][2]); - verts.tuple(dim_c<3>,ptag,quad[i]) = p[i] + dp[i]; - } - - - } - - if(category == category_c::p_kp_collision_constraint) { - auto quad = cquads.pack(dim_c<2>,"inds",coffset + gi,int_c); - - vec3 p = verts.pack(dim_c<3>,ptag,quad[0]); - vec3 kp = kverts.pack(dim_c<3>,"x",quad[1]); - vec3 knrm = kverts.pack(dim_c<3>,"nrm",quad[1]); - - auto pscale = verts("pscale",quad[0]); - auto kpscale = kverts("pscale",quad[1]); - - T minv = verts("minv",quad[0]); - // T kminv = kverts("minv",quad[1]); - - vec3 dp = {}; - auto thickness = pscale + kpscale; - thickness *= (float)1.01; - CONSTRAINT::solve_PlaneConstraint(p,minv,kp,knrm,thickness,s,dt,lambda,dp); - verts.tuple(dim_c<3>,ptag,quad[0]) = p + dp; - } - - cquads("lambda",coffset + gi) = lambda; - }); - - // total_ghost_impulse_X.setVal(0); - // total_ghost_impulse_Y.setVal(0); - // total_ghost_impulse_Z.setVal(0); - // if(category == category_c::isometric_bending_constraint) - // std::cout << "isometric_bending_constraint output" << std::endl; - // if(category == category_c::edge_length_constraint) - // std::cout << "edge_length_constraint output" << std::endl; - - // cudaPol(zs::range(verts.size()),[ - // verts = proxy({},verts), - // ptag = zs::SmallString(ptag) - // // exec_tag, - // // total_ghost_impulse_X = proxy(total_ghost_impulse_X), - // // total_ghost_impulse_Y = proxy(total_ghost_impulse_Y), - // // total_ghost_impulse_Z = proxy(total_ghost_impulse_Z), - // // pv_buffer = proxy(pv_buffer) - // ] ZS_LAMBDA(int vi) mutable { - // auto cv = verts.pack(dim_c<3>,ptag,vi); - // // auto pv = pv_buffer[vi]; - // // auto m = verts("m",vi); - // // auto dv = m * (cv - pv); - // // for(int i = 0;i != 3;++i) - // // atomic_add(exec_tag,&total_ghost_impulse_X[0],dv[0]); - // // atomic_add(exec_tag,&total_ghost_impulse_Y[0],dv[1]); - // // atomic_add(exec_tag,&total_ghost_impulse_Z[0],dv[2]); - // printf("cv[%d] : %f %f %f\n",vi,(float)cv[0],(float)cv[1],(float)cv[2]); - // }); - - // auto tgi = total_ghost_impulse.getVal(0); - // auto tgx = total_ghost_impulse_X.getVal(0); - // auto tgy = total_ghost_impulse_Y.getVal(0); - // auto tgz = total_ghost_impulse_Z.getVal(0); - // printf("ghost_impulse[%d][%d] : %f %f %f\n",(int)coffset,(int)group_size,(float)tgx,(float)tgy,(float)tgz); - } - - // cudaPol(zs::range(verts.size())) - - set_output("constraints",constraints); - set_output("zsparticles",zsparticles); - set_output("target",target); - }; -}; - -ZENDEFNODE(XPBDSolve, {{{"zsparticles"},{"constraints"},{"target"},{"float","dt","0.5"}}, - {{"zsparticles"},{"constraints"},{"target"}}, - {{"string","ptag","X"}}, - {"PBD"}}); - -struct DetangleImminentCollision : INode { - using T = float; - using vec3 = zs::vec; - using dtiles_t = zs::TileVector; - - virtual void apply() override { - using namespace zs; - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - // constexpr auto exec_tag = wrapv{}; - constexpr auto eps = (T)1e-7; - - auto zsparticles = get_input("zsparticles"); - auto current_x_tag = get_input2("current_x_tag"); - auto pre_x_tag = get_input2("previous_x_tag"); - auto repel_strength = get_input2("repeling_strength"); - auto imminent_collision_thickness = get_input2("immc_thickness"); - // apply impulse for imminent collision for previous configuration - auto& verts = zsparticles->getParticles(); - const auto& tris = zsparticles->getQuadraturePoints(); - const auto& halfedges = (*zsparticles)[ZenoParticles::s_surfHalfEdgeTag]; - const auto &edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; - - if(!verts.hasProperty("imminent_fail")) - verts.append_channels(cudaPol,{{"imminent_fail",1}}); - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts)] ZS_LAMBDA(int vi) mutable {verts("imminent_fail",vi) = (T)0;}); - - dtiles_t vtemp{verts.get_allocator(),{ - {"x",3}, - {"v",3}, - {"minv",1} - },verts.size()}; - TILEVEC_OPS::copy(cudaPol,verts,pre_x_tag,vtemp,"x"); - TILEVEC_OPS::copy(cudaPol,verts,"minv",vtemp,"minv"); - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - vtemp = proxy({},vtemp), - current_x_tag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { - vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - vtemp.pack(dim_c<3>,"x",vi); - }); - // TILEVEC_OPS::copy(cudaPol,verts,current_x_tag,vtemp,"v"); - // TILEVEC_OPS::add(cudaPol,vtemp,"v",1,"x",-1,"v"); - dtiles_t imminent_collision_buffer(verts.get_allocator(), - { - {"inds",4}, - {"bary",4}, - {"impulse",3}, - {"collision_normal",3} - },(size_t)0); - - auto imminent_restitution_rate = get_input2("imm_restitution"); - auto imminent_relaxation_rate = get_input2("imm_relaxation"); - - auto nm_iters = get_input2("nm_imminent_iters"); - - auto do_pt_detection = get_input2("use_PT"); - auto do_ee_detection = get_input2("use_EE"); - - zs::Vector nm_imminent_collision{verts.get_allocator(),(size_t)1}; - - std::cout << "do imminent detangle" << std::endl; - for(int it = 0;it != nm_iters;++it) { - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts)] ZS_LAMBDA(int vi) mutable {verts("imminent_fail",vi) = (T)0;}); - - // we use collision cell as the collision volume, PT collision is enough prevent penertation? - if(do_pt_detection) { - COLLISION_UTILS::calc_imminent_self_PT_collision_impulse(cudaPol, - vtemp,"x","v", - tris, - halfedges, - imminent_collision_thickness, - 0, - imminent_collision_buffer); - // std::cout << "nm_imminent_PT_collision : " << imminent_collision_buffer.size() << std::endl; - } - - if(do_ee_detection) { - COLLISION_UTILS::calc_imminent_self_EE_collision_impulse(cudaPol, - vtemp,"x","v", - edges, - imminent_collision_thickness, - imminent_collision_buffer.size(), - imminent_collision_buffer); - // std::cout << "nm_imminent_EE_collision : " << imminent_collision_buffer.size() << std::endl; - } - // resolve imminent PT collision - - // impulse_norm = TILEVEC_OPS::dot<3>(cudaPol,imminent_collision_buffer,"impulse","impulse"); - // std::cout << "EE_PT_impulse_norm : " << impulse_norm << std::endl; - - COLLISION_UTILS::apply_impulse(cudaPol, - vtemp,"v", - imminent_restitution_rate, - imminent_relaxation_rate, - imminent_collision_buffer); - - - auto add_repulsion_force = get_input2("add_repulsion_force"); - // add an impulse to repel the pair further - - nm_imminent_collision.setVal(0); - // if(add_repulsion_force) { - // std::cout << "add imminent replering force" << std::endl; - auto max_repel_distance = get_input2("max_repel_distance"); - - cudaPol(zs::range(imminent_collision_buffer.size()),[ - imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(int ci) mutable { - imminent_collision_buffer.tuple(dim_c<3>,"impulse",ci) = vec3::zeros(); - }); - - cudaPol(zs::range(imminent_collision_buffer.size()),[ - verts = proxy({},verts), - vtemp = proxy({},vtemp), - eps = eps, - exec_tag = wrapv{}, - k = repel_strength, - max_repel_distance = max_repel_distance, - thickness = imminent_collision_thickness, - nm_imminent_collision = proxy(nm_imminent_collision), - imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(auto id) mutable { - auto inds = imminent_collision_buffer.pack(dim_c<4>,"inds",id,int_c); - auto bary = imminent_collision_buffer.pack(dim_c<4>,"bary",id); - - vec3 ps[4] = {}; - vec3 vs[4] = {}; - auto vr = vec3::zeros(); - auto pr = vec3::zeros(); - for(int i = 0;i != 4;++i) { - ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); - vs[i] = vtemp.pack(dim_c<3>,"v",inds[i]); - pr += bary[i] * ps[i]; - vr += bary[i] * vs[i]; - } - - auto dist = pr.norm(); - vec3 collision_normal = imminent_collision_buffer.pack(dim_c<3>,"collision_normal",id); - - if(dist > thickness) - return; - - auto d = thickness - dist; - auto vn = vr.dot(collision_normal); - if(vn < -thickness * 0.05) { - atomic_add(exec_tag,&nm_imminent_collision[0],1); - for(int i = 0;i != 4;++i) - verts("imminent_fail",inds[i]) = (T)1.0; - } - if(vn > (T)max_repel_distance * d || d < 0) { - // if with current velocity, the collided particles can be repeled by more than 1% of collision depth, no extra repulsion is needed - return; - } else { - // make sure the collided particles is seperated by 1% of collision depth - // assume the particles has the same velocity - auto I = k * d; - auto I_max = (max_repel_distance * d - vn); - I = I_max < I ? I_max : I; - auto impulse = (T)I * collision_normal; - - imminent_collision_buffer.tuple(dim_c<3>,"impulse",id) = impulse; - } - }); - std::cout << "nm_imminent_collision : " << nm_imminent_collision.getVal(0) << std::endl; - if(nm_imminent_collision.getVal(0) == 0) - break; - - - // auto impulse_norm = TILEVEC_OPS::dot<3>(cudaPol,imminent_collision_buffer,"impulse","impulse"); - // std::cout << "REPEL_impulse_norm : " << impulse_norm << std::endl; - - COLLISION_UTILS::apply_impulse(cudaPol, - vtemp,"v", - imminent_restitution_rate, - imminent_relaxation_rate, - imminent_collision_buffer); - // } - } - - std::cout << "finish imminent collision" << std::endl; - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - vtemp = proxy({},vtemp), - current_x_tag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { - verts.tuple(dim_c<3>,current_x_tag,vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); - }); - - // std::cout << "finish apply imminent impulse" << std::endl; - - set_output("zsparticles",zsparticles); - } -}; - -ZENDEFNODE(DetangleImminentCollision, {{{"zsparticles"}, - {"string","current_x_tag","x"}, - {"string","previous_x_tag","px"}, - // {"string","pscaleTag","pscale"}, - {"float","repeling_strength","1.0"}, - {"float","immc_thickness","0.01"}, - {"int","nm_imminent_iters","1"}, - {"float","imm_restitution","0.1"}, - {"float","imm_relaxation","0.25"}, - {"float","max_repel_distance","0.1"}, - {"bool","add_repulsion_force","0"}, - {"bool","use_PT","1"}, - {"bool","use_EE","1"}, - }, - {{"zsparticles"}}, - {}, - {"PBD"}}); - - -struct VisualizeImminentCollision : INode { - using T = float; - using vec3 = zs::vec; - using dtiles_t = zs::TileVector; - - virtual void apply() override { - using namespace zs; - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - auto ompPol = omp_exec(); - constexpr auto omp_space = execspace_e::openmp; - - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto exec_tag = wrapv{}; - constexpr auto eps = (T)1e-7; - constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; - - auto zsparticles = get_input("zsparticles"); - auto current_x_tag = get_input2("current_x_tag"); - auto pre_x_tag = get_input2("previous_x_tag"); - auto imminent_collision_thickness = get_input2("immc_thickness"); - - // apply impulse for imminent collision for previous configuration - auto& verts = zsparticles->getParticles(); - const auto& tris = zsparticles->getQuadraturePoints(); - const auto& edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; - const auto& halfedges = (*zsparticles)[ZenoParticles::s_surfHalfEdgeTag]; - - // auto nm_imminent_iters = get_input2("nm_imminent_iters"); - dtiles_t imminent_PT_collision_buffer(verts.get_allocator(), - { - {"inds",4}, - {"bary",4}, - {"impulse",3}, - {"collision_normal",3} - },(size_t)0); - - dtiles_t imminent_EE_collision_buffer(verts.get_allocator(), - { - {"inds",4}, - {"bary",4}, - {"impulse",3}, - {"collision_normal",3} - },(size_t)0); - // zs::bht csPT{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; - // csPT.reset(cudaPol,true); - - dtiles_t vtemp(verts.get_allocator(),{ - {"x",3}, - {"v",3}, - {"minv",1} - },verts.size()); - TILEVEC_OPS::copy(cudaPol,verts,pre_x_tag,vtemp,"x"); - TILEVEC_OPS::copy(cudaPol,verts,"minv",vtemp,"minv"); - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - vtemp = proxy({},vtemp), - current_x_tag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { - vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - vtemp.pack(dim_c<3>,"x",vi); - }); - // we use collision cell as the collision volume, PT collision is enough prevent penertation? - COLLISION_UTILS::calc_imminent_self_PT_collision_impulse(cudaPol, - vtemp,"x","v", - tris, - halfedges, - imminent_collision_thickness, - 0, - imminent_PT_collision_buffer); - std::cout << "nm_PT_collision : " << imminent_PT_collision_buffer.size() << std::endl; - - COLLISION_UTILS::calc_imminent_self_EE_collision_impulse(cudaPol, - vtemp,"x","v", - edges, - imminent_collision_thickness, - 0, - imminent_EE_collision_buffer); - std::cout << "nm_EE_collision : " << imminent_EE_collision_buffer.size() << std::endl; - // resolve imminent PT collision - - dtiles_t imminent_PT_collision_vis(verts.get_allocator(),{ - {"collision_normal",3}, - {"pt",3}, - {"pp",3}},imminent_PT_collision_buffer.size()); - - cudaPol(zs::range(imminent_PT_collision_buffer.size()),[ - vtemp = proxy({},vtemp), - imminent_PT_collision_vis = proxy({},imminent_PT_collision_vis), - imminent_PT_collision_buffer = proxy({},imminent_PT_collision_buffer)] ZS_LAMBDA(auto ci) mutable { - auto inds = imminent_PT_collision_buffer.pack(dim_c<4>,"inds",ci,int_c); - auto bary = imminent_PT_collision_buffer.pack(dim_c<4>,"bary",ci); - - vec3 ps[4] = {}; - for(int i = 0;i != 4;++i) - ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); - - auto proj_t = vec3::zeros(); - auto pr = vec3::zeros(); - for(int i = 0;i != 4;++i) - pr += bary[i] * ps[i]; - - for(int i = 0;i != 3;++i) - proj_t -= bary[i] * ps[i]; - - auto collision_normal = pr.normalized(); - - imminent_PT_collision_vis.tuple(dim_c<3>,"collision_normal",ci) = collision_normal; - - imminent_PT_collision_vis.tuple(dim_c<3>,"pt",ci) = proj_t; - imminent_PT_collision_vis.tuple(dim_c<3>,"pp",ci) = vtemp.pack(dim_c<3>,"x",inds[3]); - }); - imminent_PT_collision_vis = imminent_PT_collision_vis.clone({zs::memsrc_e::host}); - - auto prim_PT = std::make_shared(); - auto& vis_verts_PT = prim_PT->verts; - auto& vis_lines_PT = prim_PT->lines; - vis_verts_PT.resize(imminent_PT_collision_buffer.size() * 2); - vis_lines_PT.resize(imminent_PT_collision_buffer.size()); - - auto nrm_prim_PT = std::make_shared(); - auto& nrm_verts_PT = nrm_prim_PT->verts; - auto& nrm_lines_PT = nrm_prim_PT->lines; - nrm_verts_PT.resize(imminent_PT_collision_buffer.size() * 2); - nrm_lines_PT.resize(imminent_PT_collision_buffer.size()); - - ompPol(zs::range(imminent_PT_collision_buffer.size()),[ - &vis_verts_PT,&vis_lines_PT, - &nrm_verts_PT,&nrm_lines_PT, - imminent_PT_collision_vis = proxy({},imminent_PT_collision_vis)] (int ci) mutable { - auto cnrm = imminent_PT_collision_vis.pack(dim_c<3>,"collision_normal",ci); - auto pt = imminent_PT_collision_vis.pack(dim_c<3>,"pt",ci); - auto pp = imminent_PT_collision_vis.pack(dim_c<3>,"pp",ci); - auto pn = pp + cnrm; - - vis_verts_PT[ci * 2 + 0] = pp.to_array(); - vis_verts_PT[ci * 2 + 1] = pt.to_array(); - vis_lines_PT[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; - - nrm_verts_PT[ci * 2 + 0] = pp.to_array(); - nrm_verts_PT[ci * 2 + 1] = pn.to_array(); - nrm_lines_PT[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; - }); - - set_output("prim_PT",std::move(prim_PT)); - set_output("cnrm_PT",std::move(nrm_prim_PT)); - - std::cout << "output PT VIS" << std::endl; - - - dtiles_t imminent_EE_collision_vis(verts.get_allocator(),{ - {"collision_normal",3}, - {"pt",3}, - {"pp",3}},imminent_EE_collision_buffer.size()); - - cudaPol(zs::range(imminent_EE_collision_buffer.size()),[ - vtemp = proxy({},vtemp), - imminent_EE_collision_vis = proxy({},imminent_EE_collision_vis), - imminent_EE_collision_buffer = proxy({},imminent_EE_collision_buffer)] ZS_LAMBDA(auto ci) mutable { - auto inds = imminent_EE_collision_buffer.pack(dim_c<4>,"inds",ci,int_c); - auto bary = imminent_EE_collision_buffer.pack(dim_c<4>,"bary",ci); - - vec3 ps[4] = {}; - for(int i = 0;i != 4;++i) - ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); - - auto pr = vec3::zeros(); - for(int i = 0;i != 4;++i) - pr += bary[i] * ps[i]; - - auto proja = vec3::zeros(); - auto projb = vec3::zeros(); - for(int i = 0;i != 2;++i) { - proja -= bary[i] * ps[i]; - projb += bary[i + 2] * ps[i + 2]; - } - - auto collision_normal = pr.normalized(); - - imminent_EE_collision_vis.tuple(dim_c<3>,"collision_normal",ci) = collision_normal; - imminent_EE_collision_vis.tuple(dim_c<3>,"pt",ci) = proja; - imminent_EE_collision_vis.tuple(dim_c<3>,"pp",ci) = projb; - }); - imminent_EE_collision_vis = imminent_EE_collision_vis.clone({zs::memsrc_e::host}); - - std::cout << "output EE VIS" << std::endl; - - auto prim_EE = std::make_shared(); - auto& vis_verts_EE = prim_EE->verts; - auto& vis_lines_EE = prim_EE->lines; - vis_verts_EE.resize(imminent_EE_collision_buffer.size() * 2); - vis_lines_EE.resize(imminent_EE_collision_buffer.size()); - - auto nrm_prim_EE = std::make_shared(); - auto& nrm_verts_EE = nrm_prim_EE->verts; - auto& nrm_lines_EE = nrm_prim_EE->lines; - nrm_verts_EE.resize(imminent_EE_collision_buffer.size() * 2); - nrm_lines_EE.resize(imminent_EE_collision_buffer.size()); - - ompPol(zs::range(imminent_EE_collision_buffer.size()),[ - &vis_verts_EE,&vis_lines_EE, - &nrm_verts_EE,&nrm_lines_EE, - imminent_EE_collision_vis = proxy({},imminent_EE_collision_vis)] (int ci) mutable { - auto cnrm = imminent_EE_collision_vis.pack(dim_c<3>,"collision_normal",ci); - auto pt = imminent_EE_collision_vis.pack(dim_c<3>,"pt",ci); - auto pp = imminent_EE_collision_vis.pack(dim_c<3>,"pp",ci); - auto pn = pp + cnrm; - - vis_verts_EE[ci * 2 + 0] = pp.to_array(); - vis_verts_EE[ci * 2 + 1] = pt.to_array(); - vis_lines_EE[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; - - nrm_verts_EE[ci * 2 + 0] = pp.to_array(); - nrm_verts_EE[ci * 2 + 1] = pn.to_array(); - nrm_lines_EE[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; - }); - - set_output("prim_EE",std::move(prim_EE)); - set_output("cnrm_EE",std::move(nrm_prim_EE)); - - } -}; - -ZENDEFNODE(VisualizeImminentCollision, {{{"zsparticles"}, - {"string","current_x_tag","x"}, - {"string","previous_x_tag","px"}, - // {"string","pscaleTag","pscale"}, - // {"float","repeling_strength","1.0"}, - {"float","immc_thickness","0.01"}, - // {"int","nm_imminent_iters","1"}, - // {"float","imm_restitution","0.1"}, - // {"float","imm_relaxation","0.25"}, - // {"bool","add_repulsion_force","0"}, - }, - {{"prim_PT"},{"cnrm_PT"},{"prim_EE"},{"cnrm_EE"}}, - {}, - {"PBD"}}); - - -struct DetangleCCDCollision : INode { - using T = float; - using vec3 = zs::vec; - using vec4 = zs::vec; - using vec4i = zs::vec; - using dtiles_t = zs::TileVector; - - virtual void apply() override { - using namespace zs; - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - using lbvh_t = typename ZenoLinearBvh::lbvh_t; - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto exec_tag = wrapv{}; - constexpr auto eps = (T)1e-7; - // constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; - - auto zsparticles = get_input("zsparticles"); - auto current_x_tag = get_input2("current_x_tag"); - auto pre_x_tag = get_input2("previous_x_tag"); - - auto restitution_rate = get_input2("restitution"); - auto relaxation_rate = get_input2("relaxation"); - - auto thickness = get_input2("thickness"); - - auto& verts = zsparticles->getParticles(); - const auto& tris = zsparticles->getQuadraturePoints(); - const auto &edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; - - dtiles_t vtemp(verts.get_allocator(),{ - {"x",3}, - {"v",3}, - {"minv",1} - },(size_t)verts.size()); - - auto nm_ccd_iters = get_input2("nm_ccd_iters"); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - vtemp = proxy({},vtemp), - current_x_tag = zs::SmallString(current_x_tag), - pre_x_tag = zs::SmallString(pre_x_tag)] ZS_LAMBDA(int vi) mutable { - vtemp.tuple(dim_c<3>,"x",vi) = verts.pack(dim_c<3>,pre_x_tag,vi); - vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - verts.pack(dim_c<3>,pre_x_tag,vi); - vtemp("minv",vi) = verts("minv",vi); - }); - - - lbvh_t triBvh{},eBvh{}; - - zs::Vector impulse_buffer{verts.get_allocator(),verts.size()}; - zs::Vector impulse_count{verts.get_allocator(),verts.size()}; - - auto do_ee_detection = get_input2("do_ee_detection"); - auto do_pt_detection = get_input2("do_pt_detection"); - - zs::Vector nm_ccd_collision{verts.get_allocator(),1}; - - std::cout << "resolve continous collision " << std::endl; - - for(int iter = 0;iter != nm_ccd_iters;++iter) { - - cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); - cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); - - nm_ccd_collision.setVal(0); - - if(do_pt_detection) { - auto do_bvh_refit = iter > 0; - COLLISION_UTILS::calc_continous_self_PT_collision_impulse(cudaPol, - verts, - vtemp,"x","v", - tris, - // thickness, - triBvh, - do_bvh_refit, - impulse_buffer, - impulse_count); - } - - if(do_ee_detection) { - auto do_bvh_refit = iter > 0; - COLLISION_UTILS::calc_continous_self_EE_collision_impulse(cudaPol, - verts, - vtemp,"x","v", - edges, - eBvh, - do_bvh_refit, - impulse_buffer, - impulse_count); - } - - // std::cout << "apply impulse" << std::endl; - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - vtemp = proxy({},vtemp), - impulse_buffer = proxy(impulse_buffer), - impulse_count = proxy(impulse_count), - relaxation_rate = relaxation_rate, - nm_ccd_collision = proxy(nm_ccd_collision), - eps = eps, - thickness = thickness, - exec_tag = exec_tag] ZS_LAMBDA(int vi) mutable { - if(impulse_count[vi] == 0) - return; - if(impulse_buffer[vi].norm() < eps) - return; - - auto impulse = relaxation_rate * impulse_buffer[vi] / impulse_count[vi]; - if(impulse.norm() > thickness * 0.01) - atomic_add(exec_tag,&nm_ccd_collision[0],1); - - // auto dv = impulse - vtemp.tuple(dim_c<3>,"v",vi) = vtemp.pack(dim_c<3>,"v",vi) + impulse; - // for(int i = 0;i != 3;++i) - // atomic_add(exec_tag,&vtemp("v",i,vi),dv[i]); - }); - - std::cout << "nm_ccd_collision : " << nm_ccd_collision.getVal() << std::endl; - if(nm_ccd_collision.getVal() == 0) - break; - } - std::cout << "finish solving continous collision " << std::endl; - cudaPol(zs::range(verts.size()),[ - vtemp = proxy({},vtemp), - verts = proxy({},verts), - xtag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { - verts.tuple(dim_c<3>,xtag,vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); - }); - - set_output("zsparticles",zsparticles); - } -}; - -ZENDEFNODE(DetangleCCDCollision, {{{"zsparticles"}, - {"string","current_x_tag","x"}, - {"string","previous_x_tag","px"}, - {"int","nm_ccd_iters","1"}, - // {"string","pscaleTag","pscale"}, - {"float","thickness","0.1"}, - // {"float","immc_thickness","0.01"}, - // {"int","nm_imminent_iters","1"}, - {"float","restitution","0.1"}, - {"float","relaxation","1"}, - {"bool","do_ee_detection","1"}, - {"bool","do_pt_detection","1"}, - // {"bool","add_repulsion_force","0"}, - }, - {{"zsparticles"}}, - {}, - {"PBD"}}); - - -struct VisualizeContinousCollision : INode { - using T = float; - using vec3 = zs::vec; - using dtiles_t = zs::TileVector; - - virtual void apply() override { - using namespace zs; - using lbvh_t = typename ZenoLinearBvh::lbvh_t; - constexpr auto cuda_space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - - constexpr auto omp_space = execspace_e::openmp; - auto ompPol = omp_exec(); - - auto zsparticles = get_input("zsparticles"); - auto current_x_tag = get_input2("current_x_tag"); - auto pre_x_tag = get_input2("previous_x_tag"); - - auto thickness = get_input2("thickness"); - - auto& verts = zsparticles->getParticles(); - const auto& tris = zsparticles->getQuadraturePoints(); - const auto &edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; - - dtiles_t vtemp(verts.get_allocator(),{ - {"x",3}, - {"v",3}, - {"minv",1} - },(size_t)verts.size()); - - lbvh_t triBvh{},eBvh{}; - // auto nm_ccd_iters = get_input2("nm_ccd_iters"); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - vtemp = proxy({},vtemp), - current_x_tag = zs::SmallString(current_x_tag), - pre_x_tag = zs::SmallString(pre_x_tag)] ZS_LAMBDA(int vi) mutable { - vtemp.tuple(dim_c<3>,"x",vi) = verts.pack(dim_c<3>,pre_x_tag,vi); - vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - verts.pack(dim_c<3>,pre_x_tag,vi); - vtemp("minv",vi) = verts("minv",vi); - }); - - zs::Vector impulse_buffer{verts.get_allocator(),verts.size()}; - zs::Vector impulse_count{verts.get_allocator(),verts.size()}; - - cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); - cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); - - COLLISION_UTILS::calc_continous_self_PT_collision_impulse(cudaPol, - verts, - vtemp,"x","v", - tris, - // thickness, - triBvh, - false, - impulse_buffer, - impulse_count); - - cudaPol(zs::range(impulse_buffer.size()),[ - impulse_buffer = proxy(impulse_buffer), - impulse_count = proxy(impulse_count)] ZS_LAMBDA(int vi) mutable { - if(impulse_count[vi] > 0) { - impulse_buffer[vi] = impulse_buffer[vi] / (T)impulse_count[vi]; - // printf("impulse[%d] : %f %f %f\n",vi - // ,(float)impulse_buffer[vi][0] - // ,(float)impulse_buffer[vi][1] - // ,(float)impulse_buffer[vi][2]); - } - }); - - dtiles_t ccd_PT_collision_vis(verts.get_allocator(),{ - {"impulse",3}, - {"p0",3}, - {"p1",3}, - {"pp",3} - },verts.size()); - - cudaPol(zs::range(verts.size()),[ - ccd_PT_collision_vis = proxy({},ccd_PT_collision_vis), - impulse_buffer = proxy(impulse_buffer), - vtemp = proxy({},vtemp)] ZS_LAMBDA(int vi) mutable { - ccd_PT_collision_vis.tuple(dim_c<3>,"impulse",vi) = impulse_buffer[vi]; - ccd_PT_collision_vis.tuple(dim_c<3>,"p0",vi) = vtemp.pack(dim_c<3>,"x",vi); - ccd_PT_collision_vis.tuple(dim_c<3>,"p1",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); - ccd_PT_collision_vis.tuple(dim_c<3>,"pp",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi) + impulse_buffer[vi]; - }); - - ccd_PT_collision_vis = ccd_PT_collision_vis.clone({zs::memsrc_e::host}); - auto prim_PT = std::make_shared(); - auto& vis_verts_PT = prim_PT->verts; - auto& vis_lines_PT = prim_PT->lines; - vis_verts_PT.resize(verts.size() * 2); - vis_lines_PT.resize(verts.size()); - - auto scale = get_input2("scale"); - - ompPol(zs::range(verts.size()),[ - &vis_verts_PT,&vis_lines_PT, - scale = scale, - ccd_PT_collision_vis = proxy({},ccd_PT_collision_vis)] (int vi) mutable { - auto p = ccd_PT_collision_vis.pack(dim_c<3>,"p0",vi); - auto impulse = ccd_PT_collision_vis.pack(dim_c<3>,"impulse",vi); - auto pc = p + impulse * scale; - vis_verts_PT[vi * 2 + 0] = p.to_array(); - vis_verts_PT[vi * 2 + 1] = pc.to_array(); - vis_lines_PT[vi] = zeno::vec2i{vi * 2 + 0,vi * 2 + 1}; - }); - - set_output("impulse_PT",std::move(prim_PT)); - - cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); - cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); - - COLLISION_UTILS::calc_continous_self_EE_collision_impulse(cudaPol, - verts, - vtemp,"x","v", - edges, - eBvh, - false, - impulse_buffer, - impulse_count); - - cudaPol(zs::range(impulse_buffer.size()),[ - impulse_buffer = proxy(impulse_buffer), - impulse_count = proxy(impulse_count)] ZS_LAMBDA(int vi) mutable { - if(impulse_count[vi] > 0) { - impulse_buffer[vi] = impulse_buffer[vi] / (T)impulse_count[vi]; - // printf("impulse[%d] : %f %f %f\n",vi - // ,(float)impulse_buffer[vi][0] - // ,(float)impulse_buffer[vi][1] - // ,(float)impulse_buffer[vi][2]); - } - }); - - dtiles_t ccd_EE_collision_vis(verts.get_allocator(),{ - {"impulse",3}, - {"p0",3}, - {"p1",3}, - {"pp",3} - },verts.size()); - - cudaPol(zs::range(verts.size()),[ - ccd_EE_collision_vis = proxy({},ccd_EE_collision_vis), - impulse_buffer = proxy(impulse_buffer), - vtemp = proxy({},vtemp)] ZS_LAMBDA(int vi) mutable { - ccd_EE_collision_vis.tuple(dim_c<3>,"impulse",vi) = impulse_buffer[vi]; - ccd_EE_collision_vis.tuple(dim_c<3>,"p0",vi) = vtemp.pack(dim_c<3>,"x",vi); - ccd_EE_collision_vis.tuple(dim_c<3>,"p1",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); - ccd_EE_collision_vis.tuple(dim_c<3>,"pp",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi) + impulse_buffer[vi]; - }); - - ccd_EE_collision_vis = ccd_EE_collision_vis.clone({zs::memsrc_e::host}); - auto prim_EE = std::make_shared(); - auto& vis_verts_EE = prim_EE->verts; - auto& vis_lines_EE = prim_EE->lines; - vis_verts_EE.resize(verts.size() * 2); - vis_lines_EE.resize(verts.size()); - - ompPol(zs::range(verts.size()),[ - &vis_verts_EE,&vis_lines_EE, - scale = scale, - ccd_EE_collision_vis = proxy({},ccd_EE_collision_vis)] (int vi) mutable { - auto p = ccd_EE_collision_vis.pack(dim_c<3>,"p0",vi); - auto impulse = ccd_EE_collision_vis.pack(dim_c<3>,"impulse",vi); - auto pc = p + impulse * scale; - vis_verts_EE[vi * 2 + 0] = p.to_array(); - vis_verts_EE[vi * 2 + 1] = pc.to_array(); - vis_lines_EE[vi] = zeno::vec2i{vi * 2 + 0,vi * 2 + 1}; - }); - - set_output("impulse_EE",std::move(prim_EE)); - } -}; - -ZENDEFNODE(VisualizeContinousCollision, {{{"zsparticles"}, - {"string","current_x_tag","x"}, - {"string","previous_x_tag","px"}, - // {"string","pscaleTag","pscale"}, - // {"float","repeling_strength","1.0"}, - {"float","thickness","0.01"}, - // {"int","nm_imminent_iters","1"}, - // {"float","imm_restitution","0.1"}, - {"float","scale","0.25"}, - // {"bool","add_repulsion_force","0"}, - }, - {{"impulse_PT"},{"impulse_EE"}}, - {}, - {"PBD"}}); - -struct SDFColliderProject : INode { - using T = float; - using vec3 = zs::vec; - - virtual void apply() override { - using namespace zs; - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - - auto zsparticles = get_input("zsparticles"); - - auto radius = get_input2("radius"); - auto center = get_input2("center"); - auto cv = get_input2("center_velocity"); - auto w = get_input2("angular_velocity"); - - // prex - auto xtag = get_input2("xtag"); - // x - auto ptag = get_input2("ptag"); - auto friction = get_input2("friction"); - - // auto do_stablize = get_input2("do_stablize"); - - auto& verts = zsparticles->getParticles(); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - xtag = zs::SmallString(xtag), - ptag = zs::SmallString(ptag), - friction, - radius, - center, - // do_stablize, - cv,w] ZS_LAMBDA(int vi) mutable { - if(verts("minv",vi) < (T)1e-6) - return; - - auto pred = verts.pack(dim_c<3>,ptag,vi); - auto pos = verts.pack(dim_c<3>,xtag,vi); - - auto center_vel = vec3::from_array(cv); - auto center_pos = vec3::from_array(center); - auto angular_velocity = vec3::from_array(w); - - auto disp = pred - center_pos; - auto dist = radius - disp.norm() + verts("pscale",vi); - - if(dist < 0) - return; - - auto nrm = disp.normalized(); - - auto dp = dist * nrm; - if(dp.norm() < (T)1e-6) - return; - - pred += dp; - - // if(do_stablize) { - // pos += dp; - // verts.tuple(dim_c<3>,xtag,vi) = pos; - // } - - auto collider_velocity_at_p = center_vel + angular_velocity.cross(pred - center_pos); - auto rel_vel = pred - pos - center_vel; - - auto tan_vel = rel_vel - nrm * rel_vel.dot(nrm); - auto tan_len = tan_vel.norm(); - auto max_tan_len = dp.norm() * friction; - - if(tan_len > (T)1e-6) { - auto alpha = (T)max_tan_len / (T)tan_len; - dp = -tan_vel * zs::min(alpha,(T)1.0); - pred += dp; - } - - // dp = dp * verts("m",vi) * verts("minv",vi); - - verts.tuple(dim_c<3>,ptag,vi) = pred; - }); - set_output("zsparticles",zsparticles); - } - -}; - -ZENDEFNODE(SDFColliderProject, {{{"zsparticles"}, - {"float","radius","1"}, - {"center"}, - {"center_velocity"}, - {"angular_velocity"}, - {"string","xtag","x"}, - {"string","ptag","x"}, - {"float","friction","0"} - // {"bool","do_stablize","0"} - }, - {{"zsparticles"}}, - {}, - {"PBD"}}); - - -struct BuildZSLBvhFromAABB : INode { - - template - void buildBvh(zs::CudaExecutionPolicy &pol, - TileVecT &verts, - const zs::SmallString& srcTag, - const zs::SmallString& dstTag, - const zs::SmallString& pscaleTag, - ZenoLinearBvh::lbvh_t &bvh) { - using namespace zs; - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto space = execspace_e::cuda; - Vector bvs{verts.get_allocator(), verts.size()}; - pol(range(verts.size()), - [verts = proxy({}, verts), - bvs = proxy(bvs), - pscaleTag,srcTag,dstTag] ZS_LAMBDA(int vi) mutable { - auto src = verts.template pack<3>(srcTag, vi); - auto dst = verts.template pack<3>(dstTag, vi); - auto pscale = verts(pscaleTag,vi); - - bv_t bv{src,dst}; - bv._min -= pscale; - bv._max += pscale; - bvs[vi] = bv; - }); - bvh.build(pol, bvs); - } - - virtual void apply() override { - using namespace zs; - auto cudaPol = cuda_exec().device(0); - - auto pars = get_input("ZSParticles"); - auto srcTag = get_input2("fromTag"); - auto dstTag = get_input2("toTag"); - auto pscaleTag = get_input2("pscaleTag"); - - auto out = std::make_shared(); - auto &bvh = out->get(); - - buildBvh(cudaPol,pars->getParticles(),srcTag,dstTag,pscaleTag,bvh); - out->thickness = 0; - - set_output("ZSLBvh",std::move(out)); - } -}; - -ZENDEFNODE(BuildZSLBvhFromAABB, {{{"ZSParticles"}, - {"string","fromTag","px"}, - {"string","toTag","x"}, - {"string","pscaleTag","pscale"} - }, {"ZSLBvh"}, {}, {"XPBD"}}); - - -struct XPBDDetangle : INode { - // ray trace bvh - template - void buildRayTraceBvh(zs::CudaExecutionPolicy &pol, - TileVecT &verts, - const zs::SmallString& srcTag, - const zs::SmallString& dstTag, - const zs::SmallString& pscaleTag, - ZenoLinearBvh::lbvh_t &bvh) { - using namespace zs; - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto space = execspace_e::cuda; - Vector bvs{verts.get_allocator(), verts.size()}; - pol(range(verts.size()), - [verts = proxy({}, verts), - bvs = proxy(bvs), - pscaleTag,srcTag,dstTag] ZS_LAMBDA(int vi) mutable { - auto src = verts.template pack<3>(srcTag, vi); - auto dst = verts.template pack<3>(dstTag, vi); - auto pscale = verts(pscaleTag,vi); - - bv_t bv{src,dst}; - bv._min -= pscale; - bv._max += pscale; - bvs[vi] = bv; - }); - bvh.build(pol, bvs); - } - // particle sphere bvh - template - void buildBvh(zs::CudaExecutionPolicy &pol, - TileVecT &verts, - const zs::SmallString& xtag, - const zs::SmallString& pscaleTag, - ZenoLinearBvh::lbvh_t& bvh) { - using namespace zs; - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto space = execspace_e::cuda; - Vector bvs{verts.get_allocator(), verts.size()}; - - pol(range(verts.size()), - [verts = proxy({}, verts), - bvs = proxy(bvs), - pscaleTag,xtag] ZS_LAMBDA(int vi) mutable { - auto pos = verts.template pack<3>(xtag, vi); - auto pscale = verts(pscaleTag,vi); - bv_t bv{pos - pscale,pos + pscale}; - bvs[vi] = bv; - }); - bvh.build(pol, bvs); - } - - virtual void apply() override { - using namespace zs; - using bvh_t = typename ZenoLinearBvh::lbvh_t; - using bv_t = typename bvh_t::Box; - - using vec3 = zs::vec; - using vec2i = zs::vec; - using vec3i = zs::vec; - using vec4i = zs::vec; - using mat4 = zs::vec; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - constexpr auto exec_tag = wrapv{}; - - auto zsparticles = get_input("zsparticles"); - auto xtag = get_input2("xtag"); - auto pxtag = get_input2("pxtag"); - auto Xtag = get_input2("Xtag"); - // auto ccdTag = get_input2("ccdTag"); - // x - auto pscaleTag = get_input2("pscaleTag"); - // auto friction = get_input2("friction"); - - auto& verts = zsparticles->getParticles(); - - auto spBvh = bvh_t{}; - buildRayTraceBvh(cudaPol,verts,pxtag,xtag,pscaleTag,spBvh); - - zs::Vector ccd_buffer{verts.get_allocator(),verts.size()}; - cudaPol(zs::range(ccd_buffer),[]ZS_LAMBDA(auto& t) mutable {t = (float)1;}); - // zs::Vector dp_count{verts.get_allocator(),verts.size()}; - // cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - xtag = zs::SmallString(xtag), - Xtag = zs::SmallString(Xtag), - pxtag = zs::SmallString(pxtag), - exec_tag, - ccd_buffer = proxy(ccd_buffer), - pscaleTag = zs::SmallString(pscaleTag), - spBvh = proxy(spBvh)] ZS_LAMBDA(int vi) mutable { - auto dst = verts.pack(dim_c<3>,xtag,vi); - auto src = verts.pack(dim_c<3>,pxtag,vi); - auto vel = dst - src; - auto rpos = verts.pack(dim_c<3>,Xtag,vi); - // auto r = verts(ptag,vi); - - auto pscale = verts(pscaleTag,vi); - bv_t bv{src,dst}; - bv._min -= pscale; - bv._max += pscale; - - auto find_particle_sphere_collision_pairs = [&] (int nvi) mutable { - if(vi >= nvi) - return; - - auto npscale = verts(pscaleTag,nvi); - auto thickness = pscale + npscale; - - auto nrpos = verts.pack(dim_c<3>,Xtag,nvi); - auto rdist = (rpos - nrpos).norm(); - if(rdist < thickness) - return; - - auto ndst = verts.pack(dim_c<3>,xtag,nvi); - auto nsrc = verts.pack(dim_c<3>,pxtag,nvi); - auto nvel = ndst - nsrc; - - auto t = LSL_GEO::ray_ray_intersect(src,vel,nsrc,nvel,thickness); - if(t <= (float)1 && t >= (float)0) { - atomic_min(exec_tag,&ccd_buffer[vi],t); - atomic_min(exec_tag,&ccd_buffer[nvi],t); - } - }; - - spBvh.iter_neighbors(bv,find_particle_sphere_collision_pairs); - }); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - xtag = zs::SmallString(xtag), - pxtag = zs::SmallString(pxtag), - // friction, - ccd_buffer = proxy(ccd_buffer)] ZS_LAMBDA(int vi) mutable { - auto src = verts.pack(dim_c<3>,pxtag,vi); - auto dst = verts.pack(dim_c<3>,xtag,vi); - - auto vel = dst - src; - auto project = src + vel * ccd_buffer[vi]; - - verts.tuple(dim_c<3>,xtag,vi) = project; - }); - - set_output("zsparticles",zsparticles); - } - -}; - -ZENDEFNODE(XPBDDetangle, {{{"zsparticles"}, - // {"float","relaxation_strength","1"}, - {"string","xtag","x"}, - {"string","pxtag","px"}, - {"string","Xtag","X"}, - {"string","pscaleTag","pscale"} - // {"float","friction","0"} - }, - {{"zsparticles"}}, - { - // {"string","ptag","x"} - }, - {"PBD"}}); - - -struct XPBDDetangle2 : INode { - // ray trace bvh - template - void buildRayTraceBvh(zs::CudaExecutionPolicy &pol, - TileVecT &verts, - const zs::SmallString& srcTag, - const zs::SmallString& dstTag, - const zs::SmallString& pscaleTag, - ZenoLinearBvh::lbvh_t &bvh) { - using namespace zs; - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto space = execspace_e::cuda; - Vector bvs{verts.get_allocator(), verts.size()}; - pol(range(verts.size()), - [verts = proxy({}, verts), - bvs = proxy(bvs), - pscaleTag,srcTag,dstTag] ZS_LAMBDA(int vi) mutable { - auto src = verts.template pack<3>(srcTag, vi); - auto dst = verts.template pack<3>(dstTag, vi); - auto pscale = verts(pscaleTag,vi); - - bv_t bv{src,dst}; - bv._min -= pscale; - bv._max += pscale; - bvs[vi] = bv; - }); - bvh.build(pol, bvs); - } - // particle sphere bvh - // template - // void buildBvh(zs::CudaExecutionPolicy &pol, - // TileVecT &verts, - // const zs::SmallString& xtag, - // const zs::SmallString& pscaleTag, - // ZenoLinearBvh::lbvh_t& bvh) { - // using namespace zs; - // using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - // constexpr auto space = execspace_e::cuda; - // Vector bvs{verts.get_allocator(), verts.size()}; - - // pol(range(verts.size()), - // [verts = proxy({}, verts), - // bvs = proxy(bvs), - // pscaleTag,xtag] ZS_LAMBDA(int vi) mutable { - // auto pos = verts.template pack<3>(xtag, vi); - // auto pscale = verts(pscaleTag,vi); - // bv_t bv{pos - pscale,pos + pscale}; - // bvs[vi] = bv; - // }); - // bvh.build(pol, bvs); - // } - - virtual void apply() override { - using namespace zs; - using bvh_t = typename ZenoLinearBvh::lbvh_t; - using bv_t = typename bvh_t::Box; - - using vec3 = zs::vec; - using vec2i = zs::vec; - using vec3i = zs::vec; - using vec4i = zs::vec; - using mat4 = zs::vec; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - constexpr auto exec_tag = wrapv{}; - constexpr auto eps = (T)1e-7; - - auto zsparticles = get_input("zsparticles"); - auto xtag = get_input2("xtag"); - auto pxtag = get_input2("pxtag"); - auto Xtag = get_input2("Xtag"); - // auto ccdTag = get_input2("ccdTag"); - // x - auto pscaleTag = get_input2("pscaleTag"); - // auto friction = get_input2("friction"); - - auto& verts = zsparticles->getParticles(); - - auto restitution_rate = get_input2("restitution"); - auto relaxation_rate = get_input2("relaxation"); - - auto spBvh = bvh_t{}; - buildRayTraceBvh(cudaPol,verts,pxtag,xtag,pscaleTag,spBvh); - - // zs::Vector ccd_buffer{verts.get_allocator(),verts.size()}; - // cudaPol(zs::range(ccd_buffer),[]ZS_LAMBDA(auto& t) mutable {t = (float)1;}); - - zs::Vector dp_buffer{verts.get_allocator(),verts.size()}; - cudaPol(zs::range(dp_buffer),[]ZS_LAMBDA(auto& dp) mutable {dp = vec3::uniform(0);}); - - zs::Vector dp_count{verts.get_allocator(),verts.size()}; - cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - xtag = zs::SmallString(xtag), - Xtag = zs::SmallString(Xtag), - pxtag = zs::SmallString(pxtag), - restitution_rate = restitution_rate, - exec_tag = exec_tag, - eps = eps, - dp_buffer = proxy(dp_buffer), - dp_count = proxy(dp_count), - pscaleTag = zs::SmallString(pscaleTag), - spBvh = proxy(spBvh)] ZS_LAMBDA(int vi) mutable { - auto dst = verts.pack(dim_c<3>,xtag,vi); - auto src = verts.pack(dim_c<3>,pxtag,vi); - auto vel = dst - src; - auto rpos = verts.pack(dim_c<3>,Xtag,vi); - // auto r = verts(ptag,vi); - - auto pscale = verts(pscaleTag,vi); - bv_t bv{src,dst}; - bv._min -= pscale; - bv._max += pscale; - - auto find_particle_sphere_collision_pairs = [&] (int nvi) mutable { - if(vi >= nvi) - return; - - auto npscale = verts(pscaleTag,nvi); - auto thickness = pscale + npscale; - - auto nrpos = verts.pack(dim_c<3>,Xtag,nvi); - auto rdist = (rpos - nrpos).norm(); - if(rdist < thickness) - return; - - auto ndst = verts.pack(dim_c<3>,xtag,nvi); - auto nsrc = verts.pack(dim_c<3>,pxtag,nvi); - auto nvel = ndst - nsrc; - - auto t = LSL_GEO::ray_ray_intersect(src,vel,nsrc,nvel,thickness); - if(t <= (float)1 && t >= (float)0) { - // atomic_min(exec_tag,&ccd_buffer[vi],t); - // atomic_min(exec_tag,&ccd_buffer[nvi],t); - // auto rel_vel = vel - nvel; - auto CR = restitution_rate; - - auto minv_a = verts("minv",vi); - auto minv_b = verts("minv",nvi); - if(minv_a + minv_b < (T)2 * eps) - return; - - auto ua = vel; - auto ub = nvel; - auto ur = ua - ub; - - vec3 va{},vb{}; - - if(minv_a > (T)eps) { - // ma / mb - auto ratio = minv_b / minv_a; - auto m = ratio + (T)1; - auto momt = ratio * ua + ub; - va = (momt - CR * ur) / m; - vb = (momt + CR * ratio * ur) / m; - - if(isnan(va.norm()) || isnan(vb.norm())) { - printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); - } - }else if(minv_b > (T)eps) { - // mb / ma - auto ratio = minv_a / minv_b; - auto m = ratio + (T)1; - auto momt = ua + ratio * ub; - va = (momt - CR * ratio * ur) / m; - vb = (momt + CR * ur) / m; - if(isnan(va.norm()) || isnan(vb.norm())) { - printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); - } - - }else { - printf("impossible reaching here\n"); - } - - // auto ma = verts("m",vi); - // auto mb = verts("m",nvi); - - // auto m = ma + mb; - // auto momt = ma * ua + mb * ub; - - - // auto va = (momt - CR * mb * ur) / m; - // auto vb = (momt + CR * ma * ur) / m; - - auto dpa = (va - ua) * (1 - t); - auto dpb = (vb - ub) * (1 - t); - - // auto dpa = (va - ua); - // auto dpb = (vb - ub); - // printf("find collision pair : %d %d\n",vi,nvi); - atomic_add(exec_tag,&dp_count[vi],1); - atomic_add(exec_tag,&dp_count[nvi],1); - - for(int i = 0;i != 3;++i) { - atomic_add(exec_tag,&dp_buffer[vi][i],dpa[i]); - atomic_add(exec_tag,&dp_buffer[nvi][i],dpb[i]); - } - } - }; - - spBvh.iter_neighbors(bv,find_particle_sphere_collision_pairs); - }); - - // zs:Vector dpnorm{verts.get_allocator(),1}; - // dpnorm.setVal(0); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - xtag = zs::SmallString(xtag), - relaxation_rate = relaxation_rate, - pxtag = zs::SmallString(pxtag), - exec_tag, - // dpnorm = proxy(dpnorm), - dp_count = proxy(dp_count), - dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { - if(dp_count[vi] == 0) - return; - auto minv = verts("minv",vi); - auto dp = dp_buffer[vi] * relaxation_rate / (T)dp_count[vi]; - // atomic_add(exec_tag,&dpnorm[0],dp.norm()); - verts.tuple(dim_c<3>,xtag,vi) = verts.pack(dim_c<3>,xtag,vi) + dp; - }); - - // std::cout << "detangle_dp_norm : " << dpnorm.getVal(0) << std::endl; - - set_output("zsparticles",zsparticles); - } -}; - -ZENDEFNODE(XPBDDetangle2, {{{"zsparticles"}, - // {"float","relaxation_strength","1"}, - {"string","xtag","x"}, - {"string","pxtag","px"}, - {"string","Xtag","X"}, - {"string","pscaleTag","pscale"}, - {"float","restitution","0.1"}, - {"float","relaxation","1"}, - // {"float","friction","0"} - }, - {{"zsparticles"}}, - { - // {"string","ptag","x"} - }, - {"PBD"}}); - - - -// struct XPBDDetangle3 : INode { -// // ray trace bvh -// template -// void buildTriRayTraceBvh(zs::CudaExecutionPolicy &pol, -// const TileVecT &verts, -// const ElmTileVec& tris, -// const T& dt, -// const zs::SmallString& xTag, -// const zs::SmallString& vTag, -// const T& thickness,ZenoLinearBvh::lbvh_t &bvh) { -// using namespace zs; -// using bv_t = typename ZenoLinearBvh::lbvh_t::Box; -// constexpr auto space = execspace_e::cuda; -// Vector bvs{tris.get_allocator(), tris.size()}; -// retrieve_bounding_volumes(pol,verts,xTag,tris,verts,vTag,dt,0,bvs); -// bvh.build(pol, bvs); -// } - -// virtual void apply() override { -// using namespace zs; -// using bvh_t = typename ZenoLinearBvh::lbvh_t; -// using bv_t = typename bvh_t::Box; - -// using vec3 = zs::vec; -// using vec2i = zs::vec; -// using vec3i = zs::vec; -// using vec4i = zs::vec; -// using mat4 = zs::vec; - -// constexpr auto space = execspace_e::cuda; -// auto cudaPol = cuda_exec(); -// constexpr auto exec_tag = wrapv{}; -// constexpr auto eps = (T)1e-7; - -// auto zsparticles = get_input("zsparticles"); -// auto xtag = get_input2("xtag"); -// auto vtag = get_input2("vtag"); -// // auto pxtag = get_input2("pxtag"); -// // auto Xtag = get_input2("Xtag"); -// // auto ccdTag = get_input2("ccdTag"); -// // x -// // auto pscaleTag = get_input2("pscaleTag"); -// // auto friction = get_input2("friction"); - -// auto& verts = zsparticles->getParticles(); - -// auto restitution_rate = get_input2("restitution"); -// auto relaxation_rate = get_input2("relaxation"); - -// auto triRayBvh = bvh_t{}; -// buildTriRayTraceBvh(cudaPol,verts,pxtag,xtag,pscaleTag,triRayBvh); - -// // zs::Vector ccd_buffer{verts.get_allocator(),verts.size()}; -// // cudaPol(zs::range(ccd_buffer),[]ZS_LAMBDA(auto& t) mutable {t = (float)1;}); - -// zs::Vector dp_buffer{verts.get_allocator(),verts.size()}; -// cudaPol(zs::range(dp_buffer),[]ZS_LAMBDA(auto& dp) mutable {dp = vec3::uniform(0);}); - -// zs::Vector dp_count{verts.get_allocator(),verts.size()}; -// cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); - -// cudaPol(zs::range(verts.size()),[ -// verts = proxy({},verts), -// xtag = zs::SmallString(xtag), -// Xtag = zs::SmallString(Xtag), -// pxtag = zs::SmallString(pxtag), -// restitution_rate = restitution_rate, -// exec_tag = exec_tag, -// eps = eps, -// dp_buffer = proxy(dp_buffer), -// dp_count = proxy(dp_count), -// pscaleTag = zs::SmallString(pscaleTag), -// spBvh = proxy(spBvh)] ZS_LAMBDA(int vi) mutable { -// auto dst = verts.pack(dim_c<3>,xtag,vi); -// auto src = verts.pack(dim_c<3>,pxtag,vi); -// auto vel = dst - src; -// auto rpos = verts.pack(dim_c<3>,Xtag,vi); -// // auto r = verts(ptag,vi); - -// auto pscale = verts(pscaleTag,vi); -// bv_t bv{src,dst}; -// bv._min -= pscale; -// bv._max += pscale; - -// auto find_particle_sphere_collision_pairs = [&] (int nvi) mutable { -// if(vi >= nvi) -// return; - -// auto npscale = verts(pscaleTag,nvi); -// auto thickness = pscale + npscale; - -// auto nrpos = verts.pack(dim_c<3>,Xtag,nvi); -// auto rdist = (rpos - nrpos).norm(); -// if(rdist < thickness) -// return; - -// auto ndst = verts.pack(dim_c<3>,xtag,nvi); -// auto nsrc = verts.pack(dim_c<3>,pxtag,nvi); -// auto nvel = ndst - nsrc; - -// auto t = LSL_GEO::ray_ray_intersect(src,vel,nsrc,nvel,thickness); -// if(t <= (float)1 && t >= (float)0) { -// // atomic_min(exec_tag,&ccd_buffer[vi],t); -// // atomic_min(exec_tag,&ccd_buffer[nvi],t); -// // auto rel_vel = vel - nvel; -// auto CR = restitution_rate; - -// auto minv_a = verts("minv",vi); -// auto minv_b = verts("minv",nvi); -// if(minv_a + minv_b < (T)2 * eps) -// return; - -// auto ua = vel; -// auto ub = nvel; -// auto ur = ua - ub; - -// vec3 va{},vb{}; - -// if(minv_a > (T)eps) { -// // ma / mb -// auto ratio = minv_b / minv_a; -// auto m = ratio + (T)1; -// auto momt = ratio * ua + ub; -// va = (momt - CR * ur) / m; -// vb = (momt + CR * ratio * ur) / m; - -// if(isnan(va.norm()) || isnan(vb.norm())) { -// printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); -// } -// }else if(minv_b > (T)eps) { -// // mb / ma -// auto ratio = minv_a / minv_b; -// auto m = ratio + (T)1; -// auto momt = ua + ratio * ub; -// va = (momt - CR * ratio * ur) / m; -// vb = (momt + CR * ur) / m; -// if(isnan(va.norm()) || isnan(vb.norm())) { -// printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); -// } - -// }else { -// printf("impossible reaching here\n"); -// } - -// // auto ma = verts("m",vi); -// // auto mb = verts("m",nvi); - -// // auto m = ma + mb; -// // auto momt = ma * ua + mb * ub; - - -// // auto va = (momt - CR * mb * ur) / m; -// // auto vb = (momt + CR * ma * ur) / m; - -// auto dpa = (va - ua) * (1 - t); -// auto dpb = (vb - ub) * (1 - t); - -// // auto dpa = (va - ua); -// // auto dpb = (vb - ub); -// // printf("find collision pair : %d %d\n",vi,nvi); -// atomic_add(exec_tag,&dp_count[vi],1); -// atomic_add(exec_tag,&dp_count[nvi],1); - -// for(int i = 0;i != 3;++i) { -// atomic_add(exec_tag,&dp_buffer[vi][i],dpa[i]); -// atomic_add(exec_tag,&dp_buffer[nvi][i],dpb[i]); -// } -// } -// }; - -// spBvh.iter_neighbors(bv,find_particle_sphere_collision_pairs); -// }); - -// // zs:Vector dpnorm{verts.get_allocator(),1}; -// // dpnorm.setVal(0); - -// cudaPol(zs::range(verts.size()),[ -// verts = proxy({},verts), -// xtag = zs::SmallString(xtag), -// relaxation_rate = relaxation_rate, -// pxtag = zs::SmallString(pxtag), -// exec_tag, -// // dpnorm = proxy(dpnorm), -// dp_count = proxy(dp_count), -// dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { -// if(dp_count[vi] == 0) -// return; -// auto minv = verts("minv",vi); -// auto dp = dp_buffer[vi] * relaxation_rate / (T)dp_count[vi]; -// // atomic_add(exec_tag,&dpnorm[0],dp.norm()); -// verts.tuple(dim_c<3>,xtag,vi) = verts.pack(dim_c<3>,xtag,vi) + dp; -// }); - -// // std::cout << "detangle_dp_norm : " << dpnorm.getVal(0) << std::endl; - -// set_output("zsparticles",zsparticles); -// } -// }; - -// ZENDEFNODE(XPBDDetangle3, {{{"zsparticles"}, -// // {"float","relaxation_strength","1"}, -// {"string","xtag","x"}, -// {"string","pxtag","px"}, -// {"string","Xtag","X"}, -// {"string","pscaleTag","pscale"}, -// {"float","restitution","0.1"}, -// {"float","relaxation","1"}, -// // {"float","friction","0"} -// }, -// {{"zsparticles"}}, -// { -// // {"string","ptag","x"} -// }, -// {"PBD"}}); - - -// make sure the mesh is intersection-free before using this node -// we use linear trajectory here, such that vel = (x - px) / dt -struct ExchangeLocalMomentum : INode { - // ray trace bvh - template - void buildSphereBvh(zs::CudaExecutionPolicy &pol, - TileVecT &verts, - const zs::SmallString& xtag, - const zs::SmallString& pscaleTag, - ZenoLinearBvh::lbvh_t& bvh) { - using namespace zs; - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto space = execspace_e::cuda; - Vector bvs{verts.get_allocator(), verts.size()}; - - pol(range(verts.size()), - [verts = proxy({}, verts), - pscaleTag = zs::SmallString(pscaleTag), - bvs = proxy(bvs),xtag] ZS_LAMBDA(int vi) mutable { - auto pos = verts.template pack<3>(xtag, vi); - auto r = verts(pscaleTag,vi); - bv_t bv{pos - r,pos + r}; - bvs[vi] = bv; - }); - bvh.build(pol, bvs); - } - - virtual void apply() override { - using namespace zs; - using bvh_t = typename ZenoLinearBvh::lbvh_t; - using bv_t = typename bvh_t::Box; - - using vec3 = zs::vec; - using vec2i = zs::vec; - using vec3i = zs::vec; - using vec4i = zs::vec; - using mat4 = zs::vec; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - constexpr auto exec_tag = wrapv{}; - constexpr auto eps = (T)1e-7; - - auto zsparticles = get_input("zsparticles"); - auto xtag = get_input2("xtag"); - auto vtag = get_input2("vtag"); - auto Xtag = get_input2("Xtag"); - // auto ccdTag = get_input2("ccdTag"); - // x - auto pscaleTag = get_input2("pscaleTag"); - // auto thickness = get_input2("thickness"); - // auto friction = get_input2("friction"); - - auto& verts = zsparticles->getParticles(); - - auto restitution_rate = get_input2("restitution"); - auto relaxation_rate = get_input2("relaxation"); - - zs::Vector impulse_buffer{verts.get_allocator(),verts.size()}; - cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); - - zs::Vector impulse_count{verts.get_allocator(),verts.size()}; - cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); - - auto spBvh = bvh_t{}; - buildSphereBvh(cudaPol,verts,xtag,pscaleTag,spBvh); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - xtag = zs::SmallString(xtag), - pscaleTag = zs::SmallString(pscaleTag), - Xtag = zs::SmallString(Xtag), - vtag = zs::SmallString(vtag), - restitution_rate = restitution_rate, - exec_tag = exec_tag, - // thickness = thickness, - eps = eps, - impulse_buffer = proxy(impulse_buffer), - impulse_count = proxy(impulse_count), - spBvh = proxy(spBvh)] ZS_LAMBDA(int vi) mutable { - auto pos = verts.pack(dim_c<3>,xtag,vi); - auto vel = verts.pack(dim_c<3>,vtag,vi); - auto r = verts(pscaleTag,vi); - auto rpos = verts.pack(dim_c<3>,Xtag,vi); - bv_t bv{pos - r,pos + r}; - - auto find_particle_sphere_collision_pairs = [&] (int nvi) mutable { - if(vi >= nvi) - return; - - auto npos = verts.pack(dim_c<3>,xtag,nvi); - auto nvel = verts.pack(dim_c<3>,vtag,nvi); - auto nr = verts(pscaleTag,nvi); - auto nrpos = verts.pack(dim_c<3>,Xtag,nvi); - auto rdist = (nrpos - rpos).norm(); - - auto thickness = r + nr; - - if(rdist < thickness) - return; - - - auto dp = pos - npos; - auto dv = vel - nvel; - if(dp.dot(dv) > 0) - return; - - auto dist = (pos - npos).norm(); - // as the bvh is bounding box, for sphere intersection, extra distance check is needed - if(dist < (float)thickness) { - auto CR = restitution_rate; - - auto minv_a = verts("minv",vi); - auto minv_b = verts("minv",nvi); - if(minv_a + minv_b < (T)2 * eps) - return; - - auto ua = vel; - auto ub = nvel; - auto ur = ua - ub; - - vec3 va{},vb{}; - - // exchange momentum - if(minv_a > (T)eps) { - // ma / mb - auto ratio = minv_b / minv_a; - auto m = ratio + (T)1; - auto momt = ratio * ua + ub; - va = (momt - CR * ur) / m; - vb = (momt + CR * ratio * ur) / m; - - if(isnan(va.norm()) || isnan(vb.norm())) { - printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); - } - }else if(minv_b > (T)eps) { - // mb / ma - auto ratio = minv_a / minv_b; - auto m = ratio + (T)1; - auto momt = ua + ratio * ub; - va = (momt - CR * ratio * ur) / m; - vb = (momt + CR * ur) / m; - if(isnan(va.norm()) || isnan(vb.norm())) { - printf("nan value detected : %f %f\n",(float)va.norm(),(float)vb.norm()); - } - - }else { - printf("impossible reaching here\n"); - } - - auto ma = verts("m",vi); - auto mb = verts("m",nvi); - - auto dva = (va - ua) * ma; - auto dvb = (vb - ub) * mb; - - // auto dpa = (va - ua); - // auto dpb = (vb - ub); - // printf("find collision pair : %d %d\n",vi,nvi); - atomic_add(exec_tag,&impulse_count[vi],1); - atomic_add(exec_tag,&impulse_count[nvi],1); - - for(int i = 0;i != 3;++i) { - atomic_add(exec_tag,&impulse_buffer[vi][i],dva[i]); - atomic_add(exec_tag,&impulse_buffer[nvi][i],dvb[i]); - } - } - }; - - spBvh.iter_neighbors(bv,find_particle_sphere_collision_pairs); - }); - - // apply the impulse - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - // xtag = zs::SmallString(xtag), - relaxation_rate = relaxation_rate, - vtag = zs::SmallString(vtag), - exec_tag, - // dpnorm = proxy(dpnorm), - impulse_count = proxy(impulse_count), - impulse_buffer = proxy(impulse_buffer)] ZS_LAMBDA(int vi) mutable { - if(impulse_count[vi] == 0) - return; - auto minv = verts("minv",vi); - auto dv = minv * impulse_buffer[vi] * relaxation_rate / (T)impulse_count[vi]; - // atomic_add(exec_tag,&dpnorm[0],dp.norm()); - verts.tuple(dim_c<3>,vtag,vi) = verts.pack(dim_c<3>,vtag,vi) + dv; - }); - - set_output("zsparticles",zsparticles); - } - -}; - -ZENDEFNODE(ExchangeLocalMomentum, {{{"zsparticles"}, - // {"float","relaxation_strength","1"}, - {"string","xtag","x"}, - {"string","Xtag","X"}, - // {"string","pxtag","px"}, - {"string","vtag","v"}, - {"string","pscaleTag","pscale"}, - // {"float","thickness","0.0001"}, - {"float","restitution","0.1"}, - {"float","relaxation","1"}, - // {"float","friction","0"} - }, - {{"zsparticles"}}, - { - // {"string","ptag","x"} - }, - {"PBD"}}); - -struct VisualizePoints : INode { - virtual void apply() override { - using namespace zs; - using bvh_t = typename ZenoLinearBvh::lbvh_t; - using bv_t = typename bvh_t::Box; - - using vec3 = zs::vec; - using vec2i = zs::vec; - using vec3i = zs::vec; - using vec4i = zs::vec; - using mat4 = zs::vec; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - constexpr auto exec_tag = wrapv{}; - - auto zsparticles = get_input("zsparticles"); - auto xtag = get_input2("xtag"); - const auto& verts = zsparticles->getParticles(); - - zs::Vector points{verts.get_allocator(),verts.size()}; - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - xtag = zs::SmallString(xtag), - points = proxy(points)] ZS_LAMBDA(int vi) mutable { - points[vi] = verts.pack(dim_c<3>,xtag,vi); - }); - - points = points.clone({zs::memsrc_e::host}); - auto points_field = std::make_shared(); - points_field->resize(verts.size()); - auto& sverts = points_field->attr("pos"); - constexpr auto omp_space = execspace_e::openmp; - auto ompPol = omp_exec(); - - ompPol(zs::range(verts.size()),[ - &sverts,points = proxy(points)] (int vi) mutable { - sverts[vi] = points[vi].to_array(); - }); - - set_output("points",std::move(points_field)); - } -}; - -ZENDEFNODE(VisualizePoints, { - {"zsparticles",{"string","xtag","x"}}, - {"points"}, - {{"float","scale","1.0"}}, - {"ZSGeometry"}, -}); - - - -struct XPBDParticlesCollider : INode { - // particle sphere bvh - template - void buildBvh(zs::CudaExecutionPolicy &pol, - TileVecT &verts, - const zs::SmallString& xtag, - const zs::SmallString& pscaleTag, - ZenoLinearBvh::lbvh_t& bvh) { - using namespace zs; - using bv_t = typename ZenoLinearBvh::lbvh_t::Box; - constexpr auto space = execspace_e::cuda; - Vector bvs{verts.get_allocator(), verts.size()}; - - pol(range(verts.size()), - [verts = proxy({}, verts), - bvs = proxy(bvs), - pscaleTag,xtag] ZS_LAMBDA(int vi) mutable { - auto pos = verts.template pack<3>(xtag, vi); - auto pscale = verts(pscaleTag,vi); - bv_t bv{pos - pscale,pos + pscale}; - bvs[vi] = bv; - }); - bvh.build(pol, bvs); - } - - virtual void apply() override { - using namespace zs; - using bvh_t = typename ZenoLinearBvh::lbvh_t; - using bv_t = typename bvh_t::Box; - - using vec3 = zs::vec; - using vec2i = zs::vec; - using vec3i = zs::vec; - using vec4i = zs::vec; - using mat4 = zs::vec; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - constexpr auto exec_tag = wrapv{}; - - auto zsparticles = get_input("zsparticles"); - // auto bvh = get_input("lbvh"); - // prex - auto xtag = get_input2("xtag"); - auto pxtag = get_input2("pxtag"); - auto Xtag = get_input2("Xtag"); - // x - auto ptag = get_input2("pscaleTag"); - auto friction = get_input2("friction"); - - auto& verts = zsparticles->getParticles(); - - auto spBvh = bvh_t{}; - buildBvh(cudaPol,verts,xtag,ptag,spBvh); - - // auto collisionThickness = - - zs::Vector dp_buffer{verts.get_allocator(),verts.size() * 3}; - cudaPol(zs::range(dp_buffer),[]ZS_LAMBDA(auto& v) mutable {v = 0;}); - zs::Vector dp_count{verts.get_allocator(),verts.size()}; - cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); - - // find collision pairs - zs::bht csPT{verts.get_allocator(),100000}; - csPT.reset(cudaPol,true); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - xtag = zs::SmallString(xtag), - pxtag = zs::SmallString(pxtag), - Xtag = zs::SmallString(Xtag), - ptag = zs::SmallString(ptag), - spBvh = proxy(spBvh), - csPT = proxy(csPT)] ZS_LAMBDA(int vi) mutable { - auto pos = verts.pack(dim_c<3>,xtag,vi); - auto ppos = verts.pack(dim_c<3>,pxtag,vi); - auto vel = pos - ppos; - auto rpos = verts.pack(dim_c<3>,Xtag,vi); - auto r = verts(ptag,vi); - - auto bv = bv_t{pos - r,pos + r}; - - auto find_particle_sphere_collision_pairs = [&] (int nvi) { - if(vi >= nvi) - return; - - auto npos = verts.pack(dim_c<3>,xtag,nvi); - // auto nppos = verts.pack(dim_c<3>,pxtag,nvi); - // auto nvel = npos - nppos; - - auto nrpos = verts.pack(dim_c<3>,Xtag,nvi); - auto nr = verts(ptag,nvi); - - auto rdist = (rpos - nrpos).norm(); - if(rdist < r + nr) - return; - - auto dist = (pos - npos).norm(); - if(dist > r + nr) - return; - - // find a collision pairs - csPT.insert(zs::vec{vi,nvi}); - }; - - spBvh.iter_neighbors(bv,find_particle_sphere_collision_pairs); - }); - - // std::cout << "csPT.size() = " << csPT.size() << std::endl; - - auto w = get_input2("relaxation_strength"); - // process collision pairs - cudaPol(zip(zs::range(csPT.size()),csPT._activeKeys),[ - verts = proxy({},verts), - xtag = zs::SmallString(xtag), - ptag = zs::SmallString(ptag), - pxtag = zs::SmallString(pxtag), - dp_buffer = proxy(dp_buffer), - dp_count = proxy(dp_count), - exec_tag, - friction] ZS_LAMBDA(auto,const auto& pair) mutable { - auto vi = pair[0]; - auto nvi = pair[1]; - auto p0 = verts.pack(dim_c<3>,xtag,vi); - auto p1 = verts.pack(dim_c<3>,xtag,nvi); - auto vel0 = p0 - verts.pack(dim_c<3>,pxtag,vi); - auto vel1 = p1 - verts.pack(dim_c<3>,pxtag,nvi); - - auto nrm = (p0 - p1).normalized(); - - auto r = verts(ptag,vi) + verts(ptag,nvi); - - float minv0 = verts("minv",vi); - float minv1 = verts("minv",nvi); - - vec3 dp0{},dp1{}; - if(!CONSTRAINT::solve_DistanceConstraint( - p0,minv0, - p1,minv1, - r, - (float)1, - dp0,dp1)) { - // auto rel_vel = vel0 - vel1 + dp0 - dp1; - // auto tan_vel = rel_vel - nrm * rel_vel.dot(nrm); - // auto tan_len = tan_vel.norm(); - // auto max_tan_len = (dp0 - dp1).norm() * friction; - - // if(tan_len > (T)1e-6) { - // auto ratio = (T)max_tan_len / (T)tan_len; - // auto alpha = zs::min(ratio,(T)1.0); - - // dp = -tan_vel * zs::min(alpha,(T)1.0); - // pred += dp; - // } - - for(int i = 0;i != 3;++i) - atomic_add(exec_tag,&dp_buffer[vi * 3 + i], dp0[i]); - for(int i = 0;i != 3;++i) - atomic_add(exec_tag,&dp_buffer[nvi * 3 + i],dp1[i]); - - atomic_add(exec_tag,&dp_count[vi],(int)1); - atomic_add(exec_tag,&dp_count[nvi],(int)1); - } - }); - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - xtag = zs::SmallString(xtag), - w, - dp_count = proxy(dp_count), - dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { - if(dp_count[vi] > 0) { - auto dp = vec3{dp_buffer[vi * 3 + 0],dp_buffer[vi * 3 + 1],dp_buffer[vi * 3 + 2]}; - verts.tuple(dim_c<3>,xtag,vi) = verts.pack(dim_c<3>,xtag,vi) + w * dp / (T)dp_count[vi]; - } - }); - - set_output("zsparticles",zsparticles); - } -}; - -ZENDEFNODE(XPBDParticlesCollider, {{{"zsparticles"}, - {"float","relaxation_strength","1"}, - {"string","xtag","x"}, - {"string","pxtag","px"}, - {"string","Xtag","X"}, - {"string","pscaleTag","pscale"}, - {"float","friction","0"} - }, - {{"zsparticles"}}, - { - // {"string","ptag","x"} - }, - {"PBD"}}); - -struct XPBDSolveSmooth : INode { - - virtual void apply() override { - using namespace zs; - using namespace PBD_CONSTRAINT; - - using vec3 = zs::vec; - using vec2i = zs::vec; - using vec3i = zs::vec; - using vec4i = zs::vec; - using mat4 = zs::vec; - - constexpr auto space = execspace_e::cuda; - auto cudaPol = cuda_exec(); - constexpr auto exec_tag = wrapv{}; - - auto zsparticles = get_input("zsparticles"); - - auto all_constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "all_constraints"); - auto ptag = get_param("ptag"); - auto w = get_input2("relaxation_strength"); - - auto& verts = zsparticles->getParticles(); - - zs::Vector dp_buffer{verts.get_allocator(),verts.size() * 3}; - cudaPol(zs::range(dp_buffer),[]ZS_LAMBDA(auto& v) {v = 0;}); - zs::Vector dp_count{verts.get_allocator(),verts.size()}; - cudaPol(zs::range(dp_count),[]ZS_LAMBDA(auto& c) {c = 0;}); - - for(auto &&constraints : all_constraints) { - const auto& cquads = constraints->getQuadraturePoints(); - auto category = constraints->readMeta(CONSTRAINT_KEY,wrapt{}); - - cudaPol(zs::range(cquads.size()),[ - verts = proxy({},verts), - category, - // dt, - // w, - exec_tag, - dp_buffer = proxy(dp_buffer), - dp_count = proxy(dp_count), - ptag = zs::SmallString(ptag), - cquads = proxy({},cquads)] ZS_LAMBDA(int ci) mutable { - float s = cquads("stiffness",ci); - float lambda = cquads("lambda",ci); - - if(category == category_c::edge_length_constraint) { - auto edge = cquads.pack(dim_c<2>,"inds",ci,int_c); - vec3 p0{},p1{}; - p0 = verts.pack(dim_c<3>,ptag,edge[0]); - p1 = verts.pack(dim_c<3>,ptag,edge[1]); - float minv0 = verts("minv",edge[0]); - float minv1 = verts("minv",edge[1]); - float r = cquads("r",ci); - - vec3 dp0{},dp1{}; - if(!CONSTRAINT::solve_DistanceConstraint( - p0,minv0, - p1,minv1, - r, - (float)1, - dp0,dp1)) { - - for(int i = 0;i != 3;++i) - atomic_add(exec_tag,&dp_buffer[edge[0] * 3 + i],dp0[i]); - for(int i = 0;i != 3;++i) - atomic_add(exec_tag,&dp_buffer[edge[1] * 3 + i],dp1[i]); - - atomic_add(exec_tag,&dp_count[edge[0]],(int)1); - atomic_add(exec_tag,&dp_count[edge[1]],(int)1); - } - } - if(category == category_c::isometric_bending_constraint) { - return; - auto quad = cquads.pack(dim_c<4>,"inds",ci,int_c); - vec3 p[4] = {}; - float minv[4] = {}; - for(int i = 0;i != 4;++i) { - p[i] = verts.pack(dim_c<3>,ptag,quad[i]); - minv[i] = verts("minv",quad[i]); - } - - auto Q = cquads.pack(dim_c<4,4>,"Q",ci); - - vec3 dp[4] = {}; - float lambda = 0; - CONSTRAINT::solve_IsometricBendingConstraint( - p[0],minv[0], - p[1],minv[1], - p[2],minv[2], - p[3],minv[3], - Q, - (float)1, - dp[0], - dp[1], - dp[2], - dp[3]); - - auto has_nan = false; - for(int i = 0;i != 4;++i) - if(zs::isnan(dp[i].norm())) - has_nan = true; - if(has_nan) { - printf("nan dp detected : %f %f %f %f %f %f %f %f\n", - (float)p[0].norm(), - (float)p[1].norm(), - (float)p[2].norm(), - (float)p[3].norm(), - (float)dp[0].norm(), - (float)dp[1].norm(), - (float)dp[2].norm(), - (float)dp[3].norm()); - return; - } - for(int i = 0;i != 4;++i) - for(int j = 0;j != 3;++j) - atomic_add(exec_tag,&dp_buffer[quad[i] * 3 + j],dp[i][j]); - for(int i = 0;i != 4;++i) - atomic_add(exec_tag,&dp_count[quad[i]],(int)1); - } - }); - } - - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - ptag = zs::SmallString(ptag),w, - dp_count = proxy(dp_count), - dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { - if(dp_count[vi] > 0) { - auto dp = w * vec3{dp_buffer[vi * 3 + 0],dp_buffer[vi * 3 + 1],dp_buffer[vi * 3 + 2]}; - verts.tuple(dim_c<3>,ptag,vi) = verts.pack(dim_c<3>,ptag,vi) + dp / (T)dp_count[vi]; - } - }); - - // set_output("all_constraints",all_constraints); - set_output("zsparticles",zsparticles); - }; -}; - -ZENDEFNODE(XPBDSolveSmooth, {{{"zsparticles"},{"all_constraints"},{"float","relaxation_strength","1"}}, - {{"zsparticles"}}, - {{"string","ptag","x"}}, - {"PBD"}}); - - - - - - -}; \ No newline at end of file diff --git a/projects/CuLagrange/pbd/SelfCollision.cu b/projects/CuLagrange/pbd/SelfCollision.cu new file mode 100644 index 0000000000..f9c5b22933 --- /dev/null +++ b/projects/CuLagrange/pbd/SelfCollision.cu @@ -0,0 +1,820 @@ +#include "Structures.hpp" +#include "zensim/Logger.hpp" +#include "zensim/cuda/execution/ExecutionPolicy.cuh" +#include "zensim/omp/execution/ExecutionPolicy.hpp" +#include "zensim/io/MeshIO.hpp" +#include "zensim/math/bit/Bits.h" +#include "zensim/types/Property.h" +#include +#include +#include +#include +#include +#include + +// #include "constraint_function_kernel/constraint.cuh" +#include "../geometry/kernel/tiled_vector_ops.hpp" +#include "../geometry/kernel/topology.hpp" +#include "../geometry/kernel/geo_math.hpp" +#include "../fem/collision_energy/evaluate_collision.hpp" +// #include "constraint_function_kernel/constraint_types.hpp" + +namespace zeno { + +struct DetangleImminentCollision : INode { + using T = float; + using vec3 = zs::vec; + using dtiles_t = zs::TileVector; + + virtual void apply() override { + using namespace zs; + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + // constexpr auto exec_tag = wrapv{}; + constexpr auto eps = (T)1e-7; + + auto zsparticles = get_input("zsparticles"); + auto current_x_tag = get_input2("current_x_tag"); + auto pre_x_tag = get_input2("previous_x_tag"); + auto repel_strength = get_input2("repeling_strength"); + auto imminent_collision_thickness = get_input2("immc_thickness"); + // apply impulse for imminent collision for previous configuration + auto& verts = zsparticles->getParticles(); + const auto& tris = zsparticles->getQuadraturePoints(); + const auto& halfedges = (*zsparticles)[ZenoParticles::s_surfHalfEdgeTag]; + const auto &edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + + if(!verts.hasProperty("imminent_fail")) + verts.append_channels(cudaPol,{{"imminent_fail",1}}); + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts)] ZS_LAMBDA(int vi) mutable {verts("imminent_fail",vi) = (T)0;}); + + dtiles_t vtemp{verts.get_allocator(),{ + {"x",3}, + {"v",3}, + {"minv",1} + },verts.size()}; + TILEVEC_OPS::copy(cudaPol,verts,pre_x_tag,vtemp,"x"); + TILEVEC_OPS::copy(cudaPol,verts,"minv",vtemp,"minv"); + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + current_x_tag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { + vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - vtemp.pack(dim_c<3>,"x",vi); + }); + // TILEVEC_OPS::copy(cudaPol,verts,current_x_tag,vtemp,"v"); + // TILEVEC_OPS::add(cudaPol,vtemp,"v",1,"x",-1,"v"); + dtiles_t imminent_collision_buffer(verts.get_allocator(), + { + {"inds",4}, + {"bary",4}, + {"impulse",3}, + {"collision_normal",3} + },(size_t)0); + + auto imminent_restitution_rate = get_input2("imm_restitution"); + auto imminent_relaxation_rate = get_input2("imm_relaxation"); + + auto nm_iters = get_input2("nm_imminent_iters"); + + auto do_pt_detection = get_input2("use_PT"); + auto do_ee_detection = get_input2("use_EE"); + + zs::Vector nm_imminent_collision{verts.get_allocator(),(size_t)1}; + + std::cout << "do imminent detangle" << std::endl; + for(int it = 0;it != nm_iters;++it) { + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts)] ZS_LAMBDA(int vi) mutable {verts("imminent_fail",vi) = (T)0;}); + + // we use collision cell as the collision volume, PT collision is enough prevent penertation? + if(do_pt_detection) { + COLLISION_UTILS::calc_imminent_self_PT_collision_impulse(cudaPol, + vtemp,"x","v", + tris, + halfedges, + imminent_collision_thickness, + 0, + imminent_collision_buffer); + // std::cout << "nm_imminent_PT_collision : " << imminent_collision_buffer.size() << std::endl; + } + + if(do_ee_detection) { + COLLISION_UTILS::calc_imminent_self_EE_collision_impulse(cudaPol, + vtemp,"x","v", + edges, + imminent_collision_thickness, + imminent_collision_buffer.size(), + imminent_collision_buffer); + // std::cout << "nm_imminent_EE_collision : " << imminent_collision_buffer.size() << std::endl; + } + // resolve imminent PT collision + + // impulse_norm = TILEVEC_OPS::dot<3>(cudaPol,imminent_collision_buffer,"impulse","impulse"); + // std::cout << "EE_PT_impulse_norm : " << impulse_norm << std::endl; + + COLLISION_UTILS::apply_impulse(cudaPol, + vtemp,"v", + imminent_restitution_rate, + imminent_relaxation_rate, + imminent_collision_buffer); + + + auto add_repulsion_force = get_input2("add_repulsion_force"); + // add an impulse to repel the pair further + + nm_imminent_collision.setVal(0); + // if(add_repulsion_force) { + // std::cout << "add imminent replering force" << std::endl; + auto max_repel_distance = get_input2("max_repel_distance"); + + cudaPol(zs::range(imminent_collision_buffer.size()),[ + imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(int ci) mutable { + imminent_collision_buffer.tuple(dim_c<3>,"impulse",ci) = vec3::zeros(); + }); + + cudaPol(zs::range(imminent_collision_buffer.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + eps = eps, + exec_tag = wrapv{}, + k = repel_strength, + max_repel_distance = max_repel_distance, + thickness = imminent_collision_thickness, + nm_imminent_collision = proxy(nm_imminent_collision), + imminent_collision_buffer = proxy({},imminent_collision_buffer)] ZS_LAMBDA(auto id) mutable { + auto inds = imminent_collision_buffer.pack(dim_c<4>,"inds",id,int_c); + auto bary = imminent_collision_buffer.pack(dim_c<4>,"bary",id); + + vec3 ps[4] = {}; + vec3 vs[4] = {}; + auto vr = vec3::zeros(); + auto pr = vec3::zeros(); + for(int i = 0;i != 4;++i) { + ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); + vs[i] = vtemp.pack(dim_c<3>,"v",inds[i]); + pr += bary[i] * ps[i]; + vr += bary[i] * vs[i]; + } + + auto dist = pr.norm(); + vec3 collision_normal = imminent_collision_buffer.pack(dim_c<3>,"collision_normal",id); + + if(dist > thickness) + return; + + auto d = thickness - dist; + auto vn = vr.dot(collision_normal); + if(vn < -thickness * 0.05) { + atomic_add(exec_tag,&nm_imminent_collision[0],1); + for(int i = 0;i != 4;++i) + verts("imminent_fail",inds[i]) = (T)1.0; + } + if(vn > (T)max_repel_distance * d || d < 0) { + // if with current velocity, the collided particles can be repeled by more than 1% of collision depth, no extra repulsion is needed + return; + } else { + // make sure the collided particles is seperated by 1% of collision depth + // assume the particles has the same velocity + auto I = k * d; + auto I_max = (max_repel_distance * d - vn); + I = I_max < I ? I_max : I; + auto impulse = (T)I * collision_normal; + + imminent_collision_buffer.tuple(dim_c<3>,"impulse",id) = impulse; + } + }); + std::cout << "nm_imminent_collision : " << nm_imminent_collision.getVal(0) << std::endl; + if(nm_imminent_collision.getVal(0) == 0) + break; + + + // auto impulse_norm = TILEVEC_OPS::dot<3>(cudaPol,imminent_collision_buffer,"impulse","impulse"); + // std::cout << "REPEL_impulse_norm : " << impulse_norm << std::endl; + + COLLISION_UTILS::apply_impulse(cudaPol, + vtemp,"v", + imminent_restitution_rate, + imminent_relaxation_rate, + imminent_collision_buffer); + // } + } + + std::cout << "finish imminent collision" << std::endl; + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + current_x_tag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { + verts.tuple(dim_c<3>,current_x_tag,vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); + }); + + // std::cout << "finish apply imminent impulse" << std::endl; + + set_output("zsparticles",zsparticles); + } +}; + +ZENDEFNODE(DetangleImminentCollision, {{{"zsparticles"}, + {"string","current_x_tag","x"}, + {"string","previous_x_tag","px"}, + // {"string","pscaleTag","pscale"}, + {"float","repeling_strength","1.0"}, + {"float","immc_thickness","0.01"}, + {"int","nm_imminent_iters","1"}, + {"float","imm_restitution","0.1"}, + {"float","imm_relaxation","0.25"}, + {"float","max_repel_distance","0.1"}, + {"bool","add_repulsion_force","0"}, + {"bool","use_PT","1"}, + {"bool","use_EE","1"}, + }, + {{"zsparticles"}}, + {}, + {"PBD"}}); + + +struct VisualizeImminentCollision : INode { + using T = float; + using vec3 = zs::vec; + using dtiles_t = zs::TileVector; + + virtual void apply() override { + using namespace zs; + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + auto ompPol = omp_exec(); + constexpr auto omp_space = execspace_e::openmp; + + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto exec_tag = wrapv{}; + constexpr auto eps = (T)1e-7; + constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; + + auto zsparticles = get_input("zsparticles"); + auto current_x_tag = get_input2("current_x_tag"); + auto pre_x_tag = get_input2("previous_x_tag"); + auto imminent_collision_thickness = get_input2("immc_thickness"); + + // apply impulse for imminent collision for previous configuration + auto& verts = zsparticles->getParticles(); + const auto& tris = zsparticles->getQuadraturePoints(); + const auto& edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + const auto& halfedges = (*zsparticles)[ZenoParticles::s_surfHalfEdgeTag]; + + // auto nm_imminent_iters = get_input2("nm_imminent_iters"); + dtiles_t imminent_PT_collision_buffer(verts.get_allocator(), + { + {"inds",4}, + {"bary",4}, + {"impulse",3}, + {"collision_normal",3} + },(size_t)0); + + dtiles_t imminent_EE_collision_buffer(verts.get_allocator(), + { + {"inds",4}, + {"bary",4}, + {"impulse",3}, + {"collision_normal",3} + },(size_t)0); + // zs::bht csPT{verts.get_allocator(),MAX_PT_COLLISION_PAIRS}; + // csPT.reset(cudaPol,true); + + dtiles_t vtemp(verts.get_allocator(),{ + {"x",3}, + {"v",3}, + {"minv",1} + },verts.size()); + TILEVEC_OPS::copy(cudaPol,verts,pre_x_tag,vtemp,"x"); + TILEVEC_OPS::copy(cudaPol,verts,"minv",vtemp,"minv"); + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + current_x_tag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { + vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - vtemp.pack(dim_c<3>,"x",vi); + }); + // we use collision cell as the collision volume, PT collision is enough prevent penertation? + COLLISION_UTILS::calc_imminent_self_PT_collision_impulse(cudaPol, + vtemp,"x","v", + tris, + halfedges, + imminent_collision_thickness, + 0, + imminent_PT_collision_buffer); + std::cout << "nm_PT_collision : " << imminent_PT_collision_buffer.size() << std::endl; + + COLLISION_UTILS::calc_imminent_self_EE_collision_impulse(cudaPol, + vtemp,"x","v", + edges, + imminent_collision_thickness, + 0, + imminent_EE_collision_buffer); + std::cout << "nm_EE_collision : " << imminent_EE_collision_buffer.size() << std::endl; + // resolve imminent PT collision + + dtiles_t imminent_PT_collision_vis(verts.get_allocator(),{ + {"collision_normal",3}, + {"pt",3}, + {"pp",3}},imminent_PT_collision_buffer.size()); + + cudaPol(zs::range(imminent_PT_collision_buffer.size()),[ + vtemp = proxy({},vtemp), + imminent_PT_collision_vis = proxy({},imminent_PT_collision_vis), + imminent_PT_collision_buffer = proxy({},imminent_PT_collision_buffer)] ZS_LAMBDA(auto ci) mutable { + auto inds = imminent_PT_collision_buffer.pack(dim_c<4>,"inds",ci,int_c); + auto bary = imminent_PT_collision_buffer.pack(dim_c<4>,"bary",ci); + + vec3 ps[4] = {}; + for(int i = 0;i != 4;++i) + ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); + + auto proj_t = vec3::zeros(); + auto pr = vec3::zeros(); + for(int i = 0;i != 4;++i) + pr += bary[i] * ps[i]; + + for(int i = 0;i != 3;++i) + proj_t -= bary[i] * ps[i]; + + auto collision_normal = pr.normalized(); + + imminent_PT_collision_vis.tuple(dim_c<3>,"collision_normal",ci) = collision_normal; + + imminent_PT_collision_vis.tuple(dim_c<3>,"pt",ci) = proj_t; + imminent_PT_collision_vis.tuple(dim_c<3>,"pp",ci) = vtemp.pack(dim_c<3>,"x",inds[3]); + }); + imminent_PT_collision_vis = imminent_PT_collision_vis.clone({zs::memsrc_e::host}); + + auto prim_PT = std::make_shared(); + auto& vis_verts_PT = prim_PT->verts; + auto& vis_lines_PT = prim_PT->lines; + vis_verts_PT.resize(imminent_PT_collision_buffer.size() * 2); + vis_lines_PT.resize(imminent_PT_collision_buffer.size()); + + auto nrm_prim_PT = std::make_shared(); + auto& nrm_verts_PT = nrm_prim_PT->verts; + auto& nrm_lines_PT = nrm_prim_PT->lines; + nrm_verts_PT.resize(imminent_PT_collision_buffer.size() * 2); + nrm_lines_PT.resize(imminent_PT_collision_buffer.size()); + + ompPol(zs::range(imminent_PT_collision_buffer.size()),[ + &vis_verts_PT,&vis_lines_PT, + &nrm_verts_PT,&nrm_lines_PT, + imminent_PT_collision_vis = proxy({},imminent_PT_collision_vis)] (int ci) mutable { + auto cnrm = imminent_PT_collision_vis.pack(dim_c<3>,"collision_normal",ci); + auto pt = imminent_PT_collision_vis.pack(dim_c<3>,"pt",ci); + auto pp = imminent_PT_collision_vis.pack(dim_c<3>,"pp",ci); + auto pn = pp + cnrm; + + vis_verts_PT[ci * 2 + 0] = pp.to_array(); + vis_verts_PT[ci * 2 + 1] = pt.to_array(); + vis_lines_PT[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + + nrm_verts_PT[ci * 2 + 0] = pp.to_array(); + nrm_verts_PT[ci * 2 + 1] = pn.to_array(); + nrm_lines_PT[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + }); + + set_output("prim_PT",std::move(prim_PT)); + set_output("cnrm_PT",std::move(nrm_prim_PT)); + + std::cout << "output PT VIS" << std::endl; + + + dtiles_t imminent_EE_collision_vis(verts.get_allocator(),{ + {"collision_normal",3}, + {"pt",3}, + {"pp",3}},imminent_EE_collision_buffer.size()); + + cudaPol(zs::range(imminent_EE_collision_buffer.size()),[ + vtemp = proxy({},vtemp), + imminent_EE_collision_vis = proxy({},imminent_EE_collision_vis), + imminent_EE_collision_buffer = proxy({},imminent_EE_collision_buffer)] ZS_LAMBDA(auto ci) mutable { + auto inds = imminent_EE_collision_buffer.pack(dim_c<4>,"inds",ci,int_c); + auto bary = imminent_EE_collision_buffer.pack(dim_c<4>,"bary",ci); + + vec3 ps[4] = {}; + for(int i = 0;i != 4;++i) + ps[i] = vtemp.pack(dim_c<3>,"x",inds[i]); + + auto pr = vec3::zeros(); + for(int i = 0;i != 4;++i) + pr += bary[i] * ps[i]; + + auto proja = vec3::zeros(); + auto projb = vec3::zeros(); + for(int i = 0;i != 2;++i) { + proja -= bary[i] * ps[i]; + projb += bary[i + 2] * ps[i + 2]; + } + + auto collision_normal = pr.normalized(); + + imminent_EE_collision_vis.tuple(dim_c<3>,"collision_normal",ci) = collision_normal; + imminent_EE_collision_vis.tuple(dim_c<3>,"pt",ci) = proja; + imminent_EE_collision_vis.tuple(dim_c<3>,"pp",ci) = projb; + }); + imminent_EE_collision_vis = imminent_EE_collision_vis.clone({zs::memsrc_e::host}); + + std::cout << "output EE VIS" << std::endl; + + auto prim_EE = std::make_shared(); + auto& vis_verts_EE = prim_EE->verts; + auto& vis_lines_EE = prim_EE->lines; + vis_verts_EE.resize(imminent_EE_collision_buffer.size() * 2); + vis_lines_EE.resize(imminent_EE_collision_buffer.size()); + + auto nrm_prim_EE = std::make_shared(); + auto& nrm_verts_EE = nrm_prim_EE->verts; + auto& nrm_lines_EE = nrm_prim_EE->lines; + nrm_verts_EE.resize(imminent_EE_collision_buffer.size() * 2); + nrm_lines_EE.resize(imminent_EE_collision_buffer.size()); + + ompPol(zs::range(imminent_EE_collision_buffer.size()),[ + &vis_verts_EE,&vis_lines_EE, + &nrm_verts_EE,&nrm_lines_EE, + imminent_EE_collision_vis = proxy({},imminent_EE_collision_vis)] (int ci) mutable { + auto cnrm = imminent_EE_collision_vis.pack(dim_c<3>,"collision_normal",ci); + auto pt = imminent_EE_collision_vis.pack(dim_c<3>,"pt",ci); + auto pp = imminent_EE_collision_vis.pack(dim_c<3>,"pp",ci); + auto pn = pp + cnrm; + + vis_verts_EE[ci * 2 + 0] = pp.to_array(); + vis_verts_EE[ci * 2 + 1] = pt.to_array(); + vis_lines_EE[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + + nrm_verts_EE[ci * 2 + 0] = pp.to_array(); + nrm_verts_EE[ci * 2 + 1] = pn.to_array(); + nrm_lines_EE[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; + }); + + set_output("prim_EE",std::move(prim_EE)); + set_output("cnrm_EE",std::move(nrm_prim_EE)); + + } +}; + +ZENDEFNODE(VisualizeImminentCollision, {{{"zsparticles"}, + {"string","current_x_tag","x"}, + {"string","previous_x_tag","px"}, + // {"string","pscaleTag","pscale"}, + // {"float","repeling_strength","1.0"}, + {"float","immc_thickness","0.01"}, + // {"int","nm_imminent_iters","1"}, + // {"float","imm_restitution","0.1"}, + // {"float","imm_relaxation","0.25"}, + // {"bool","add_repulsion_force","0"}, + }, + {{"prim_PT"},{"cnrm_PT"},{"prim_EE"},{"cnrm_EE"}}, + {}, + {"PBD"}}); + + +struct DetangleCCDCollision : INode { + using T = float; + using vec3 = zs::vec; + using vec4 = zs::vec; + using vec4i = zs::vec; + using dtiles_t = zs::TileVector; + + virtual void apply() override { + using namespace zs; + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + using lbvh_t = typename ZenoLinearBvh::lbvh_t; + using bv_t = typename ZenoLinearBvh::lbvh_t::Box; + constexpr auto exec_tag = wrapv{}; + constexpr auto eps = (T)1e-7; + // constexpr auto MAX_PT_COLLISION_PAIRS = 1000000; + + auto zsparticles = get_input("zsparticles"); + auto current_x_tag = get_input2("current_x_tag"); + auto pre_x_tag = get_input2("previous_x_tag"); + + auto restitution_rate = get_input2("restitution"); + auto relaxation_rate = get_input2("relaxation"); + + auto thickness = get_input2("thickness"); + + auto& verts = zsparticles->getParticles(); + const auto& tris = zsparticles->getQuadraturePoints(); + const auto &edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + + dtiles_t vtemp(verts.get_allocator(),{ + {"x",3}, + {"v",3}, + {"minv",1} + },(size_t)verts.size()); + + auto nm_ccd_iters = get_input2("nm_ccd_iters"); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + current_x_tag = zs::SmallString(current_x_tag), + pre_x_tag = zs::SmallString(pre_x_tag)] ZS_LAMBDA(int vi) mutable { + vtemp.tuple(dim_c<3>,"x",vi) = verts.pack(dim_c<3>,pre_x_tag,vi); + vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - verts.pack(dim_c<3>,pre_x_tag,vi); + vtemp("minv",vi) = verts("minv",vi); + }); + + + lbvh_t triBvh{},eBvh{}; + + zs::Vector impulse_buffer{verts.get_allocator(),verts.size()}; + zs::Vector impulse_count{verts.get_allocator(),verts.size()}; + + auto do_ee_detection = get_input2("do_ee_detection"); + auto do_pt_detection = get_input2("do_pt_detection"); + + zs::Vector nm_ccd_collision{verts.get_allocator(),1}; + + std::cout << "resolve continous collision " << std::endl; + + for(int iter = 0;iter != nm_ccd_iters;++iter) { + + cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); + cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + + nm_ccd_collision.setVal(0); + + if(do_pt_detection) { + auto do_bvh_refit = iter > 0; + COLLISION_UTILS::calc_continous_self_PT_collision_impulse(cudaPol, + verts, + vtemp,"x","v", + tris, + // thickness, + triBvh, + do_bvh_refit, + impulse_buffer, + impulse_count); + } + + if(do_ee_detection) { + auto do_bvh_refit = iter > 0; + COLLISION_UTILS::calc_continous_self_EE_collision_impulse(cudaPol, + verts, + vtemp,"x","v", + edges, + eBvh, + do_bvh_refit, + impulse_buffer, + impulse_count); + } + + // std::cout << "apply impulse" << std::endl; + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count), + relaxation_rate = relaxation_rate, + nm_ccd_collision = proxy(nm_ccd_collision), + eps = eps, + thickness = thickness, + exec_tag = exec_tag] ZS_LAMBDA(int vi) mutable { + if(impulse_count[vi] == 0) + return; + if(impulse_buffer[vi].norm() < eps) + return; + + auto impulse = relaxation_rate * impulse_buffer[vi] / impulse_count[vi]; + if(impulse.norm() > thickness * 0.01) + atomic_add(exec_tag,&nm_ccd_collision[0],1); + + // auto dv = impulse + vtemp.tuple(dim_c<3>,"v",vi) = vtemp.pack(dim_c<3>,"v",vi) + impulse; + // for(int i = 0;i != 3;++i) + // atomic_add(exec_tag,&vtemp("v",i,vi),dv[i]); + }); + + std::cout << "nm_ccd_collision : " << nm_ccd_collision.getVal() << std::endl; + if(nm_ccd_collision.getVal() == 0) + break; + } + std::cout << "finish solving continous collision " << std::endl; + cudaPol(zs::range(verts.size()),[ + vtemp = proxy({},vtemp), + verts = proxy({},verts), + xtag = zs::SmallString(current_x_tag)] ZS_LAMBDA(int vi) mutable { + verts.tuple(dim_c<3>,xtag,vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); + }); + + set_output("zsparticles",zsparticles); + } +}; + +ZENDEFNODE(DetangleCCDCollision, {{{"zsparticles"}, + {"string","current_x_tag","x"}, + {"string","previous_x_tag","px"}, + {"int","nm_ccd_iters","1"}, + // {"string","pscaleTag","pscale"}, + {"float","thickness","0.1"}, + // {"float","immc_thickness","0.01"}, + // {"int","nm_imminent_iters","1"}, + {"float","restitution","0.1"}, + {"float","relaxation","1"}, + {"bool","do_ee_detection","1"}, + {"bool","do_pt_detection","1"}, + // {"bool","add_repulsion_force","0"}, + }, + {{"zsparticles"}}, + {}, + {"PBD"}}); + + +struct VisualizeContinousCollision : INode { + using T = float; + using vec3 = zs::vec; + using dtiles_t = zs::TileVector; + + virtual void apply() override { + using namespace zs; + using lbvh_t = typename ZenoLinearBvh::lbvh_t; + constexpr auto cuda_space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + + constexpr auto omp_space = execspace_e::openmp; + auto ompPol = omp_exec(); + + auto zsparticles = get_input("zsparticles"); + auto current_x_tag = get_input2("current_x_tag"); + auto pre_x_tag = get_input2("previous_x_tag"); + + auto thickness = get_input2("thickness"); + + auto& verts = zsparticles->getParticles(); + const auto& tris = zsparticles->getQuadraturePoints(); + const auto &edges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; + + dtiles_t vtemp(verts.get_allocator(),{ + {"x",3}, + {"v",3}, + {"minv",1} + },(size_t)verts.size()); + + lbvh_t triBvh{},eBvh{}; + // auto nm_ccd_iters = get_input2("nm_ccd_iters"); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + vtemp = proxy({},vtemp), + current_x_tag = zs::SmallString(current_x_tag), + pre_x_tag = zs::SmallString(pre_x_tag)] ZS_LAMBDA(int vi) mutable { + vtemp.tuple(dim_c<3>,"x",vi) = verts.pack(dim_c<3>,pre_x_tag,vi); + vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,current_x_tag,vi) - verts.pack(dim_c<3>,pre_x_tag,vi); + vtemp("minv",vi) = verts("minv",vi); + }); + + zs::Vector impulse_buffer{verts.get_allocator(),verts.size()}; + zs::Vector impulse_count{verts.get_allocator(),verts.size()}; + + cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); + cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + + COLLISION_UTILS::calc_continous_self_PT_collision_impulse(cudaPol, + verts, + vtemp,"x","v", + tris, + // thickness, + triBvh, + false, + impulse_buffer, + impulse_count); + + cudaPol(zs::range(impulse_buffer.size()),[ + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count)] ZS_LAMBDA(int vi) mutable { + if(impulse_count[vi] > 0) { + impulse_buffer[vi] = impulse_buffer[vi] / (T)impulse_count[vi]; + // printf("impulse[%d] : %f %f %f\n",vi + // ,(float)impulse_buffer[vi][0] + // ,(float)impulse_buffer[vi][1] + // ,(float)impulse_buffer[vi][2]); + } + }); + + dtiles_t ccd_PT_collision_vis(verts.get_allocator(),{ + {"impulse",3}, + {"p0",3}, + {"p1",3}, + {"pp",3} + },verts.size()); + + cudaPol(zs::range(verts.size()),[ + ccd_PT_collision_vis = proxy({},ccd_PT_collision_vis), + impulse_buffer = proxy(impulse_buffer), + vtemp = proxy({},vtemp)] ZS_LAMBDA(int vi) mutable { + ccd_PT_collision_vis.tuple(dim_c<3>,"impulse",vi) = impulse_buffer[vi]; + ccd_PT_collision_vis.tuple(dim_c<3>,"p0",vi) = vtemp.pack(dim_c<3>,"x",vi); + ccd_PT_collision_vis.tuple(dim_c<3>,"p1",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); + ccd_PT_collision_vis.tuple(dim_c<3>,"pp",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi) + impulse_buffer[vi]; + }); + + ccd_PT_collision_vis = ccd_PT_collision_vis.clone({zs::memsrc_e::host}); + auto prim_PT = std::make_shared(); + auto& vis_verts_PT = prim_PT->verts; + auto& vis_lines_PT = prim_PT->lines; + vis_verts_PT.resize(verts.size() * 2); + vis_lines_PT.resize(verts.size()); + + auto scale = get_input2("scale"); + + ompPol(zs::range(verts.size()),[ + &vis_verts_PT,&vis_lines_PT, + scale = scale, + ccd_PT_collision_vis = proxy({},ccd_PT_collision_vis)] (int vi) mutable { + auto p = ccd_PT_collision_vis.pack(dim_c<3>,"p0",vi); + auto impulse = ccd_PT_collision_vis.pack(dim_c<3>,"impulse",vi); + auto pc = p + impulse * scale; + vis_verts_PT[vi * 2 + 0] = p.to_array(); + vis_verts_PT[vi * 2 + 1] = pc.to_array(); + vis_lines_PT[vi] = zeno::vec2i{vi * 2 + 0,vi * 2 + 1}; + }); + + set_output("impulse_PT",std::move(prim_PT)); + + cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); + cudaPol(zs::range(impulse_count),[]ZS_LAMBDA(auto& c) mutable {c = 0;}); + + COLLISION_UTILS::calc_continous_self_EE_collision_impulse(cudaPol, + verts, + vtemp,"x","v", + edges, + eBvh, + false, + impulse_buffer, + impulse_count); + + cudaPol(zs::range(impulse_buffer.size()),[ + impulse_buffer = proxy(impulse_buffer), + impulse_count = proxy(impulse_count)] ZS_LAMBDA(int vi) mutable { + if(impulse_count[vi] > 0) { + impulse_buffer[vi] = impulse_buffer[vi] / (T)impulse_count[vi]; + // printf("impulse[%d] : %f %f %f\n",vi + // ,(float)impulse_buffer[vi][0] + // ,(float)impulse_buffer[vi][1] + // ,(float)impulse_buffer[vi][2]); + } + }); + + dtiles_t ccd_EE_collision_vis(verts.get_allocator(),{ + {"impulse",3}, + {"p0",3}, + {"p1",3}, + {"pp",3} + },verts.size()); + + cudaPol(zs::range(verts.size()),[ + ccd_EE_collision_vis = proxy({},ccd_EE_collision_vis), + impulse_buffer = proxy(impulse_buffer), + vtemp = proxy({},vtemp)] ZS_LAMBDA(int vi) mutable { + ccd_EE_collision_vis.tuple(dim_c<3>,"impulse",vi) = impulse_buffer[vi]; + ccd_EE_collision_vis.tuple(dim_c<3>,"p0",vi) = vtemp.pack(dim_c<3>,"x",vi); + ccd_EE_collision_vis.tuple(dim_c<3>,"p1",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); + ccd_EE_collision_vis.tuple(dim_c<3>,"pp",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi) + impulse_buffer[vi]; + }); + + ccd_EE_collision_vis = ccd_EE_collision_vis.clone({zs::memsrc_e::host}); + auto prim_EE = std::make_shared(); + auto& vis_verts_EE = prim_EE->verts; + auto& vis_lines_EE = prim_EE->lines; + vis_verts_EE.resize(verts.size() * 2); + vis_lines_EE.resize(verts.size()); + + ompPol(zs::range(verts.size()),[ + &vis_verts_EE,&vis_lines_EE, + scale = scale, + ccd_EE_collision_vis = proxy({},ccd_EE_collision_vis)] (int vi) mutable { + auto p = ccd_EE_collision_vis.pack(dim_c<3>,"p0",vi); + auto impulse = ccd_EE_collision_vis.pack(dim_c<3>,"impulse",vi); + auto pc = p + impulse * scale; + vis_verts_EE[vi * 2 + 0] = p.to_array(); + vis_verts_EE[vi * 2 + 1] = pc.to_array(); + vis_lines_EE[vi] = zeno::vec2i{vi * 2 + 0,vi * 2 + 1}; + }); + + set_output("impulse_EE",std::move(prim_EE)); + } +}; + +ZENDEFNODE(VisualizeContinousCollision, {{{"zsparticles"}, + {"string","current_x_tag","x"}, + {"string","previous_x_tag","px"}, + // {"string","pscaleTag","pscale"}, + // {"float","repeling_strength","1.0"}, + {"float","thickness","0.01"}, + // {"int","nm_imminent_iters","1"}, + // {"float","imm_restitution","0.1"}, + {"float","scale","0.25"}, + // {"bool","add_repulsion_force","0"}, + }, + {{"impulse_PT"},{"impulse_EE"}}, + {}, + {"PBD"}}); + +}; \ No newline at end of file diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh index 3dda9544ba..615b218861 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh @@ -216,12 +216,19 @@ namespace zeno { namespace CONSTRAINT { const VECTOR3d x[4] = { p2, p3, p0, p1 }; // Q = MATRIX4d::uniform(0); + // const auto e0 = x[1].cast() - x[0].cast(); + // const auto e1 = x[2].cast() - x[0].cast(); + // const auto e2 = x[3].cast() - x[0].cast(); + // const auto e3 = x[2].cast() - x[1].cast(); + // const auto e4 = x[3].cast() - x[1].cast(); + const auto e0 = x[1].cast() - x[0].cast(); const auto e1 = x[2].cast() - x[0].cast(); const auto e2 = x[3].cast() - x[0].cast(); const auto e3 = x[2].cast() - x[1].cast(); const auto e4 = x[3].cast() - x[1].cast(); + // auto e0 = e0_.cast(); // auto e1 = e1_.cast(); // auto e2 = e2_.cast(); @@ -302,8 +309,7 @@ namespace zeno { namespace CONSTRAINT { } // exit early if required - // for(int i = 0;i != 4;++i) - // printf("gradC[%d] : %f %f %f\n",i,(float)gradC[i][0],(float)gradC[i][1],(float)gradC[i][2]); + if (sum_normGradC > eps) { @@ -317,6 +323,15 @@ namespace zeno { namespace CONSTRAINT { corr3 = (delta_lambda * invMass[1]) * gradC[1]; // printf("update corr for iso_bending energy\n"); + auto gradCSum = (float)0; + for(int i = 0;i != 4;++i) + gradCSum += gradC[i].norm(); + if(gradCSum > 1e-4) { + printf("gradC : %f %f %f %f corr : %f %f %f %f\n", + (float)gradC[0].norm(),(float)gradC[1].norm(),(float)gradC[2].norm(),(float)gradC[3].norm(), + (float)corr0.norm(),(float)corr1.norm(),(float)corr2.norm(),(float)corr3.norm()); + } + return true; }else { corr0 = VECTOR3d::uniform(0); From d73848e728e02e148b4048efa10b61c3743a4657 Mon Sep 17 00:00:00 2001 From: Lu Shuliang Date: Fri, 22 Sep 2023 18:53:33 +0800 Subject: [PATCH 21/21] dihedral bending wip --- .../CuLagrange/geometry/kernel/topology.hpp | 1 + projects/CuLagrange/pbd/Constraints.cu | 216 ++++++++++++------ projects/CuLagrange/pbd/SelfCollision.cu | 19 +- .../constraint_function_kernel/constraint.cuh | 178 ++++++++++++--- .../constraint_types.hpp | 1 + 5 files changed, 301 insertions(+), 114 deletions(-) diff --git a/projects/CuLagrange/geometry/kernel/topology.hpp b/projects/CuLagrange/geometry/kernel/topology.hpp index 883952e991..843185142e 100644 --- a/projects/CuLagrange/geometry/kernel/topology.hpp +++ b/projects/CuLagrange/geometry/kernel/topology.hpp @@ -1369,6 +1369,7 @@ namespace zeno { return out_topo; } + // the topos: triA: [idx0,idx2,idx3] triB: [idx1,idx3,idx2] template void retrieve_tri_bending_topology(Pol& pol, const TriTileVec& tris, diff --git a/projects/CuLagrange/pbd/Constraints.cu b/projects/CuLagrange/pbd/Constraints.cu index 79b6baea4a..1e3b501069 100644 --- a/projects/CuLagrange/pbd/Constraints.cu +++ b/projects/CuLagrange/pbd/Constraints.cu @@ -118,6 +118,7 @@ struct MakeSurfaceConstraintTopology : INode { } + // angle on (p2, p3) between triangles (p0, p2, p3) and (p1, p3, p2) if(type == "bending") { constraint->setMeta(CONSTRAINT_KEY,category_c::isometric_bending_constraint); // constraint->category = ZenoParticles::tri_bending_spring; @@ -155,6 +156,40 @@ struct MakeSurfaceConstraintTopology : INode { eles.tuple(dim_c<16>,"Q",oei) = Q; }); } + // angle on (p2, p3) between triangles (p0, p2, p3) and (p1, p3, p2) + if(type == "dihedral") { + constraint->setMeta(CONSTRAINT_KEY,category_c::dihedral_bending_constraint); + + const auto& halfedges = (*source)[ZenoParticles::s_surfHalfEdgeTag]; + + zs::Vector> bd_topos{quads.get_allocator(),0}; + retrieve_tri_bending_topology(cudaPol,quads,halfedges,bd_topos); + + eles.resize(bd_topos.size()); + + topological_coloring(cudaPol,bd_topos,colors); + sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); + // std::cout << "quads.size() = " << quads.size() << "\t" << "edge_topos.size() = " << edge_topos.size() << std::endl; + + eles.append_channels(cudaPol,{{"inds",4},{"ra",1}}); + + cudaPol(zs::range(eles.size()),[ + eles = proxy({},eles), + bd_topos = proxy(bd_topos), + reordered_map = proxy(reordered_map), + verts = proxy({},verts)] ZS_LAMBDA(int oei) mutable { + auto ei = reordered_map[oei]; + eles.tuple(dim_c<4>,"inds",oei) = bd_topos[ei].reinterpret_bits(float_c); + + vec3 x[4] = {}; + for(int i = 0;i != 4;++i) + x[i] = verts.pack(dim_c<3>,"x",bd_topos[ei][i]); + + float alpha{}; + CONSTRAINT::init_DihedralBendingConstraint(x[0],x[1],x[2],x[3],alpha); + eles("ra",oei) = alpha; + }); + } if(type == "kcollision") { using bv_t = typename ZenoLinearBvh::lbvh_t::Box; @@ -326,7 +361,7 @@ struct VisualizePBDConstraint : INode { plines[ci] = zeno::vec2i{ci * 2 + 0,ci * 2 + 1}; ltclrs[ci] = cclrs[ci]; }); - }else if(constraint_type == category_c::isometric_bending_constraint) { + }else if(constraint_type == category_c::isometric_bending_constraint || constraint_type == category_c::dihedral_bending_constraint) { pverts.resize(constraints.size() * 2); auto& plines = prim->lines; plines.resize(constraints.size()); @@ -402,9 +437,6 @@ struct XPBDSolve : INode { for(int g = 0;g != nm_group;++g) { - // if(category == category_c::isometric_bending_constraint) - // break; - auto coffset = coffsets.getVal(g); int group_size = 0; if(g == nm_group - 1) @@ -412,15 +444,6 @@ struct XPBDSolve : INode { else group_size = coffsets.getVal(g + 1) - coffsets.getVal(g); - // cudaPol(zs::range(verts.size()),[ - // ptag = zs::SmallString(ptag), - // verts = proxy({},verts), - // pv_buffer = proxy(pv_buffer)] ZS_LAMBDA(int vi) mutable { - // pv_buffer[vi] = verts.pack(dim_c<3>,ptag,vi); - // }); - - - cudaPol(zs::range(group_size),[ coffset, verts = proxy({},verts), @@ -442,24 +465,19 @@ struct XPBDSolve : INode { float r = cquads("r",coffset + gi); vec3 dp0{},dp1{}; - CONSTRAINT::solve_DistanceConstraint( + if(!CONSTRAINT::solve_DistanceConstraint( p0,minv0, p1,minv1, r, s, dt, lambda, - dp0,dp1); + dp0,dp1)) + return; verts.tuple(dim_c<3>,ptag,edge[0]) = p0 + dp0; verts.tuple(dim_c<3>,ptag,edge[1]) = p1 + dp1; - - // float m0 = verts("m",edge[0]); - // float m1 = verts("m",edge[1]); - // auto ghost_impulse = (dp0 * m0 + dp1 * m1).norm(); - // if(ghost_impulse > 1e-6) - // printf("dmomentum : %f\n",(float)ghost_impulse); } if(category == category_c::isometric_bending_constraint) { auto quad = cquads.pack(dim_c<4>,"inds",coffset + gi,int_c); @@ -473,7 +491,7 @@ struct XPBDSolve : INode { auto Q = cquads.pack(dim_c<4,4>,"Q",coffset + gi); vec3 dp[4] = {}; - CONSTRAINT::solve_IsometricBendingConstraint( + if(!CONSTRAINT::solve_IsometricBendingConstraint( p[0],minv[0], p[1],minv[1], p[2],minv[2], @@ -482,14 +500,41 @@ struct XPBDSolve : INode { s, dt, lambda, - dp[0],dp[1],dp[2],dp[3]); + dp[0],dp[1],dp[2],dp[3])) + return; for(int i = 0;i != 4;++i) { // printf("dp[%d][%d] : %f %f %f %f\n",gi,i,s,(float)dp[i][0],(float)dp[i][1],(float)dp[i][2]); verts.tuple(dim_c<3>,ptag,quad[i]) = p[i] + dp[i]; } + } - + if(category == category_c::dihedral_bending_constraint) { + auto quad = cquads.pack(dim_c<4>,"inds",coffset + gi,int_c); + vec3 p[4] = {}; + float minv[4] = {}; + for(int i = 0;i != 4;++i) { + p[i] = verts.pack(dim_c<3>,ptag,quad[i]); + minv[i] = verts("minv",quad[i]); + } + + auto ra = cquads("ra",coffset + gi); + vec3 dp[4] = {}; + if(!CONSTRAINT::solve_DihedralConstraint( + p[0],minv[0], + p[1],minv[1], + p[2],minv[2], + p[3],minv[3], + ra, + s, + dt, + lambda, + dp[0],dp[1],dp[2],dp[3])) + return; + for(int i = 0;i != 4;++i) { + // printf("dp[%d][%d] : %f %f %f %f\n",gi,i,s,(float)dp[i][0],(float)dp[i][1],(float)dp[i][2]); + verts.tuple(dim_c<3>,ptag,quad[i]) = p[i] + dp[i]; + } } if(category == category_c::p_kp_collision_constraint) { @@ -576,6 +621,33 @@ struct XPBDSolveSmooth : INode { float s = cquads("stiffness",ci); float lambda = cquads("lambda",ci); + if(category == category_c::dihedral_bending_constraint) { + auto quad = cquads.pack(dim_c<4>,"inds",ci,int_c); + vec3 p[4] = {}; + float minv[4] = {}; + for(int i = 0;i != 4;++i) { + p[i] = verts.pack(dim_c<3>,ptag,quad[i]); + minv[i] = verts("minv",quad[i]); + } + + auto ra = cquads("ra",ci); + vec3 dp[4] = {}; + if(!CONSTRAINT::solve_DihedralConstraint( + p[0],minv[0], + p[1],minv[1], + p[2],minv[2], + p[3],minv[3], + ra, + s, + dp[0],dp[1],dp[2],dp[3])) + return; + for(int i = 0;i != 4;++i) + for(int j = 0;j != 3;++j) + atomic_add(exec_tag,&dp_buffer[quad[i] * 3 + j],dp[i][j]); + for(int i = 0;i != 4;++i) + atomic_add(exec_tag,&dp_count[quad[i]],(int)1); + } + if(category == category_c::edge_length_constraint) { auto edge = cquads.pack(dim_c<2>,"inds",ci,int_c); vec3 p0{},p1{}; @@ -602,54 +674,54 @@ struct XPBDSolveSmooth : INode { atomic_add(exec_tag,&dp_count[edge[1]],(int)1); } } - if(category == category_c::isometric_bending_constraint) { - return; - auto quad = cquads.pack(dim_c<4>,"inds",ci,int_c); - vec3 p[4] = {}; - float minv[4] = {}; - for(int i = 0;i != 4;++i) { - p[i] = verts.pack(dim_c<3>,ptag,quad[i]); - minv[i] = verts("minv",quad[i]); - } - - auto Q = cquads.pack(dim_c<4,4>,"Q",ci); - - vec3 dp[4] = {}; - float lambda = 0; - CONSTRAINT::solve_IsometricBendingConstraint( - p[0],minv[0], - p[1],minv[1], - p[2],minv[2], - p[3],minv[3], - Q, - (float)1, - dp[0], - dp[1], - dp[2], - dp[3]); - - auto has_nan = false; - for(int i = 0;i != 4;++i) - if(zs::isnan(dp[i].norm())) - has_nan = true; - if(has_nan) { - printf("nan dp detected : %f %f %f %f %f %f %f %f\n", - (float)p[0].norm(), - (float)p[1].norm(), - (float)p[2].norm(), - (float)p[3].norm(), - (float)dp[0].norm(), - (float)dp[1].norm(), - (float)dp[2].norm(), - (float)dp[3].norm()); - return; - } - for(int i = 0;i != 4;++i) - for(int j = 0;j != 3;++j) - atomic_add(exec_tag,&dp_buffer[quad[i] * 3 + j],dp[i][j]); - for(int i = 0;i != 4;++i) - atomic_add(exec_tag,&dp_count[quad[i]],(int)1); - } + // if(category == category_c::isometric_bending_constraint) { + // return; + // auto quad = cquads.pack(dim_c<4>,"inds",ci,int_c); + // vec3 p[4] = {}; + // float minv[4] = {}; + // for(int i = 0;i != 4;++i) { + // p[i] = verts.pack(dim_c<3>,ptag,quad[i]); + // minv[i] = verts("minv",quad[i]); + // } + + // auto Q = cquads.pack(dim_c<4,4>,"Q",ci); + + // vec3 dp[4] = {}; + // float lambda = 0; + // CONSTRAINT::solve_IsometricBendingConstraint( + // p[0],minv[0], + // p[1],minv[1], + // p[2],minv[2], + // p[3],minv[3], + // Q, + // (float)1, + // dp[0], + // dp[1], + // dp[2], + // dp[3]); + + // auto has_nan = false; + // for(int i = 0;i != 4;++i) + // if(zs::isnan(dp[i].norm())) + // has_nan = true; + // if(has_nan) { + // printf("nan dp detected : %f %f %f %f %f %f %f %f\n", + // (float)p[0].norm(), + // (float)p[1].norm(), + // (float)p[2].norm(), + // (float)p[3].norm(), + // (float)dp[0].norm(), + // (float)dp[1].norm(), + // (float)dp[2].norm(), + // (float)dp[3].norm()); + // return; + // } + // for(int i = 0;i != 4;++i) + // for(int j = 0;j != 3;++j) + // atomic_add(exec_tag,&dp_buffer[quad[i] * 3 + j],dp[i][j]); + // for(int i = 0;i != 4;++i) + // atomic_add(exec_tag,&dp_count[quad[i]],(int)1); + // } }); } diff --git a/projects/CuLagrange/pbd/SelfCollision.cu b/projects/CuLagrange/pbd/SelfCollision.cu index f9c5b22933..74cec669b7 100644 --- a/projects/CuLagrange/pbd/SelfCollision.cu +++ b/projects/CuLagrange/pbd/SelfCollision.cu @@ -84,6 +84,9 @@ struct DetangleImminentCollision : INode { zs::Vector nm_imminent_collision{verts.get_allocator(),(size_t)1}; std::cout << "do imminent detangle" << std::endl; + + auto vn_threshold = 5e-3; + for(int it = 0;it != nm_iters;++it) { cudaPol(zs::range(verts.size()),[ @@ -141,6 +144,7 @@ struct DetangleImminentCollision : INode { eps = eps, exec_tag = wrapv{}, k = repel_strength, + vn_threshold = vn_threshold, max_repel_distance = max_repel_distance, thickness = imminent_collision_thickness, nm_imminent_collision = proxy(nm_imminent_collision), @@ -167,7 +171,7 @@ struct DetangleImminentCollision : INode { auto d = thickness - dist; auto vn = vr.dot(collision_normal); - if(vn < -thickness * 0.05) { + if(vn < -vn_threshold) { atomic_add(exec_tag,&nm_imminent_collision[0],1); for(int i = 0;i != 4;++i) verts("imminent_fail",inds[i]) = (T)1.0; @@ -534,6 +538,9 @@ struct DetangleCCDCollision : INode { std::cout << "resolve continous collision " << std::endl; + auto res_threshold = thickness * 0.01; + res_threshold = res_threshold < 1e-3 ? 1e-3 : res_threshold; + for(int iter = 0;iter != nm_ccd_iters;++iter) { cudaPol(zs::range(impulse_buffer),[]ZS_LAMBDA(auto& imp) mutable {imp = vec3::uniform(0);}); @@ -542,6 +549,8 @@ struct DetangleCCDCollision : INode { nm_ccd_collision.setVal(0); if(do_pt_detection) { + // std::cout << "do continous self PT cololision impulse" << std::endl; + auto do_bvh_refit = iter > 0; COLLISION_UTILS::calc_continous_self_PT_collision_impulse(cudaPol, verts, @@ -555,6 +564,7 @@ struct DetangleCCDCollision : INode { } if(do_ee_detection) { + // std::cout << "do continous self EE cololision impulse" << std::endl; auto do_bvh_refit = iter > 0; COLLISION_UTILS::calc_continous_self_EE_collision_impulse(cudaPol, verts, @@ -566,7 +576,7 @@ struct DetangleCCDCollision : INode { impulse_count); } - // std::cout << "apply impulse" << std::endl; + // std::cout << "apply CCD impulse" << std::endl; cudaPol(zs::range(verts.size()),[ verts = proxy({},verts), vtemp = proxy({},vtemp), @@ -574,6 +584,7 @@ struct DetangleCCDCollision : INode { impulse_count = proxy(impulse_count), relaxation_rate = relaxation_rate, nm_ccd_collision = proxy(nm_ccd_collision), + res_threshold = res_threshold, eps = eps, thickness = thickness, exec_tag = exec_tag] ZS_LAMBDA(int vi) mutable { @@ -583,7 +594,7 @@ struct DetangleCCDCollision : INode { return; auto impulse = relaxation_rate * impulse_buffer[vi] / impulse_count[vi]; - if(impulse.norm() > thickness * 0.01) + if(impulse.norm() > res_threshold) atomic_add(exec_tag,&nm_ccd_collision[0],1); // auto dv = impulse @@ -596,7 +607,7 @@ struct DetangleCCDCollision : INode { if(nm_ccd_collision.getVal() == 0) break; } - std::cout << "finish solving continous collision " << std::endl; + // std::cout << "finish solving continous collision " << std::endl; cudaPol(zs::range(verts.size()),[ vtemp = proxy({},vtemp), verts = proxy({},verts), diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh index 615b218861..9215cde901 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh @@ -1,6 +1,7 @@ #pragma once #include "../../geometry/kernel/geo_math.hpp" +#include "zensim/math/MathUtils.h" namespace zeno { namespace CONSTRAINT { // FOR CLOTH SIMULATION @@ -24,7 +25,7 @@ namespace zeno { namespace CONSTRAINT { { corr0 = VECTOR3d::uniform(0); corr1 = VECTOR3d::uniform(0); - return true; + return false; } SCALER alpha = 0.0; @@ -41,7 +42,7 @@ namespace zeno { namespace CONSTRAINT { { corr0 = VECTOR3d::uniform(0); corr1 = VECTOR3d::uniform(0); - return true; + return false; } const SCALER delta_lambda = -Kinv * (C + alpha * lambda); @@ -132,14 +133,42 @@ namespace zeno { namespace CONSTRAINT { return true; } + template + constexpr bool init_DihedralBendingConstraint( + const VECTOR3d& p0, + const VECTOR3d& p1, + const VECTOR3d& p2, + const VECTOR3d& p3, + SCALER& restAngle){ + VECTOR3d e = p3 - p2; + SCALER elen = e.norm(); + if (elen < 1e-4) + return false; + + SCALER invElen = static_cast(1.0) / elen; + + VECTOR3d n1 = LSL_GEO::facet_normal(p0,p2,p3); + VECTOR3d n2 = LSL_GEO::facet_normal(p1,p3,p2); + + SCALER n12 = n1.dot(n2); + SCALER angle = 0; + if((1 - n12) > 1e-6) + angle = zs::sqrt((1 - n12) / 2); + if(n1.cross(n2).dot(e) < 0) + angle = -angle; + restAngle = angle; + return true; + } + + template constexpr bool solve_DihedralConstraint( - const VECTOR3d &p0, SCALER invMass0, - const VECTOR3d &p1, SCALER invMass1, - const VECTOR3d &p2, SCALER invMass2, - const VECTOR3d &p3, SCALER invMass3, - const SCALER restAngle, - const SCALER stiffness, + const VECTOR3d &p0,const SCALER& invMass0, + const VECTOR3d &p1,const SCALER& invMass1, + const VECTOR3d &p2,const SCALER& invMass2, + const VECTOR3d &p3,const SCALER& invMass3, + const SCALER& restAngle, + const SCALER& stiffness, VECTOR3d &corr0, VECTOR3d &corr1, VECTOR3d &corr2, VECTOR3d &corr3) { constexpr SCALER eps = static_cast(1e-6); @@ -155,7 +184,7 @@ namespace zeno { namespace CONSTRAINT { // auto n1 = LSL_GEO::facet_normal(p0,p2,p3); // auto n2 = LSL_GEO::facet_normal(p1,p3,p2); - VECTOR3d n1 = (p2-p0).cross(p3-p0).normal; n1 /= n1.l2NormSqr(); + VECTOR3d n1 = (p2 - p0).cross(p3 - p0); n1 /= n1.l2NormSqr(); VECTOR3d n2 = (p3 - p1).cross(p2 - p1); n2 /= n2.l2NormSqr(); VECTOR3d d0 = elen*n1; @@ -163,23 +192,21 @@ namespace zeno { namespace CONSTRAINT { VECTOR3d d2 = (p0-p3).dot(e) * invElen * n1 + (p1-p3).dot(e) * invElen * n2; VECTOR3d d3 = (p2-p0).dot(e) * invElen * n1 + (p2-p1).dot(e) * invElen * n2; - n1 = n1.normalize(); - n2 = n2.normalize(); - // n1.normalize(); - // n2.normalize(); - SCALER dot = n1.dot(n2); - - if (dot < -1.0) dot = -1.0; - if (dot > 1.0) dot = 1.0; - SCALER phi = acos(dot); - // SCALER phi = (-0.6981317 * dot * dot - 0.8726646) * dot + 1.570796; // fast approximation + n1 = n1.normalized(); + n2 = n2.normalized(); + SCALER n12 = n1.dot(n2); + SCALER angle = 0; + if((1 - n12) > 1e-6) + angle = zs::sqrt((1 - n12) / 2); + if(n1.cross(n2).dot(e) < 0) + angle = -angle; SCALER lambda = - invMass0 * d0.squaredNorm() + - invMass1 * d1.squaredNorm() + - invMass2 * d2.squaredNorm() + - invMass3 * d3.squaredNorm(); + invMass0 * d0.l2NormSqr() + + invMass1 * d1.l2NormSqr() + + invMass2 * d2.l2NormSqr() + + invMass3 * d3.l2NormSqr(); if (lambda == 0.0) return false; @@ -189,19 +216,86 @@ namespace zeno { namespace CONSTRAINT { //if (stiffness > 0.5 && fabs(phi - b.restAngle) > 1.5) // stiffness = 0.5; - lambda = (phi - restAngle) / lambda * stiffness; + lambda = (angle - restAngle) / lambda * stiffness; - if (n1.cross(n2).dot(e) > 0.0) - lambda = -lambda; + // if (n1.cross(n2).dot(e) > 0.0) + // lambda = -lambda; - corr0 = - invMass0 * lambda * d0; - corr1 = - invMass1 * lambda * d1; - corr2 = - invMass2 * lambda * d2; - corr3 = - invMass3 * lambda * d3; + corr0 = invMass0 * lambda * d0; + corr1 = invMass1 * lambda * d1; + corr2 = invMass2 * lambda * d2; + corr3 = invMass3 * lambda * d3; return true; } + template + constexpr bool solve_DihedralConstraint( + const VECTOR3d& p0, const SCALER& invMass0, + const VECTOR3d& p1, const SCALER& invMass1, + const VECTOR3d& p2, const SCALER& invMass2, + const VECTOR3d& p3, const SCALER& invMass3, + const SCALER& ra, + const SCALER& stiffness, + const SCALER& dt, + SCALER& lambda, + VECTOR3d& corr0, VECTOR3d& corr1, VECTOR3d& corr2, VECTOR3d& corr3){ + constexpr SCALER eps = static_cast(1e-4); + + auto e = p3 - p2; + auto elen = e.norm(); + if(elen < eps) + return false; + + SCALER invElen = static_cast(1.0) / elen; + + VECTOR3d n1 = (p2 - p0).cross(p3 - p0); n1 /= n1.l2NormSqr(); + VECTOR3d n2 = (p3 - p1).cross(p2 - p1); n2 /= n2.l2NormSqr(); + + VECTOR3d gradC[4] = {}; + gradC[0] = elen*n1; + gradC[1] = elen*n2; + gradC[2] = (p0-p3).dot(e) * invElen * n1 + (p1-p3).dot(e) * invElen * n2; + gradC[3] = (p2-p0).dot(e) * invElen * n1 + (p2-p1).dot(e) * invElen * n2; + + n1 = n1.normalized(); + n2 = n2.normalized(); + SCALER n12 = n1.dot(n2); + SCALER angle = 0; + if((1 - n12) > 1e-6) + angle = zs::sqrt((1 - n12) / 2); + if(n1.cross(n2).dot(e) < 0) + angle = -angle; + + SCALER alpha = 0.0; + if (stiffness != 0.0) + alpha = static_cast(1.0) / (stiffness * dt * dt); + + + SCALER sum_normGradC = + invMass0 * gradC[0].l2NormSqr() + + invMass1 * gradC[1].l2NormSqr() + + invMass2 * gradC[2].l2NormSqr() + + invMass3 * gradC[3].l2NormSqr() + alpha; + + if(sum_normGradC < eps) + return false; + + auto energy = angle - ra; + + // compute impulse-based scaling factor + SCALER delta_lambda = -(energy + alpha * lambda) / sum_normGradC; + lambda += delta_lambda; + + // if(n1.cross(n2).dot(e) > 0.0) + // delta_lambda = -delta_lambda; + corr0 = (delta_lambda * invMass0) * gradC[0]; + corr1 = (delta_lambda * invMass1) * gradC[1]; + corr2 = (delta_lambda * invMass2) * gradC[2]; + corr3 = (delta_lambda * invMass3) * gradC[3]; + return true; + } + // ---------------------------------------------------------------------------------------------- template constexpr bool init_IsometricBendingConstraint( @@ -235,6 +329,12 @@ namespace zeno { namespace CONSTRAINT { // auto e3 = e3_.cast(); // auto e4 = e4_.cast(); + // printf("init isometric bending energy : %f %f %f %f\n", + // (float)p0.norm(), + // (float)p1.norm(), + // (float)p2.norm(), + // (float)p3.norm()); + const double c01 = LSL_GEO::cotTheta(e0, e1); const double c02 = LSL_GEO::cotTheta(e0, e2); const double c03 = LSL_GEO::cotTheta(-e0, e3); @@ -283,6 +383,12 @@ namespace zeno { namespace CONSTRAINT { // printf("isometric_bending_energy : %f\n",(float)energy); + // printf("solve isometric bending energy : %f %f %f %f\n", + // (float)p0.norm(), + // (float)p1.norm(), + // (float)p2.norm(), + // (float)p3.norm()); + VECTOR3d gradC[4] = {}; gradC[0] = VECTOR3d::uniform(0); gradC[1] = VECTOR3d::uniform(0); @@ -327,19 +433,15 @@ namespace zeno { namespace CONSTRAINT { for(int i = 0;i != 4;++i) gradCSum += gradC[i].norm(); if(gradCSum > 1e-4) { - printf("gradC : %f %f %f %f corr : %f %f %f %f\n", - (float)gradC[0].norm(),(float)gradC[1].norm(),(float)gradC[2].norm(),(float)gradC[3].norm(), - (float)corr0.norm(),(float)corr1.norm(),(float)corr2.norm(),(float)corr3.norm()); + // printf("gradC : %f %f %f %f corr : %f %f %f %f\n", + // (float)gradC[0].norm(),(float)gradC[1].norm(),(float)gradC[2].norm(),(float)gradC[3].norm(), + // (float)corr0.norm(),(float)corr1.norm(),(float)corr2.norm(),(float)corr3.norm()); } return true; }else { - corr0 = VECTOR3d::uniform(0); - corr1 = VECTOR3d::uniform(0); - corr2 = VECTOR3d::uniform(0); - corr3 = VECTOR3d::uniform(0); + return false; } - return false; } diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp b/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp index 04231db1d4..17c8c43d40 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp @@ -7,6 +7,7 @@ constexpr auto CONSTRAINT_KEY = "XPBD_CONSTRAINT"; enum category_c : int { edge_length_constraint, isometric_bending_constraint, + dihedral_bending_constraint, p_kp_collision_constraint, p_p_collision_constraint, vert_bending_spring,