diff --git a/projects/CUDA/Utils.hpp b/projects/CUDA/Utils.hpp index 9938132296..ed93bd4464 100644 --- a/projects/CUDA/Utils.hpp +++ b/projects/CUDA/Utils.hpp @@ -103,8 +103,8 @@ retrieve_bounding_volumes(Pol &pol, const TileVecT &vtemp, template zs::Vector retrieve_bounding_volumes(Pol &pol, const TileVecT &vtemp, - float radius = 0.f, - const zs::SmallString &xTag = "xn") { + const float& radius, + const zs::SmallString &xTag) { using namespace zs; using bv_t = typename ZenoParticles::lbvh_t::Box; constexpr auto space = Pol::exec_tag::value; @@ -225,7 +225,7 @@ void retrieve_bounding_volumes(Pol &pol, const TileVecT &vtemp, } // for ccd -template +template zs::Vector retrieve_bounding_volumes( Pol &pol, const TileVecT0 &verts, const typename ZenoParticles::particles_t &eles, const TileVecT1 &vtemp, diff --git a/projects/CuLagrange/CMakeLists.txt b/projects/CuLagrange/CMakeLists.txt index cc82d8067f..1ece87b638 100644 --- a/projects/CuLagrange/CMakeLists.txt +++ b/projects/CuLagrange/CMakeLists.txt @@ -38,6 +38,7 @@ target_sources(zeno PRIVATE pbd/ConstraintsBuilder.cu pbd/ConstraintsSolver.cu pbd/CollisionSolver.cu + pbd/ConstraintsUpdator.cu ) # fem-cloth @@ -125,4 +126,6 @@ target_sources(zeno PRIVATE geometry/Intersections.cu # CHECK THIS geometry/Detangle.cu geometry/HalfEdgeStructures.cu + + geometry/ShapeMatching.cu ) diff --git a/projects/CuLagrange/fem/FleshDynamicStepping.cu b/projects/CuLagrange/fem/FleshDynamicStepping.cu index 5197ce238e..50ccfb0dd4 100644 --- a/projects/CuLagrange/fem/FleshDynamicStepping.cu +++ b/projects/CuLagrange/fem/FleshDynamicStepping.cu @@ -833,7 +833,10 @@ struct FleshDynamicStepping : INode { T stiffness = (2.0066 * mu + 1.0122 * lambda) * b_verts("strength",vi); - auto alpha = stiffness * bone_driven_weight * bcws("strength",vi) * bcws("cnorm",vi) * eles("vol",ei) * eles("bdw",ei); + auto area = (T)1.0; + if(b_verts.hasProperty("area")) + area = b_verts("area",vi); + auto alpha = area * stiffness * bone_driven_weight * bcws("strength",vi) * bcws("cnorm",vi) * eles("vol",ei) * eles("bdw",ei); for(size_t i = 0;i != 4;++i){ auto tmp = -pdiff * alpha * w[i]; @@ -1331,12 +1334,13 @@ struct FleshDynamicStepping : INode { {"inds",1}, {"w",4}, {"strength",1}, - {"cnorm",1}},0,zs::memsrc_e::device,0); + {"cnorm",1}},0,zs::memsrc_e::device); auto bverts = typename ZenoParticles::particles_t({ {"x",3}, {"intersect",1}, - {"strength",1}},0,zs::memsrc_e::device,0); + {"area",1}, + {"strength",1}},0,zs::memsrc_e::device); if(has_input("driven_boudary") && zsparticles->hasAuxData(driven_tag)){ auto zsbones = get_input("driven_boudary"); const auto& zsbones_verts = zsbones->getParticles(); @@ -1354,6 +1358,12 @@ struct FleshDynamicStepping : INode { else TILEVEC_OPS::fill(cudaPol,bverts,"intersect",(T)0.0); + if(zsbones_verts.hasProperty("area")) { + TILEVEC_OPS::copy(cudaPol,zsbones_verts,"area",bverts,"area"); + std::cout << "use dynamic area driven weight" << std::endl; + } else + TILEVEC_OPS::fill(cudaPol,bverts,"area",(T)1.0); + const auto& inbbw = (*zsparticles)[driven_tag]; bbw.resize(inbbw.size()); TILEVEC_OPS::copy(cudaPol,inbbw,"X",bbw,"X"); @@ -1361,16 +1371,8 @@ struct FleshDynamicStepping : INode { TILEVEC_OPS::copy(cudaPol,inbbw,"w",bbw,"w"); TILEVEC_OPS::copy(cudaPol,inbbw,"strength",bbw,"strength"); TILEVEC_OPS::copy(cudaPol,inbbw,"cnorm",bbw,"cnorm"); - - // if(zsbones_verts.has_attr("drivenStrength")) - // ompExec(zs::range(zsbones_verts.size()), - // [bverts = proxy(bverts),&zsbones_verts] (int i) mutable { - // auto v = zsbones_verts[i]; - // bverts[i] = zs::vec{v[0],v[1],v[2]}; - // }); - } - // bverts = bverts.clone({zs::memsrc_e::device,0}); + // bverts = bverts.clone({zs::memsrc_e::device}); // std::cout << "bverts.size() = " << bverts.size() << std::endl; auto kverts = typename ZenoParticles::particles_t({ @@ -1380,29 +1382,11 @@ struct FleshDynamicStepping : INode { {"binderStiffness",1}, {planeConsIDTag,1}, {"nrm",3}, - {"area",1}},0,zs::memsrc_e::device,0); + {"area",1}},0,zs::memsrc_e::device); auto ktris = typename ZenoParticles::particles_t({ {"inds",3}, - {"nrm",3}},0,zs::memsrc_e::device,0); - + {"nrm",3}},0,zs::memsrc_e::device); - // dtiles_t surf_tris_buffer{tris.get_allocator(),{ - // {"inds",3}, - // {"nrm",3}, - // {"he_inds",1} - // },tris.size()}; - - // dtiles_t surf_verts_buffer{points.get_allocator(),{ - // {"inds",1}, - // {"xn",3}, - // {"is_loop_vertex",1}, - // {"mustExclude",1} - // },points.size()}; - // TILEVEC_OPS::copy(cudaPol,points,"inds",surf_verts_buffer,"inds"); - // TILEVEC_OPS::copy(cudaPol,tris,"inds",surf_tris_buffer,"inds"); - // TILEVEC_OPS::copy(cudaPol,tris,"he_inds",surf_tris_buffer,"he_inds"); - // reorder_topology(cudaPol,points,surf_tris_buffer); - // zs::Vector nodal_colors{surf_verts_buffer.get_allocator(),surf_verts_buffer.size()}; dtiles_t gia_res{points.get_allocator(),{ {"ring_mask",1}, {"type_mask",1}, @@ -1515,10 +1499,11 @@ struct FleshDynamicStepping : INode { // auto max_collision_pairs = tris.size() / 10; dtiles_t etemp(eles.get_allocator(), { // {"H", 12 * 12}, - {"ActInv",3*3},\ + {"ActInv",3*3}, {"dfiber",3}, // {"muscle_ID",1}, - {"is_inverted",1} + {"is_inverted",1}, + {"fiberStretch",1} }, eles.size() ); @@ -1619,6 +1604,12 @@ struct FleshDynamicStepping : INode { if(!eles.hasProperty("Act")) eles.append_channels(cudaPol,{{"Act",1}}); + + if(!eles.hasProperty("fiberStretch")) + eles.append_channels(cudaPol,{{"fiberStretch",1}}); + + TILEVEC_OPS::fill(cudaPol,eles,"fiberStretch",1.f); + if(!eles.hasProperty("fiber")) fmt::print(fg(fmt::color::red),"the quadrature has no \"fiber\"\n"); if(!verts.hasProperty(muscle_id_tag)) @@ -1691,13 +1682,17 @@ struct FleshDynamicStepping : INode { verts.template pack<3>("x",inds[3]), eles.template pack<3,3>("IB",ei)); auto dfiber = F * fiber; + auto dfiberN = dfiber.norm(); + auto fiberN = fiber.norm(); dfiber = dfiber / dfiber.norm(); etemp.tuple(dim_c<3>,"dfiber",ei) = dfiber; + eles("fiberStretch",ei) = dfiberN / fiberN; } }else{ fiber = zs::vec(1.0,0.0,0.0); act = vec3{1,1,1}; eles("Act",ei) = (T)0.0; + } if(fabs(fiber.norm() - 1.0) > 0.1) { printf("invalid fiber[%d] detected : %f %f %f\n",(int)ei, diff --git a/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp b/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp index f11557b5b6..ce3b61c534 100644 --- a/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp +++ b/projects/CuLagrange/fem/collision_energy/evaluate_collision.hpp @@ -50,6 +50,9 @@ using vec3 = zs::vec; using vec4 = zs::vec; using vec4i = zs::vec; +#define COLLISION_AMONG_SAME_GROUP 1 +#define COLLISION_AMONG_DIFFERENT_GROUP 2 + template 0.1) { + return; + } + } + } + } + if(!(collision_group_strategy & COLLISION_AMONG_SAME_GROUP)) { + for(int i = 0;i != 2;++i) { + auto eaGroup = verts(collision_group_name,ea[i]); + for(int j = 0;j != 2;++j){ + auto ebGroup = verts(collision_group_name,eb[j]); + if(zs::abs(eaGroup - ebGroup) < 0.1) { + return; + } + } + } + } + } + + if(has_collision_cancel) for(int i = 0;i != 2;++i) if(verts("collision_cancel",eb[i]) > 1e-3) @@ -712,7 +744,8 @@ void detect_self_imminent_PT_close_proximity(Pol& pol, ProximityBuffer& proximity_buffer, PTHashMap& csPT, bool skip_too_close_pair_at_rest_configuration = false, - bool use_collision_group = false) { + bool use_collision_group = false, + int collision_group_strategy = COLLISION_AMONG_SAME_GROUP | COLLISION_AMONG_DIFFERENT_GROUP) { using namespace zs; constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; constexpr auto eps = (T)1e-6; @@ -728,7 +761,7 @@ void detect_self_imminent_PT_close_proximity(Pol& pol, skip_rest = skip_too_close_pair_at_rest_configuration, use_collision_group = use_collision_group, eps = eps, - Xtag = Xtag, + XtagOffset = verts.getPropertyOffset(Xtag), xtag = xtag, verts = proxy({},verts), tris = proxy({},tris), @@ -736,6 +769,7 @@ void detect_self_imminent_PT_close_proximity(Pol& pol, thickness = thickness, thickness2 = thickness * thickness, triBvh = proxy(triBvh), + collision_group_strategy = collision_group_strategy, proximity_buffer = proxy({},proximity_buffer), csPT = proxy(csPT)] ZS_LAMBDA(int vi) mutable { @@ -751,6 +785,27 @@ void detect_self_imminent_PT_close_proximity(Pol& pol, if(tri[i] == vi) return; + if(has_collision_group) { + if(!(collision_group_strategy & COLLISION_AMONG_DIFFERENT_GROUP)) { + auto vgroup = verts(collisionGroupTag,vi); + for(int i = 0;i != 3;++i) { + auto tgroup = verts(collisionGroupTag,tri[i]); + // if they belong to two different groups + if(zs::abs(vgroup - tgroup) > 0.1) { + return; + } + } + } + if(!(collision_group_strategy & COLLISION_AMONG_SAME_GROUP)) { + auto vgroup = verts(collisionGroupTag,vi); + for(int i = 0;i != 3;++i) { + auto tgroup = verts(collisionGroupTag,tri[i]); + if(zs::abs(vgroup - tgroup) < 0.1) + return; + } + } + } + for(int i = 0;i != 3;++i) if(verts.hasProperty("collision_cancel") && verts("collision_cancel",tri[i]) > eps) return; @@ -792,10 +847,10 @@ void detect_self_imminent_PT_close_proximity(Pol& pol, #endif if(has_rest_shape && skip_rest) { - auto rp = verts.pack(dim_c<3>,Xtag,vi); + auto rp = verts.pack(dim_c<3>,XtagOffset,vi); vec3 rts[3] = {}; for(int i = 0;i != 3;++i) - rts[i] = verts.pack(dim_c<3>,Xtag,tri[i]); + rts[i] = verts.pack(dim_c<3>,XtagOffset,tri[i]); auto is_same_collision_group = true; if(use_collision_group && has_collision_group) @@ -833,7 +888,8 @@ void detect_self_imminent_PT_close_proximity(Pol& pol, const TriBvh& triBvh, PTHashMap& csPT, bool skip_too_close_pair_at_rest_configuration = false, - bool use_collision_group = false) { + bool use_collision_group = false, + int collision_group_strategy = COLLISION_AMONG_SAME_GROUP | COLLISION_AMONG_DIFFERENT_GROUP) { using namespace zs; constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; constexpr auto eps = (T)1e-6; @@ -846,6 +902,7 @@ void detect_self_imminent_PT_close_proximity(Pol& pol, pol(zs::range(verts.size()),[ collisionGroupTag = collisionGroupTag, + collision_group_strategy = collision_group_strategy, has_collision_cancel = has_collision_cancel, has_rest_shape = has_rest_shape, has_collision_group = has_collision_group, @@ -865,7 +922,7 @@ void detect_self_imminent_PT_close_proximity(Pol& pol, if(verts("collision_cancel",vi) > 1e-3) return; auto p = verts.pack(dim_c<3>,xtag,vi); - auto bv = bv_t{get_bounding_box(p - thickness/(T)2,p + thickness/(T)2)}; + auto bv = bv_t{ (p - thickness/(T)2,p + thickness/(T)2)}; zs::vec ts[3] = {}; @@ -875,6 +932,27 @@ void detect_self_imminent_PT_close_proximity(Pol& pol, if(tri[i] == vi) return; + if(has_collision_group) { + if(!(collision_group_strategy & COLLISION_AMONG_DIFFERENT_GROUP)) { + auto vgroup = verts(collisionGroupTag,vi); + for(int i = 0;i != 3;++i) { + auto tgroup = verts(collisionGroupTag,tri[i]); + // if they belong to two different groups + if(zs::abs(vgroup - tgroup) > 0.1) { + return; + } + } + } + if(!(collision_group_strategy & COLLISION_AMONG_SAME_GROUP)) { + auto vgroup = verts(collisionGroupTag,vi); + for(int i = 0;i != 3;++i) { + auto tgroup = verts(collisionGroupTag,tri[i]); + if(zs::abs(vgroup - tgroup) < 0.1) + return; + } + } + } + if(has_collision_cancel) for(int i = 0;i != 3;++i) if(verts("collision_cancel",tri[i]) > eps) @@ -952,7 +1030,8 @@ void calc_imminent_self_EE_collision_impulse(Pol& pol, const REAL& thickness, size_t buffer_offset, const EdgeBvh& edgeBvh, - CollisionBuffer& imminent_collision_buffer,EEHashMap& csEE) { + CollisionBuffer& imminent_collision_buffer,EEHashMap& csEE, + int collision_group_strategy = COLLISION_AMONG_SAME_GROUP | COLLISION_AMONG_DIFFERENT_GROUP) { using namespace zs; constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; // constexpr auto exec_tag = wrapv{}; @@ -967,6 +1046,7 @@ void calc_imminent_self_EE_collision_impulse(Pol& pol, csEE.reset(pol,true); pol(zs::range(edges.size()),[ + collision_group_strategy = collision_group_strategy, xtag = zs::SmallString(xtag), verts = proxy({},verts), edges = proxy({},edges), @@ -995,6 +1075,9 @@ void calc_imminent_self_EE_collision_impulse(Pol& pol, if(eb[i] == ea[0] || eb[i] == ea[1]) return; } + + + for(int i = 0;i != 2;++i) if(verts.hasProperty("collision_cancel") && verts("collision_cancel",eb[i]) > 1e-3) return; @@ -1012,19 +1095,7 @@ void calc_imminent_self_EE_collision_impulse(Pol& pol, 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(); - - // 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; + vec2 bary{}; LSL_GEO::get_edge_edge_barycentric_coordinates(pas[0],pas[1],pbs[0],pbs[1],bary); @@ -1129,7 +1200,8 @@ void calc_continous_self_PT_collision_impulse(Pol& pol, // bool recalc_collision_pairs = true, bool skip_too_close_pair_at_rest_configuration = false, bool use_collision_group = false, - bool output_debug_inform = false) { + bool output_debug_inform = false, + int collision_group_strategy = COLLISION_AMONG_SAME_GROUP | COLLISION_AMONG_DIFFERENT_GROUP) { using namespace zs; constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; // constexpr auto exec_tag = wrapv{}; @@ -1843,8 +1915,8 @@ void calc_continous_self_PT_collision_impulse_with_toc(Pol& pol, ImpulseCount& impulse_count, bool skip_too_close_pair_at_rest_configuration = false, bool use_collision_group = false, - // bool recalc_collision_pairs = true, - bool output_debug_inform = false) { + bool output_debug_inform = false, + int collision_group_strategy = COLLISION_AMONG_SAME_GROUP | COLLISION_AMONG_DIFFERENT_GROUP) { using namespace zs; constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; // constexpr auto exec_tag = wrapv{}; @@ -1858,11 +1930,6 @@ void calc_continous_self_PT_collision_impulse_with_toc(Pol& pol, auto execTag = wrapv{}; - // std::cout << "do continous PT collilsion detection" << std::endl; - - // std::cout << "build continous PT spacial structure" << std::endl; - - auto bvs = retrieve_bounding_volumes(pol,verts,tris,verts,wrapv<3>{},(T)1.0,(T)thickness,xtag,vtag); if(refit_bvh) triCCDBvh.refit(pol,bvs); @@ -1879,6 +1946,7 @@ void calc_continous_self_PT_collision_impulse_with_toc(Pol& pol, pol(zs::range(verts.size()),[tocs = proxy(tocs)] ZS_LAMBDA(auto vi) mutable {tocs[vi] = 1;}); pol(zs::range(verts.size()),[ + collision_group_strategy = collision_group_strategy, use_collision_group = use_collision_group, skip_rest = skip_too_close_pair_at_rest_configuration, has_collision_group = has_collision_group, @@ -1888,7 +1956,6 @@ void calc_continous_self_PT_collision_impulse_with_toc(Pol& pol, vtag = vtag, verts = proxy({},verts), tris = proxy({},tris), - // csPT = proxy(csPT), thickness = thickness, igore_rest_shape_thickness = igore_rest_shape_thickness, tocs = proxy(tocs), @@ -1925,6 +1992,28 @@ void calc_continous_self_PT_collision_impulse_with_toc(Pol& pol, if(!has_dynamic_points) return; + if(has_collision_group) { + if(!(collision_group_strategy & COLLISION_AMONG_DIFFERENT_GROUP)) { + auto vgroup = verts("collision_group",vi); + for(int i = 0;i != 3;++i) { + auto tgroup = verts("collision_group",tri[i]); + // if they belong to two different groups + if(zs::abs(vgroup - tgroup) > 0.1) { + return; + } + } + } + if(!(collision_group_strategy & COLLISION_AMONG_SAME_GROUP)) { + auto vgroup = verts("collision_group",vi); + for(int i = 0;i != 3;++i) { + auto tgroup = verts("collision_group",tri[i]); + if(zs::abs(vgroup - tgroup) < 0.1) + return; + } + } + } + + if(skip_rest && has_rest_shape) { auto rp = verts.pack(dim_c<3>,"X",vi); vec3 rts[3] = {}; @@ -2606,7 +2695,8 @@ void calc_continous_self_EE_collision_impulse_with_toc(Pol& pol, ImpulseCountBuffer& impulse_count, bool skip_too_close_pair_at_rest_configuration = false, bool use_collision_group = false, - bool output_debug_inform = false) { + bool output_debug_inform = false, + int collision_group_strategy = COLLISION_AMONG_SAME_GROUP | COLLISION_AMONG_DIFFERENT_GROUP) { using namespace zs; constexpr auto space = RM_CVREF_T(pol)::exec_tag::value; // constexpr auto exec_tag = wrapv{}; @@ -2646,6 +2736,7 @@ void calc_continous_self_EE_collision_impulse_with_toc(Pol& pol, auto execTag = wrapv{}; pol(zs::range(nm_test_edges),[ + collision_group_strategy = collision_group_strategy, use_collision_group = use_collision_group, skip_rest = skip_too_close_pair_at_rest_configuration, has_collision_group = has_collision_group, @@ -2699,6 +2790,32 @@ void calc_continous_self_EE_collision_impulse_with_toc(Pol& pol, if(verts.hasProperty("collision_cancel") && verts("collision_cancel",eb[i]) > 1e-3) return; + if(has_collision_group) { + if(!(collision_group_strategy & COLLISION_AMONG_DIFFERENT_GROUP)) { + for(int i = 0;i != 2;++i) { + auto eaGroup = verts("collision_group",ea[i]); + for(int j = 0;j != 2;++j){ + auto ebGroup = verts("collision_group",eb[j]); + if(zs::abs(eaGroup - ebGroup) > 0.1) { + return; + } + } + } + } + if(!(collision_group_strategy & COLLISION_AMONG_SAME_GROUP)) { + for(int i = 0;i != 2;++i) { + auto eaGroup = verts("collision_group",ea[i]); + for(int j = 0;j != 2;++j){ + auto ebGroup = verts("collision_group",eb[j]); + if(zs::abs(eaGroup - ebGroup) < 0.1) { + return; + } + } + } + } + } + + auto has_dynamic_points = false; for(int i = 0;i != 2;++i) { if(invMass("minv",ea[i]) > eps) diff --git a/projects/CuLagrange/fem/collision_energy/vertex_face_sqrt_collision.hpp b/projects/CuLagrange/fem/collision_energy/vertex_face_sqrt_collision.hpp index 9a18bfe6f7..5d809df922 100644 --- a/projects/CuLagrange/fem/collision_energy/vertex_face_sqrt_collision.hpp +++ b/projects/CuLagrange/fem/collision_energy/vertex_face_sqrt_collision.hpp @@ -153,15 +153,18 @@ namespace VERTEX_FACE_SQRT_COLLISION { auto tn = t.template cast().normalized().template cast(); auto productn = tDiff.transpose() * tn; - alpha = alpha > 0 ? alpha : 0; - beta = beta > 0 ? beta : 0; + // alpha = alpha > 0 ? alpha : 0; + alpha = alpha > 0 ? alpha : -alpha; + beta = beta > 0 ? beta : -beta; + // beta = beta > 0 ? beta : 0; auto result = (REAL)2.0 * _mu * ((REAL)alpha * (zs::dyadic_prod(productn,productn)) + (REAL)beta * tDiff.transpose() * tDiff); if(zs::isnan(result.norm())) { printf("nan cH detected %f %f %f %f\n",(float)alpha,(float)productn.norm(),(float)beta,(float)tDiff.norm()); } - return (REAL)2.0 * _mu * ((REAL)alpha * (zs::dyadic_prod(productn,productn)) + (REAL)beta * tDiff.transpose() * tDiff); + // return (REAL)2.0 * _mu * ((REAL)alpha * (zs::dyadic_prod(productn,productn)) + (REAL)beta * tDiff.transpose() * tDiff); + return result; // auto H = (REAL)2.0 * _mu * ((REAL)alpha * (zs::dyadic_prod(productn,productn)) + (REAL)beta * tDiff.transpose() * tDiff); // make_pd(H); // return H; diff --git a/projects/CuLagrange/geometry/BaryCentricInterpolator.cu b/projects/CuLagrange/geometry/BaryCentricInterpolator.cu index 49ea5f3bda..9531879874 100644 --- a/projects/CuLagrange/geometry/BaryCentricInterpolator.cu +++ b/projects/CuLagrange/geometry/BaryCentricInterpolator.cu @@ -95,7 +95,7 @@ struct ZSComputeRBFWeights : INode { rbf_weights = typename ZenoParticles::particles_t({ {"inds",1}, - {"w",1}},close_proximity.size(),zs::memsrc_e::device,0); + {"w",1}},close_proximity.size(),zs::memsrc_e::device); auto varience = get_input2("varience"); @@ -252,7 +252,7 @@ struct ZSComputeSurfaceBaryCentricWeights : INode { sampler = typename ZenoParticles::particles_t({ {"inds",1}, - {"w",1}},csPT.size() * 3,zs::memsrc_e::device,0); + {"w",1}},csPT.size() * 3,zs::memsrc_e::device); zs::Vector nm_updated{dverts.get_allocator(),dverts.size()}; cudaPol(zs::range(nm_updated),[] ZS_LAMBDA(auto& cnt) mutable {cnt = 0;}); @@ -1174,14 +1174,14 @@ struct ZSComputeBaryCentricWeights : INode { {"inds",1}, {"w",4}, {"strength",1}, - {"cnorm",1}},everts.size(),zs::memsrc_e::device,0); + {"cnorm",1}},everts.size(),zs::memsrc_e::device); // auto topo_tag = tag + std::string("_topo"); // auto &bcw_topo = (*zsvolume)[topo_tag]; // auto e_dim = e_eles.getPropertySize("inds"); - // bcw_topo = typename ZenoParticles::particles_t({{"inds",e_dim}},e_eles.size(),zs::memsrc_e::device,0); + // bcw_topo = typename ZenoParticles::particles_t({{"inds",e_dim}},e_eles.size(),zs::memsrc_e::device); auto cudaExec = zs::cuda_exec(); @@ -1393,7 +1393,7 @@ struct ZSSampleEmbedVectorField : zeno::INode { if(!sampler->hasAuxData(tag)){ fmt::print("no specified bcw channel detected, create a new one...\n"); auto& sample_bcw = (*sampler)[tag]; - sample_bcw = typename ZenoParticles::particles_t({{"inds",1},{"w",4}},verts.size(),zs::memsrc_e::device,0); + sample_bcw = typename ZenoParticles::particles_t({{"inds",1},{"w",4}},verts.size(),zs::memsrc_e::device); } const auto& sample_bcw = (*sampler)[tag]; @@ -1464,7 +1464,7 @@ struct ZSSampleEmbedTagField : zeno::INode { if(!sampler->hasAuxData(tag)){ fmt::print("no specified bcw channel detected, create a new one...\n"); auto& sample_bcw = (*sampler)[tag]; - sample_bcw = typename ZenoParticles::particles_t({{"inds",1},{"w",4}},verts.size(),zs::memsrc_e::device,0); + sample_bcw = typename ZenoParticles::particles_t({{"inds",1},{"w",4}},verts.size(),zs::memsrc_e::device); } const auto& sample_bcw = (*sampler)[tag]; diff --git a/projects/CuLagrange/geometry/Detangle.cu b/projects/CuLagrange/geometry/Detangle.cu index 514d9c7794..79b7cf9a74 100644 --- a/projects/CuLagrange/geometry/Detangle.cu +++ b/projects/CuLagrange/geometry/Detangle.cu @@ -42,7 +42,7 @@ struct Detangle2 : zeno::INode { constexpr auto DETANGLE_CS_KET_BUFFER_KEY = "DETANGLE_CS_KET_BUFFER_KEY"; constexpr auto DETANGLE_TRI_BVH_BUFFER_KEY = "DETANGLE_TRI_BVH_BUFFER_KEY"; constexpr auto DETANGLE_ICM_GRADIENT_BUFFER_KEY = "DETANGLE_ICM_GRADIENT_BUFFER_KEY"; - constexpr auto DEFAULT_MAX_DETANGLE_INTERSECTION_PAIR = 10000; + constexpr auto DEFAULT_MAX_DETANGLE_INTERSECTION_PAIR = 100000; auto zsparticles = get_input("zsparticles"); auto& verts = zsparticles->getParticles(); @@ -111,6 +111,7 @@ struct Detangle2 : zeno::INode { dtiles_t kvtemp{verts.get_allocator(),{ {"x",3}, {"collision_cancel",1}, + {"collision_group",1} },0}; dtiles_t kttemp{tris.get_allocator(),{ {"inds",3}, @@ -129,6 +130,9 @@ struct Detangle2 : zeno::INode { auto detangle_with_boundary = get_input2("detangle_with_boundary"); auto do_self_detangle = get_input2("do_self_detangle"); + auto among_same_group = get_input2("among_same_group"); + auto among_different_group = get_input2("among_different_group"); + if(has_input("kboundary") && detangle_with_boundary) { auto kboundary = get_input("kboundary"); auto substep_id = get_input2("substep_id"); @@ -149,6 +153,8 @@ struct Detangle2 : zeno::INode { kxtag = zs::SmallString(kxtag), kpxtag = zs::SmallString(kpxtag), kverts = proxy({},kverts), + has_collision_group = kverts.hasProperty(collision_group_name), + collision_group_name = zs::SmallString(collision_group_name), hasKCollisionCancel = kverts.hasProperty("colllision_cancel"), kvtemp = proxy({},kvtemp)] ZS_LAMBDA(int kvi) mutable { auto kvert = kverts.pack(dim_c<3>,kpxtag,kvi) * (1 - alpha) + kverts.pack(dim_c<3>,kxtag,kvi) * alpha; @@ -157,6 +163,11 @@ struct Detangle2 : zeno::INode { kvtemp("collision_cancel",kvi) = kverts("collision_cancel",kvi); else kvtemp("collision_cancel",kvi) = 0; + if(has_collision_group) { + kvtemp("collision_group",kvi) = kverts(collision_group_name,kvi); + }else { + kvtemp("collision_group",kvi) = -1.0f; + } }); kttemp.resize(ktris.size()); TILEVEC_OPS::copy<3>(cudaExec,ktris,"inds",kttemp,"inds"); @@ -184,6 +195,8 @@ struct Detangle2 : zeno::INode { auto kbvs = retrieve_bounding_volumes(cudaExec,kvtemp,ktris,wrapv<3>{},(T)0,"x"); ktri_bvh.build(cudaExec,kbvs); + + // std::cout << "detangle build bvh : " << kbvs.size() << std::endl; } int nm_intersections = 0; @@ -275,14 +288,18 @@ struct Detangle2 : zeno::INode { // std::cout << "retrive_intersections_between_edges_and_ktris" << std::endl; if(do_proximity_detection) { retrieve_intersection_with_edge_tri_pairs(cudaExec, - verts,xtag, + verts,xtag,collision_group_name, edges, - kvtemp,"x", + kvtemp,"x","collision_group", kttemp, ktri_bvh, csEKT, icm_grad, - false); + false, + among_same_group, + among_different_group); + + // std::cout << "do EKT intersection detection : " << csEKT.size() << "\t" << kvtemp.size() << "\t" << kttemp.size() << std::endl; } if(iter == 0) nm_kinematic_intersection += csEKT.size(); @@ -414,14 +431,16 @@ struct Detangle2 : zeno::INode { if(do_proximity_detection) { retrieve_intersection_with_edge_tri_pairs(cudaExec, - kvtemp,"x", + kvtemp,"x","collision_group", kedges, - verts,xtag, + verts,xtag,collision_group_name, tris, tri_bvh, csKET, icm_grad, - false); + false, + among_same_group, + among_different_group); } #ifdef TIMING_DETANGLE timer.tock("retrieve_intersection_with_KET_pairs"); @@ -429,7 +448,7 @@ struct Detangle2 : zeno::INode { // nm_intersections += csET.size(); - // std::cout << "finish retrive_intersections_between_kedges_and_tris" << std::endl; + // std::cout << "do TKE intersection detection" << csKET.size() << "\t" << kvtemp.size() << "\t" << kedges.size() << std::endl; if(csKET.size() > 0) has_kine_intersection = true; @@ -564,7 +583,9 @@ struct Detangle2 : zeno::INode { skip_distance, false, skip_too_close_pair_at_rest_shape, - use_collision_group); + use_collision_group, + among_same_group, + among_different_group); } // std::cout << "nm_self_intersections_ET : " << csET.size() << std::endl; @@ -730,6 +751,7 @@ struct Detangle2 : zeno::INode { std::cout << "nm_kin_intersections : " << nm_kinematic_intersection << std::endl; } + auto gradInfNorm = TILEVEC_OPS::inf_norm<3>(cudaExec,verts,"grad"); if(gradInfNorm < 1e-3) break; @@ -802,7 +824,9 @@ ZENDEFNODE(Detangle2, { {"bool","enforce_self_intersection_normal","0"}, {"bool","detangle_with_boundary","1"}, {"bool","do_self_detangle","1"}, - {"bool","skip_animation_intersection","1"} + {"bool","skip_animation_intersection","1"}, + {"bool","among_same_group","1"}, + {"bool","among_different_group","1"} }, { {"zsparticles"} diff --git a/projects/CuLagrange/geometry/HalfEdgeStructures.cu b/projects/CuLagrange/geometry/HalfEdgeStructures.cu index e974b6d13b..b4a8878eee 100644 --- a/projects/CuLagrange/geometry/HalfEdgeStructures.cu +++ b/projects/CuLagrange/geometry/HalfEdgeStructures.cu @@ -41,7 +41,7 @@ namespace zeno { 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); + tris.size() * 3,zs::memsrc_e::device); auto cudaPol = zs::cuda_exec(); @@ -186,7 +186,7 @@ namespace zeno { }); auto &surfEdges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; - surfEdges = typename ZenoParticles::particles_t({{"inds", 2},{"he_inds",1}}, edgeSet.size(),zs::memsrc_e::device,0); + surfEdges = typename ZenoParticles::particles_t({{"inds", 2},{"he_inds",1}}, edgeSet.size(),zs::memsrc_e::device); cudaPol(zip(zs::range(edgeSet.size()),edgeSet._activeKeys),[ halfedges = proxy({},halfEdge), surfEdges = proxy({},surfEdges), @@ -208,7 +208,7 @@ namespace zeno { auto& boundaryHalfEdges = (*zsparticles)[ZenoParticles::s_surfBoundaryEdgeTag]; boundaryHalfEdges = typename ZenoParticles::particles_t({{"he_inds",1}}, - boundaryHalfEdgeSet.size(),zs::memsrc_e::device,0); + boundaryHalfEdgeSet.size(),zs::memsrc_e::device); cudaPol(zip(zs::range(boundaryHalfEdgeSet.size()),boundaryHalfEdgeSet._activeKeys),[ boundaryHalfEdges = boundaryHalfEdges.begin("he_inds",dim_c<1>,int_c)] ZS_LAMBDA(int id,const auto& key) mutable { @@ -243,7 +243,7 @@ namespace zeno { auto& halfFacet = (*zsparticles)[ZenoParticles::s_tetHalfFacetTag]; halfFacet = typename ZenoParticles::particles_t({{"opposite_hf",1},{"next_hf",1},{"to_tet",1},{"local_idx",1}}, - tets.size() * 4,zs::memsrc_e::device,0); + tets.size() * 4,zs::memsrc_e::device); build_tetrahedra_half_facet(cudaPol,tets,halfFacet); @@ -286,7 +286,7 @@ namespace zeno { }); auto &surfEdges = (*zsparticles)[ZenoParticles::s_surfEdgeTag]; - surfEdges = typename ZenoParticles::particles_t({{"inds", 2}}, edgeSet.size(),zs::memsrc_e::device,0); + surfEdges = typename ZenoParticles::particles_t({{"inds", 2}}, edgeSet.size(),zs::memsrc_e::device); 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); diff --git a/projects/CuLagrange/geometry/ShapeMatching.cu b/projects/CuLagrange/geometry/ShapeMatching.cu new file mode 100644 index 0000000000..90e1216f01 --- /dev/null +++ b/projects/CuLagrange/geometry/ShapeMatching.cu @@ -0,0 +1,175 @@ +#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 "../geometry/kernel/tiled_vector_ops.hpp" +// #include "../geometry/kernel/topology.hpp" +// #include "../geometry/kernel/geo_math.hpp" + +#include "zensim/math/Rotation.hpp" +#include "zensim/math/matrix/QRSVD.hpp" + + +namespace zeno { + +struct MatchTransformation : zeno::INode { + + virtual void apply() override { + using namespace zs; + using vec3 = zs::vec; + using vec4 = zs::vec; + using mat3 = zs::vec; + + constexpr auto space = execspace_e::openmp; + auto ompPol = omp_exec(); + constexpr auto exec_tag = wrapv{}; + + auto rprim = get_input("refObj"); + auto tprim = get_input("targetObj"); + + auto shape_size = rprim->verts.size(); + + // compute the center of mass of rprim + auto rcm = vec3::zeros(); + auto tcm = vec3::zeros(); + // cmVec[0] = zs::vec::zeros(); + + const auto& rverts = rprim->verts; + const auto& tverts = tprim->verts; + + ompPol(zs::range(shape_size),[ + exec_tag = exec_tag, + &rverts,&tverts,&rcm,&tcm] (int vi) mutable { + for(int d = 0;d != 3;++d) { + atomic_add(exec_tag,&rcm[d],rverts[vi][d]); + atomic_add(exec_tag,&tcm[d],tverts[vi][d]); + } + }); + + rcm /= shape_size; + tcm /= shape_size; + + std::vector dAs(shape_size,mat3::zeros()); + ompPol(zs::range(shape_size),[ + &dAs,rcm,tcm,&rverts,&tverts] (int vi) mutable { + auto q = vec3::from_array(rverts[vi]) - rcm; + auto p = vec3::from_array(tverts[vi]) - tcm; + dAs[vi] = dyadic_prod(p,q); + }); + + auto A = mat3::zeros(); + ompPol(zs::range(shape_size * 9),[ + exec_tag = exec_tag, + &A, + &dAs] (int dof) mutable { + auto dAid = dof / 9; + auto Aoffset = dof % 9; + auto r = Aoffset / 3; + auto c = Aoffset % 3; + const auto& dA = dAs[dAid]; + atomic_add(exec_tag,&A[r][c],dA[r][c]); + }); + A /= shape_size; + + auto [R,S] = math::polar_decomposition(A); + + // R = R.transpose(); + + printf("R:\n%f\b%f\b%f\n%f\b%f\b%f\n%f\b%f\b%f\n", + (float)R(0,0),(float)R(0,1),(float)R(0,2), + (float)R(1,0),(float)R(1,1),(float)R(1,2), + (float)R(2,0),(float)R(2,1),(float)R(2,2)); + + auto b = tcm - R * rcm; + + auto q = vec4::zeros(); + auto m00 = R(0,0); + auto m01 = R(0,1); + auto m02 = R(0,2); + auto m10 = R(1,0); + auto m11 = R(1,1); + auto m12 = R(1,2); + auto m20 = R(2,0); + auto m21 = R(2,1); + auto m22 = R(2,2); + // float t{0}; + + // if (m22 < 0) { + // if (m00 > m11) { + // t = 1 + m00 -m11 -m22; + // q = vec4( t, m01+m10, m20+m02, m12-m21 ); + // } + // else { + // t = 1 -m00 + m11 -m22; + // q = vec4( m01+m10, t, m12+m21, m20-m02 ); + // } + // } + // else { + // if (m00 < -m11) { + // t = 1 -m00 -m11 + m22; + // q = vec4( m20+m02, m12+m21, t, m01-m10 ); + // } + // else { + // t = 1 + m00 + m11 + m22; + // q = vec4( m12-m21, m20-m02, m01-m10, t ); + // } + // } + // q *= 0.5 / zs::sqrt(t); + + auto trace = m00 + m11 + m22; + if (trace > 0.0f) + { + auto k = 0.5f / zs::sqrt(1.0f + trace); + q = vec4( k * (m12 - m21), k * (m20 - m02), k * (m01 - m10), 0.25f / k ); + } + else if ((m00 > m11) && (m00 > m22)) + { + auto k = 0.5f / zs::sqrt(1.0f + m00 - m11 - m22); + q = vec4( 0.25f / k, k * (m10 + m01), k * (m20 + m02), k * (m12 - m21) ); + } + else if (m11 > m22) + { + auto k = 0.5f / zs::sqrt(1.0f + m11 - m00 - m22); + q = vec4( k * (m10 + m01), 0.25f / k, k * (m21 + m12), k * (m20 - m02) ); + } + else + { + auto k = 0.5f / zs::sqrt(1.0f + m22 - m00 - m11); + q = vec4( k * (m20 + m02), k * (m21 + m12), 0.25f / k, k * (m01 - m10) ); + } + + // due to the column-major setting, need a quaternion negation here + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + + auto retq = std::make_shared(); + retq->set(zeno::vec4f(q[0],q[1],q[2],q[3])); + + auto retb = std::make_shared(); + retb->set(zeno::vec3f(b[0],b[1],b[2])); + + + set_output("quat",std::move(retq)); + set_output("trans",std::move(retb)); + } + +}; + +ZENDEFNODE(MatchTransformation,{ + {{"refObj"},{"targetObj"}}, + {"quat","trans"}, + {}, + {"Geometry"}, +}); + +}; \ No newline at end of file diff --git a/projects/CuLagrange/geometry/VectorField.cu b/projects/CuLagrange/geometry/VectorField.cu index 228d50947f..dbe5a8ce14 100644 --- a/projects/CuLagrange/geometry/VectorField.cu +++ b/projects/CuLagrange/geometry/VectorField.cu @@ -185,8 +185,8 @@ struct ZSRetrieveVectorField : zeno::INode { std::vector tags{{"x",3},{"vec",3}}; - auto vec_buffer = typename ZenoParticles::particles_t(tags,on_elm ? eles.size() : verts.size(),zs::memsrc_e::device,0); - auto zsvec_buffer = zs::Vector(on_elm ? eles.size() : verts.size(),zs::memsrc_e::device,0); + auto vec_buffer = typename ZenoParticles::particles_t(tags,on_elm ? eles.size() : verts.size(),zs::memsrc_e::device); + auto zsvec_buffer = zs::Vector(on_elm ? eles.size() : verts.size(),zs::memsrc_e::device); // transfer the data from gpu to cpu constexpr auto cuda_space = execspace_e::cuda; auto cudaPol = cuda_exec(); @@ -862,8 +862,8 @@ struct ZSGaussianSampler : zeno::INode { } int dim = source_dim; - auto src = typename ZenoParticles::particles_t({{"x",3},{"attr",dim},{"inds",1}},0,zs::memsrc_e::device,0); - auto dst = typename ZenoParticles::particles_t({{"x",3},{"attr",dim},{"mark",1}},0,zs::memsrc_e::device,0); + auto src = typename ZenoParticles::particles_t({{"x",3},{"attr",dim},{"inds",1}},0,zs::memsrc_e::device); + auto dst = typename ZenoParticles::particles_t({{"x",3},{"attr",dim},{"mark",1}},0,zs::memsrc_e::device); int source_simplex_size = srcQuads.getPropertySize("inds"); if(srcType == "vert"){ diff --git a/projects/CuLagrange/geometry/kernel/bary_centric_weights.hpp b/projects/CuLagrange/geometry/kernel/bary_centric_weights.hpp index 005d1e04b1..40c452f0da 100644 --- a/projects/CuLagrange/geometry/kernel/bary_centric_weights.hpp +++ b/projects/CuLagrange/geometry/kernel/bary_centric_weights.hpp @@ -143,7 +143,7 @@ namespace zeno { const zs::vec& b2, T& toc, zs::vec& bary, - const T& eta = (T)0.1) { + const T& eta = (T)0.001) { auto v0 = b0 - a0; auto v1 = b1 - a1; diff --git a/projects/CuLagrange/geometry/kernel/geo_math.hpp b/projects/CuLagrange/geometry/kernel/geo_math.hpp index 9aebc609f6..c4e185ab50 100644 --- a/projects/CuLagrange/geometry/kernel/geo_math.hpp +++ b/projects/CuLagrange/geometry/kernel/geo_math.hpp @@ -285,6 +285,7 @@ namespace zeno { namespace LSL_GEO { /////////////////////////////////////////////////////////////////////// constexpr VECTOR3 get_vertex_triangle_barycentric_coordinates(const VECTOR3 vertices[4]) { +#if 1 const VECTOR3 v0 = vertices[1]; const VECTOR3 v1 = vertices[2]; const VECTOR3 v2 = vertices[3]; @@ -304,6 +305,31 @@ namespace zeno { namespace LSL_GEO { n.dot(nc) / n.l2NormSqr()); return barycentric; +#else + constexpr auto eps = 1e-6; + const auto& v1 = vertices[1]; + const auto& v2 = vertices[2]; + const auto& v3 = vertices[3]; + const auto& v4 = vertices[0]; + + auto x13 = v1 - v3; + auto x23 = v2 - v3; + auto x43 = v4 - v3; + auto A00 = x13.dot(x13); + auto A01 = x13.dot(x23); + auto A11 = x23.dot(x23); + auto b0 = x13.dot(x43); + auto b1 = x23.dot(x43); + auto detA = A00 * A11 - A01 * A01; + + VECTOR3 bary{}; + + bary[0] = ( A11 * b0 - A01 * b1) / detA; + bary[1] = (-A01 * b0 + A00 * b1) / detA; + bary[2] = 1 - bary[0] - bary[1]; + + return bary; +#endif } @@ -369,28 +395,33 @@ namespace zeno { namespace LSL_GEO { /////////////////////////////////////////////////////////////////////// constexpr REAL get_vertex_triangle_distance(const VECTOR3& v0, const VECTOR3& v1, - const VECTOR3& v2, const VECTOR3& v,VECTOR3& barycentric,VECTOR3& project_bary) + const VECTOR3& v2, const VECTOR3& v,VECTOR3& barycentric,VECTOR3& project_point) { // get the barycentric coordinates const VECTOR3 e1 = v1 - v0; const VECTOR3 e2 = v2 - v0; + const VECTOR3 e3 = v2 - v1; 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); - barycentric = VECTOR3(n.dot(na) / n.l2NormSqr(), - n.dot(nb) / n.l2NormSqr(), - n.dot(nc) / n.l2NormSqr()); - + // barycentric = VECTOR3(n.dot(na) / n.l2NormSqr(), + // n.dot(nb) / n.l2NormSqr(), + // n.dot(nc) / n.l2NormSqr()); + auto n2 = n.l2NormSqr(); + barycentric = VECTOR3(n.dot(na),n.dot(nb),n.dot(nc)); const REAL barySum = zs::abs(barycentric[0]) + zs::abs(barycentric[1]) + zs::abs(barycentric[2]); // if the point projects to inside the triangle, it should sum to 1 - if (zs::abs(barySum - 1.0) < 1e-6) + if (zs::abs(barySum - n2) < static_cast(1e-6) && n2 > static_cast(1e-6)) { const VECTOR3 nHat = n / n.norm(); const REAL normalDistance = (nHat.dot(v - v0)); - project_bary = barycentric; - // project_v = barycentric[0] * v0 + barycentric[1] * v1 + barycentric[2] * v2; + barycentric /= n2; + // project_bary = barycentric; + // project_point = VECTOR3::zeros(); + + project_point = barycentric[0] * v0 + barycentric[1] * v1 + barycentric[2] * v2; return zs::abs(normalDistance); } @@ -399,44 +430,51 @@ constexpr REAL get_vertex_triangle_distance(const VECTOR3& v0, const VECTOR3& v1 VECTOR3 es[3] = {}; // project onto each edge, find the distance to each edge - const VECTOR3 e3 = v2 - v1; + const VECTOR3 ev = v - v0; const VECTOR3 ev3 = v - v1; - const VECTOR3 e1Hat = e1 / e1.norm(); - const VECTOR3 e2Hat = e2 / e2.norm(); - const VECTOR3 e3Hat = e3 / e3.norm(); - VECTOR3 edgeDistances(1e8, 1e8, 1e8); + + // const VECTOR3 e2Hat = e2 / e2.norm(); + // const VECTOR3 e3Hat = e3 / e3.norm(); + VECTOR3 edgeDistances{1e8, 1e8, 1e8}; // see if it projects onto the interval of the edge // if it doesn't, then the vertex distance will be smaller, // so we can skip computing anything - const REAL e1dot = e1Hat.dot(ev); + // VECTOR3 e1Hat = e1; + REAL e1dot = e1.dot(ev); // VECTOR3 projected_e[3] = {}; - if (e1dot > 0.0 && e1dot < e1.norm()) + // auto e1n = e1.norm(); + auto e1n2 = e1.l2NormSqr(); + if (e1dot > 0.0 && e1dot < e1n2 && e1n2 > static_cast(1e-6)) { - const VECTOR3 projected = v0 + e1Hat * e1dot; + // e1Hat /= e1.norm(); + const VECTOR3 projected = v0 + e1 * e1dot / e1n2; es[0] = projected; edgeDistances[0] = (v - projected).norm(); } - const REAL e2dot = e2Hat.dot(ev); - if (e2dot > 0.0 && e2dot < e2.norm()) + + const REAL e2dot = e2.dot(ev); + auto e2n2 = e2.l2NormSqr(); + if (e2dot > 0.0 && e2dot < e2n2 && e2n2 > static_cast(1e-6)) { - const VECTOR3 projected = v0 + e2Hat * e2dot; + const VECTOR3 projected = v0 + e2 * e2dot / e2n2; es[1] = projected; edgeDistances[1] = (v - projected).norm(); } - const REAL e3dot = e3Hat.dot(ev3); - if (e3dot > 0.0 && e3dot < e3.norm()) + const REAL e3dot = e3.dot(ev3); + auto e3n2 = e3.l2NormSqr(); + if (e3dot > 0.0 && e3dot < e3n2 && e3n2 > static_cast(1e-6)) { - const VECTOR3 projected = v1 + e3Hat * e3dot; + const VECTOR3 projected = v1 + e3 * e3dot / e3n2; es[2] = projected; edgeDistances[2] = (v - projected).norm(); } // get the distance to each vertex - const VECTOR3 vertexDistances((v - v0).norm(), + const VECTOR3 vertexDistances{(v - v0).norm(), (v - v1).norm(), - (v - v2).norm()); + (v - v2).norm()}; // get the smallest of both the edge and vertex distances REAL vertexMin = 1e8; @@ -459,22 +497,22 @@ constexpr REAL get_vertex_triangle_distance(const VECTOR3& v0, const VECTOR3& v1 // vertexMin = vertexMin > vertexDistances[i] ? vertexDistances[i] : vertexMin; // edgeMin = edgeMin > edgeDistances[i] ? edgeDistances[i] : edgeMin; } - VECTOR3 project_v{}; + // VECTOR3 project_v{}; if(vertexMin < edgeMin) - project_v = vs[min_v_idx]; + project_point = vs[min_v_idx]; else - project_v = es[min_e_idx]; + project_point = es[min_e_idx]; // const VECTOR3 e1 = v1 - v0; // const VECTOR3 e2 = v2 - v0; // const VECTOR3 n = e1.cross(e2); - auto na_p = (v2 - v1).cross(project_v - v1); - auto nb_p = (v0 - v2).cross(project_v - v2); - auto nc_p = (v1 - v0).cross(project_v - v0); - project_bary = VECTOR3(n.dot(na_p) / n.l2NormSqr(), - n.dot(nb_p) / n.l2NormSqr(), - n.dot(nc_p) / n.l2NormSqr()); + // auto na_p = (v2 - v1).cross(project_v - v1); + // auto nb_p = (v0 - v2).cross(project_v - v2); + // auto nc_p = (v1 - v0).cross(project_v - v0); + // project_bary = VECTOR3(n.dot(na_p) / n.l2NormSqr(), + // n.dot(nb_p) / n.l2NormSqr(), + // n.dot(nc_p) / n.l2NormSqr()); // return the smallest of those return (vertexMin < edgeMin) ? vertexMin : edgeMin; @@ -570,8 +608,8 @@ constexpr REAL get_vertex_triangle_distance(const VECTOR3& v0, const VECTOR3& v1 auto b0 = x13.dot(x43); auto b1 = x23.dot(x43); auto detA = A00 * A11 - A01 * A01; - bary[0] = ( A11 * b0 - A01 * b1) / detA; - bary[1] = (-A01 * b0 + A00 * b1) / detA; + bary[0] = ( A11 * b0 - A01 * b1) / (detA + eps); + bary[1] = (-A01 * b0 + A00 * b1) / (detA + eps); bary[2] = 1 - bary[0] - bary[1]; } @@ -587,8 +625,8 @@ constexpr REAL get_vertex_triangle_distance(const VECTOR3& v0, const VECTOR3& v1 auto b0 = x13.dot(x43); auto b1 = x23.dot(x43); detA = A00 * A11 - A01 * A01; - bary[0] = ( A11 * b0 - A01 * b1) / detA; - bary[1] = (-A01 * b0 + A00 * b1) / detA; + bary[0] = ( A11 * b0 - A01 * b1) / (detA + eps); + bary[1] = (-A01 * b0 + A00 * b1) / (detA + eps); bary[2] = 1 - bary[0] - bary[1]; } @@ -604,8 +642,8 @@ constexpr REAL get_vertex_triangle_distance(const VECTOR3& v0, const VECTOR3& v1 auto b1 = -x43.dot(x31); auto detA = A00 * A11 - A01 * A01; - bary[0] = ( A11 * b0 - A01 * b1) / detA; - bary[1] = (-A01 * b0 + A00 * b1) / detA; + bary[0] = ( A11 * b0 - A01 * b1) / (detA + eps); + bary[1] = (-A01 * b0 + A00 * b1) / (detA + eps); // }else { // the two edge is almost parallel // } diff --git a/projects/CuLagrange/geometry/kernel/intersection.hpp b/projects/CuLagrange/geometry/kernel/intersection.hpp index 49ff89e373..296a7dedb2 100644 --- a/projects/CuLagrange/geometry/kernel/intersection.hpp +++ b/projects/CuLagrange/geometry/kernel/intersection.hpp @@ -321,7 +321,9 @@ int retrieve_intersection_tri_halfedge_info_of_two_meshes(Pol& pol, const float& thickness, bool use_barycentric_interpolator = false, bool skip_too_close_pair_at_rest_configuration = false, - bool use_collision_group = false) { + bool use_collision_group = false, + bool among_same_group = true, + bool among_different_group = true) { using namespace zs; using vec2i = zs::vec; using bv_t = typename ZenoParticles::lbvh_t::Box; @@ -348,6 +350,8 @@ int retrieve_intersection_tri_halfedge_info_of_two_meshes(Pol& pol, // nm_ints.setVal(0); #endif pol(zs::range(edges.size()),[ + among_different_group = among_different_group, + among_same_group = among_same_group, exec_tag = exec_tag, edges = edges.begin("inds", dim_c<2>, int_c), triNrmOffset = tris.getPropertyOffset("nrm"), @@ -451,6 +455,19 @@ int retrieve_intersection_tri_halfedge_info_of_two_meshes(Pol& pol, } } + if(hasCollisionGroup) { + for(int i = 0;i != 2;++i) { + for(int j = 0;j != 3;++j) { + if(zs::abs(verts(collisionGroupOffset,edge[i]) - verts(collisionGroupOffset,tri[j])) > 0.5) { + if(!among_different_group) + return; + if(!among_same_group) + return; + } + } + } + } + vec3 tvs[3] = {}; for(int i = 0;i != 3;++i) tvs[i] = verts.pack(dim_c<3>,xOffset,tri[i]); @@ -555,18 +572,20 @@ int retrieve_intersection_tri_halfedge_info_of_two_meshes(Pol& pol, typename BaryTileVec, typename TriBVH> void retrieve_intersection_with_edge_tri_pairs(Pol& pol, - const PosTileVec& verts, const zs::SmallString& xtag, + const PosTileVec& verts, const zs::SmallString& xtag,const zs::SmallString& collisionGroupName, const EdgeTileVec& edges, // const TriTileVec& tris, // const TriBVH& tri_bvh, - const PosTileVec& kverts, const zs::SmallString& kxtag, + const PosTileVec& kverts, const zs::SmallString& kxtag,const zs::SmallString& kCollisionGroupName, // const EdgeTileVec& kedges, const TriTileVec& ktris, const TriBVH& ktri_bvh, zs::bht& res, // zs::bht& cs_KET, BaryTileVec& bary_buffer, - bool use_barycentric_interpolator = false) { + bool use_barycentric_interpolator = false, + bool among_same_group = true, + bool among_different_group = true) { using namespace zs; using vec2i = zs::vec; using bv_t = typename ZenoParticles::lbvh_t::Box; @@ -583,10 +602,16 @@ int retrieve_intersection_tri_halfedge_info_of_two_meshes(Pol& pol, // timer.tick(); pol(zs::range(edges.size()),[ + among_same_group = among_same_group, + among_different_group = among_different_group, exec_tag = exec_tag, use_barycentric_interpolator = use_barycentric_interpolator, edges = edges.begin("inds", dim_c<2>, int_c), bary_buffer = proxy({},bary_buffer), + hasKCollisionGroup = kverts.hasProperty(kCollisionGroupName), + kCollisionGroupOffset = kverts.getPropertyOffset(kCollisionGroupName), + hasCollisionGroup = verts.hasProperty(collisionGroupName), + collisionGroupOffset = verts.getPropertyOffset(collisionGroupName), ktriNrmOffset = ktris.getPropertyOffset("nrm"), ktriDOffset = ktris.getPropertyOffset("d"), ktriIndsOffset = ktris.getPropertyOffset("inds"), @@ -643,6 +668,20 @@ int retrieve_intersection_tri_halfedge_info_of_two_meshes(Pol& pol, if(!is_dynamic_edge || !is_dynamic_ktri) return; + + if(hasCollisionGroup && hasKCollisionGroup) { + for(int i = 0;i != 2;++i) { + for(int j = 0;j != 3;++j) { + if(zs::abs(verts(collisionGroupOffset,edge[i]) - kverts(kCollisionGroupOffset,ktri[j])) > 0.5) { + if(!among_different_group) + return; + if(!among_same_group) + return; + } + } + } + } + vec3 ktvs[3] = {}; for(int i = 0;i != 3;++i) ktvs[i] = kverts.pack(dim_c<3>,kxOffset,ktri[i]); diff --git a/projects/CuLagrange/pbd/CollisionSolver.cu b/projects/CuLagrange/pbd/CollisionSolver.cu index bead8b5bcc..7d6a1a6757 100644 --- a/projects/CuLagrange/pbd/CollisionSolver.cu +++ b/projects/CuLagrange/pbd/CollisionSolver.cu @@ -64,6 +64,13 @@ struct DetangleCCDCollisionWithBoundary : INode { auto kboundary = get_input2("boundary"); + auto among_same_group = get_input2("among_same_group"); + auto among_different_groups = get_input2("among_different_groups"); + + int group_strategy = 0; + group_strategy |= (among_same_group ? 1 : 0); + group_strategy |= (among_different_groups ? 2 : 0); + auto boundary_velocity_scale = get_input2("boundary_velocity_scale"); // auto current_kx_tag = get_input2("current_kx_tag"); // auto pre_kx_tag = get_input2("previous_kx_tag"); @@ -115,7 +122,8 @@ struct DetangleCCDCollisionWithBoundary : INode { auto cur_kvert = kverts.pack(dim_c<3>,"px",kvi) * (1 - w) + kverts.pack(dim_c<3>,"x",kvi) * w; auto pre_kvert = kverts.pack(dim_c<3>,"px",kvi) * (1 - pw) + kverts.pack(dim_c<3>,"x",kvi) * pw; vtemp("collision_group",kvi + voffset) = kverts(collision_group_name,kvi); - vtemp.tuple(dim_c<3>,"X",kvi + voffset) = kverts.pack(dim_c<3>,"X",kvi); + // for alignment, we directly assign the current boundary as reference shape + vtemp.tuple(dim_c<3>,"X",kvi + voffset) = kverts.pack(dim_c<3>,"x",kvi); vtemp.tuple(dim_c<3>,"x",kvi + voffset) = pre_kvert; vtemp.tuple(dim_c<3>,"v",kvi + voffset) = (cur_kvert - pre_kvert) * boundary_velocity_scale; vtemp("minv",kvi + voffset) = (T)0; @@ -208,7 +216,8 @@ struct DetangleCCDCollisionWithBoundary : INode { do_bvh_refit, csPT, impulse_buffer, - impulse_count,true,true); + impulse_count,true,true,false,group_strategy); + // std::cout << "nm_PT_continuous_collisions : " << csPT.size() << std::endl; } } @@ -243,7 +252,8 @@ struct DetangleCCDCollisionWithBoundary : INode { do_bvh_refit, csEE, impulse_buffer, - impulse_count,true,true); + impulse_count,true,true,false,group_strategy); + // std::cout << "nm_EE_continuous_collisions : " << csPT.size() << std::endl; } } @@ -317,6 +327,8 @@ ZENDEFNODE(DetangleCCDCollisionWithBoundary, {{{"zsparticles"}, {"int","substep_id","0"}, {"int","nm_substeps","1"}, {"float","boundary_velocity_scale","1"}, + {"bool","among_same_group","1"}, + {"bool","among_different_groups","1"} }, {{"zsparticles"}}, {}, diff --git a/projects/CuLagrange/pbd/ConstraintsBuilder.cu b/projects/CuLagrange/pbd/ConstraintsBuilder.cu index cad66159fe..d3f86b7f60 100644 --- a/projects/CuLagrange/pbd/ConstraintsBuilder.cu +++ b/projects/CuLagrange/pbd/ConstraintsBuilder.cu @@ -11,6 +11,7 @@ #include #include #include +#include "../../Utils.hpp" #include "constraint_function_kernel/constraint.cuh" #include "../geometry/kernel/tiled_vector_ops.hpp" @@ -65,6 +66,7 @@ virtual void apply() override { using vec3 = zs::vec; using vec4 = zs::vec; using vec9 = zs::vec; + using mat3 = zs::vec; using vec2i = zs::vec; using vec3i = zs::vec; using vec4i = zs::vec; @@ -88,10 +90,6 @@ virtual void apply() override { auto uniform_xpbd_affiliation = get_input2("xpbd_affiliation"); auto damping_coeff = get_input2("damping_coeff"); - - // auto paramsList = get_input("paramList")->getLiterial(); - - auto make_empty = get_input2("make_empty_constraint"); if(!make_empty) { @@ -107,12 +105,390 @@ virtual void apply() override { {"lambda",1}, {"damping_coeff",1}, {"tclr",1} - }, 0, zs::memsrc_e::device,0); + }, 0, zs::memsrc_e::device); auto &eles = constraint->getQuadraturePoints(); constraint->setMeta(CONSTRAINT_TARGET,source.get()); auto do_constraint_topological_coloring = get_input2("do_constraint_topological_coloring"); + + if(type == "shape_matching") { + constraint->setMeta(CONSTRAINT_KEY,category_c::shape_matching_constraint); + auto radii = get_input2("thickness"); + auto shape_matching_group_name = get_input2("group_name"); + + zs::bht shape_matching_set{verts.get_allocator(),verts.size()}; + shape_matching_set.reset(cudaPol,true); + + int nm_shapes = 1; + + auto has_group = verts.hasProperty(shape_matching_group_name); + int shape_group_offset = -1; + + constexpr auto exec_tag = wrapv{}; + + if(has_group) { + shape_group_offset = verts.getPropertyOffset(shape_matching_group_name); + + zs::Vector maxGroupID{verts.get_allocator(),1}; + maxGroupID.setVal(0); + + cudaPol(zs::range(verts.size()),[ + exec_tag = exec_tag, + verts = proxy({},verts), + maxGroupID = proxy(maxGroupID), + shape_group_offset = shape_group_offset] ZS_LAMBDA(int vi) mutable { + auto groupID = (int)verts(shape_group_offset,vi); + atomic_max(exec_tag,&maxGroupID[0],groupID); + }); + + auto maxGroupID_val = maxGroupID.getVal(0); + nm_shapes = maxGroupID_val + 1; + } + + std::cout << "shape_matching::nm_shapes : " << nm_shapes << std::endl; + + std::vector shape_matching_rest_cm((size_t)nm_shapes,vec3::zeros()); + std::vector shape_matching_weight_sum((size_t)nm_shapes,0.f); + + zs::Vector nm_vertices_every_shape{verts.get_allocator(),(size_t)nm_shapes}; + cudaPol(zs::range(nm_vertices_every_shape),[] ZS_LAMBDA(auto& cnt) mutable {cnt = 0;}); + + eles.resize(verts.size()); + eles.append_channels(cudaPol,{{"inds",1}}); + TILEVEC_OPS::fill(cudaPol,eles,"inds",zs::reinterpret_bits((int)-1)); + + zs::Vector shape_matching_shape_offsets{verts.get_allocator(),(size_t)(nm_shapes + 1)}; + cudaPol(zs::range(shape_matching_shape_offsets),[] ZS_LAMBDA(auto& offset) mutable {offset = 0;}); + + // find the number of groups + for(int shape_id = 0;shape_id < nm_shapes;++shape_id) { + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + eles = proxy({},eles), + shape_matching_set = proxy(shape_matching_set), + shape_id = shape_id, + has_group = has_group, + nm_vertices_every_shape = proxy(nm_vertices_every_shape), + shape_group_offset = shape_group_offset] ZS_LAMBDA(int vi) mutable { + if(has_group) { + auto groupID = (int)verts(shape_group_offset,vi); + if(groupID == shape_id) { + auto ei = shape_matching_set.insert(vi); + atomic_add(exec_tag,&nm_vertices_every_shape[shape_id],1); + eles("inds",ei) = zs::reinterpret_bits(vi); + } + } else { + auto ei = shape_matching_set.insert(vi); + atomic_add(exec_tag,&nm_vertices_every_shape[shape_id],1); + eles("inds",ei) = zs::reinterpret_bits(vi); + } + }); + } + + exclusive_scan(cudaPol,std::begin(nm_vertices_every_shape),std::end(nm_vertices_every_shape),std::begin(shape_matching_shape_offsets)); + shape_matching_shape_offsets.setVal(shape_matching_set.size(),nm_shapes); + + for(int shape_id = 0;shape_id < nm_shapes;++shape_id) { + zs::Vector restCm{verts.get_allocator(),1}; + restCm.setVal(vec3::zeros()); + zs::Vector wsum{verts.get_allocator(),1}; + wsum.setVal(static_cast(0)); + + // std::cout << "shapeMatching::compute barycentric point" << std::endl; + + auto shape_size = nm_vertices_every_shape.getVal(shape_id); + auto offset = shape_matching_shape_offsets.getVal(shape_id); + + // std::cout << "shape[" << shape_id << "] : " << shape_size << "\t" << offset << std::endl; + + cudaPol(zs::range(shape_size),[ + offset = shape_matching_shape_offsets.getVal(shape_id), + exec_tag = exec_tag, + eles = proxy({},eles), + xtagOffset = verts.getPropertyOffset("x"), + minvTagOffset = verts.getPropertyOffset("minv"), + verts = proxy({},verts), + restCm = proxy(restCm), + wsum = proxy(wsum)] ZS_LAMBDA(int vi_within_shape) mutable { + auto vi = zs::reinterpret_bits(eles("inds",vi_within_shape + offset)); + + auto xi = verts.pack(dim_c<3>,xtagOffset,vi); + auto minvi = verts(minvTagOffset,vi); + auto wi = static_cast(1.0) / (minvi + static_cast(1e-6)); + + auto xw = xi * wi; + + for(int d = 0;d != 3;++d) { + atomic_add(exec_tag,&restCm[0][d],xw[d]); + } + atomic_add(exec_tag,&wsum[0],wi); + }); + + auto rCm = restCm.getVal(0); + auto ws = wsum.getVal(0); + rCm = rCm / ws; + + // std::cout << "rCm[" << shape_id << "] : " << rCm[0] << "\t" << rCm[1] << "\t" << rCm[2] << std::endl; + + // shape_matching_rest_cm.setVal(rCm,shape_id); + shape_matching_rest_cm[shape_id] = rCm; + shape_matching_weight_sum[shape_id] = ws; + // shape_matching_weight_sum.setVal(ws,shape_id); + + } + + zs::Vector dAsBuffer{verts.get_allocator(),eles.size()}; + + constraint->setMeta(SHAPE_MATCHING_REST_CM,shape_matching_rest_cm); + constraint->setMeta(SHAPE_MATCHING_WEIGHT_SUM,shape_matching_weight_sum); + constraint->setMeta(SHAPE_MATCHING_SHAPE_OFFSET,shape_matching_shape_offsets); + constraint->setMeta(SHAPE_MATCHING_MATRIX_BUFFER,dAsBuffer); + + // std::cout << "shapeMatching::finish set aux data" << std::endl; + } + + if(type == "point_point_pin") { + // auto use_hard_constraint = get_input2("use") + constexpr auto eps = 1e-6; + constraint->setMeta(CONSTRAINT_KEY,category_c::vertex_pin_to_vertex_constraint); + + auto target = get_input("target"); + const auto& kverts = target->getParticles(); + constraint->setMeta(CONSTRAINT_TARGET,target.get()); + + auto search_radii = get_input2("search_radii"); + auto thickness = get_input2("thickness"); + + auto pin_group_name = get_input2("group_name"); + if(!verts.hasProperty("pinSuccess")) + verts.append_channels(cudaPol,{{"pinSuccess",1}}); + TILEVEC_OPS::fill(cudaPol,verts,"pinSuccess",0.f); + + zs::Vector> point_topos{verts.get_allocator(),0}; + if(verts.hasProperty(pin_group_name)) { + // std::cout << "binder name : " << pin_group_name << std::endl; + zs::bht pin_point_set{verts.get_allocator(),verts.size()}; + pin_point_set.reset(cudaPol,true); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + eps = eps, + gname = zs::SmallString(pin_group_name), + pin_point_set = proxy(pin_point_set)] ZS_LAMBDA(int vi) mutable { + auto gtag = verts(gname,vi); + if(gtag > eps) + pin_point_set.insert(vi); + }); + point_topos.resize(pin_point_set.size()); + cudaPol(zip(zs::range(pin_point_set.size()),pin_point_set._activeKeys),[ + point_topos = proxy(point_topos)] ZS_LAMBDA(auto id,const auto& pvec) mutable { + point_topos[id] = pvec[0]; + }); + }else { + point_topos.resize(verts.size()); + cudaPol(zip(zs::range(point_topos.size()),point_topos),[] ZS_LAMBDA(const auto& id,auto& pi) mutable {pi = id;}); + } + + if(do_constraint_topological_coloring) { + topological_coloring(cudaPol,point_topos,colors,false); + sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); + } + + eles.append_channels(cudaPol,{{"inds",2},{"r",1}}); + eles.resize(point_topos.size()); + TILEVEC_OPS::fill(cudaPol,eles,"inds",zs::reinterpret_bits((int)-1)); + TILEVEC_OPS::fill(cudaPol,eles,"r",0.f); + + auto kpBvh = bvh_t{}; + auto kpBvs = retrieve_bounding_volumes(cudaPol,kverts,search_radii / 2.0f,"x"); + kpBvh.build(cudaPol,kpBvs); + + cudaPol(zs::range(point_topos.size()),[ + point_topos = proxy(point_topos), + do_constraint_topological_coloring = do_constraint_topological_coloring, + reordered_map = proxy(reordered_map), + verts = proxy({},verts), + kpBvh = proxy(kpBvh), + search_radii = search_radii, + thickness = thickness, + // use_hard_constraint = use_hard_constraint, + eles = proxy({},eles), + kverts = proxy({},kverts)] ZS_LAMBDA(int oei) mutable { + auto ei = do_constraint_topological_coloring ? reordered_map[oei] : oei; + auto pi = point_topos[ei][0]; + + auto p = verts.pack(dim_c<3>,"x",pi); + auto bv = bv_t{get_bounding_box(p - search_radii / 2.f,p + search_radii / 2.f)}; + + int closest_kvi = -1; + auto closest_distance = std::numeric_limits::max(); + auto find_closest_point = [&](int kvi) mutable { + auto kp = kverts.pack(dim_c<3>,"x",kvi); + auto dist = (kp - p).norm(); + if(dist < closest_distance) { + closest_kvi = kvi; + closest_distance = dist; + } + }; + kpBvh.iter_neighbors(bv,find_closest_point); + eles.tuple(dim_c<2>,"inds",oei) = vec2i{pi,closest_kvi}.reinterpret_bits(float_c); + eles("r",oei) = thickness + closest_distance; + if(closest_kvi >= 0) { + // printf("vertex2vertex_pin : %d %d\n",pi,closest_kvi); + verts("pinSuccess",pi) = 1.f; + } + }); + } + + if(type == "lra_stretch") { + constraint->setMeta(CONSTRAINT_KEY,category_c::long_range_attachment); + + auto radii = get_input2("thickness"); + auto attach_group_name = get_input2("group_name"); + auto has_group = verts.hasProperty(attach_group_name); + + // constexpr auto eps = 1e-6; + // auto attach2target = get_input("target"); + + if(!has_group) { + std::cout << "the input vertices has no specified group tag : " << attach_group_name << std::endl; + throw std::runtime_error("the input vertices has no specified LRA group"); + } + + zs::bht attachAnchors{verts.get_allocator(),(size_t)verts.size()}; + attachAnchors.reset(cudaPol,true); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + attachAnchors = proxy(attachAnchors)] ZS_LAMBDA(int vi) mutable { + if(verts("minv",vi) == 0.) + attachAnchors.insert(vi); + }); + + auto nmAttachAnchors = attachAnchors.size(); + dtiles_t vtemp{verts.get_allocator(),{ + {"x",3}, + {"id",1} + },nmAttachAnchors}; + + // std::cout << "nmAttachAnchors : " << nmAttachAnchors << std::endl; + + cudaPol(zip(zs::range(nmAttachAnchors),attachAnchors._activeKeys),[ + verts = proxy({},verts), + // thickness = thickness, + vtemp = proxy({},vtemp)] ZS_LAMBDA(auto ci,const auto& pvec) mutable { + auto vi = pvec[0]; + vtemp.tuple(dim_c<3>,"x",ci) = verts.pack(dim_c<3>,"x",vi); + vtemp("id",ci) = zs::reinterpret_bits(vi); + }); + + auto attBvh = bvh_t{}; + auto attBvs = retrieve_bounding_volumes(cudaPol,vtemp,radii,"x"); + attBvh.build(cudaPol,attBvs); + + zs::Vector maxNmPairsBuffer{verts.get_allocator(),1}; + maxNmPairsBuffer.setVal(0); + + constexpr auto exec_tag = wrapv{}; + + cudaPol(zs::range(verts.size()),[ + exec_tag = exec_tag, + verts = proxy({},verts), + vtemp = proxy({},vtemp), + attBvh = proxy(attBvh), + thickness = radii, + maxNmPairsBuffer = proxy(maxNmPairsBuffer), + attach_group_name = zs::SmallString(attach_group_name)] ZS_LAMBDA(int vi) mutable { + auto p = verts.pack(dim_c<3>,"x",vi); + if(verts("minv",vi) == 0.0) + return; + + auto bv = bv_t(p,p); + + auto do_close_proximity_detection = [&](int ai) mutable { + auto ap = vtemp.pack(dim_c<3>,"x",ai); + auto avi = zs::reinterpret_bits(vtemp("id",ai)); + if(avi == vi) + return; + + auto groupVi = verts(attach_group_name,vi); + auto groupAVi = verts(attach_group_name,avi); + if(zs::abs(groupVi - groupAVi) > 0.5) + return; + + // we need to switch the distance evaluation from euclidean distance to geodesic one + auto dist = (ap - p).norm(); + if(dist < thickness) { + atomic_add(exec_tag,&maxNmPairsBuffer[0],1); + } + }; + attBvh.iter_neighbors(bv,do_close_proximity_detection); + }); + + auto maxNmPairs = maxNmPairsBuffer.getVal(0); + // std::cout << "maxNmPairs : " << maxNmPairs << std::endl; + zs::bht attachPairs{verts.get_allocator(),(size_t)maxNmPairs}; + attachPairs.reset(cudaPol,true); + + cudaPol(zs::range(verts.size()),[ + // exec_tag = exec_tag, + verts = proxy({},verts), + vtemp = proxy({},vtemp), + attBvh = proxy(attBvh), + thickness = radii, + attachPairs = proxy(attachPairs), + attach_group_name = zs::SmallString(attach_group_name)] ZS_LAMBDA(int vi) mutable { + auto p = verts.pack(dim_c<3>,"x",vi); + auto bv = bv_t(p,p); + + auto do_close_proximity_detection = [&](int ai) mutable { + auto ap = vtemp.pack(dim_c<3>,"x",ai); + auto avi = zs::reinterpret_bits(vtemp("id",ai)); + if(avi == vi) + return; + + auto groupVi = verts(attach_group_name,vi); + auto groupAVi = verts(attach_group_name,avi); + if(zs::abs(groupVi - groupAVi) > 0.5) + return; + + // we need to switch the distance evaluation from euclidean distance to geodesic one + auto dist = (ap - p).norm(); + if(dist < thickness) { + attachPairs.insert(vec2i(vi,avi)); + } + }; + attBvh.iter_neighbors(bv,do_close_proximity_detection); + }); + + auto rest_scale = get_input2("rest_scale"); + // std::cout << "number of attach pairs : " << attachPairs.size() << std::endl; + eles.resize(attachPairs.size()); + eles.append_channels(cudaPol,{{"inds",2},{"r",1}}); + cudaPol(zip(zs::range(attachPairs.size()),attachPairs._activeKeys),[ + rest_scale = rest_scale, + eles = proxy({},eles), + verts = proxy({},verts)] ZS_LAMBDA(auto ai,const auto& pair) mutable { + eles.tuple(dim_c<2>,"inds",ai) = pair.reinterpret_bits(); + auto v0 = verts.pack(dim_c<3>,"x",pair[0]); + auto v1 = verts.pack(dim_c<3>,"x",pair[1]); + eles("r",ai) = (v0 - v1).norm() * rest_scale; + }); + + zs::Vector> point_topos{verts.get_allocator(),0}; + point_topos.resize(eles.size()); + cudaPol(zip(zs::range(attachPairs.size()),attachPairs._activeKeys),[ + point_topos = proxy(point_topos)] ZS_LAMBDA(auto ai,const auto& pair) mutable { + point_topos[ai] = pair[0]; + }); + + if(do_constraint_topological_coloring) { + topological_coloring(cudaPol,point_topos,colors); + sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); + } + } + if(type == "stretch") { constraint->setMeta(CONSTRAINT_KEY,category_c::edge_length_constraint); auto quads_vec = tilevec_topo_to_zsvec_topo(cudaPol,quads,wrapv<3>{}); @@ -131,10 +507,10 @@ virtual void apply() override { 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},{"rest_scale",1}}); + eles.append_channels(cudaPol,{{"inds",2},{"r",1}}); auto rest_scale = get_input2("rest_scale"); - TILEVEC_OPS::fill(cudaPol,eles,"rest_scale",rest_scale); + // TILEVEC_OPS::fill(cudaPol,eles,"rest_scale",rest_scale); cudaPol(zs::range(eles.size()),[ has_group = has_group, @@ -170,61 +546,96 @@ virtual void apply() override { if(type == "follow_animation_constraint") { constexpr auto eps = 1e-6; + auto use_hard_constraint = get_input2("use_hard_constraint"); constraint->setMeta(CONSTRAINT_KEY,category_c::follow_animation_constraint); + constraint->setMeta(PBD_USE_HARD_CONSTRAINT,use_hard_constraint); + if(!has_input("target")) { std::cout << "no target specify while adding follow animation constraint" << std::endl; throw std::runtime_error("no target specify while adding follow animation constraint"); } + + auto animaskGroupName = get_input2("group_name"); + if(!verts.hasProperty(animaskGroupName)) { + std::cout << "the zcloth should has \'ani_mask\' nodal attribute" << std::endl; + throw std::runtime_error("the zcloth should has \'ani_mask\' nodal attribute"); + } + auto target = get_input("target"); if(target->getParticles().size() != verts.size()) { std::cout << "the size of target and the cloth not match : " << target->getParticles().size() << "\t" << source->getParticles().size() << std::endl; throw std::runtime_error("the size of the target and source not matched"); } const auto& kverts = target->getParticles(); - if(!kverts.hasProperty("ani_mask")) { - std::cout << "the animation target should has \'ani_mask\' nodal attribute" << std::endl; - throw std::runtime_error("the animation target should has \'ani_mask\' nodal attribute"); - } - zs::Vector> point_topos{quads.get_allocator(),0}; - point_topos.resize(verts.size()); - cudaPol(zip(zs::range(point_topos.size()),point_topos),[] ZS_LAMBDA(const auto& id,auto& pi) mutable {pi = id;}); + zs::Vector> point_topos{verts.get_allocator(),0}; + zs:bht pin_point_set{verts.get_allocator(),verts.size()}; + pin_point_set.reset(cudaPol,true); + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + gname = zs::SmallString(animaskGroupName), + pin_point_set = proxy(pin_point_set)] ZS_LAMBDA(int vi) mutable { + auto gtag = verts(gname,vi); + if(gtag > 1e-6) + pin_point_set.insert(vi); + }); + + point_topos.resize(pin_point_set.size()); + cudaPol(zip(zs::range(pin_point_set.size()),pin_point_set._activeKeys),[ + point_topos = proxy(point_topos) + ] ZS_LAMBDA(const auto& id,const auto& pvec) mutable { + point_topos[id] = pvec; + }); // std::cout << "nm binder point : " << point_topos.size() << std::endl; if(do_constraint_topological_coloring) { topological_coloring(cudaPol,point_topos,colors,false); sort_topology_by_coloring_tag(cudaPol,colors,reordered_map,color_offset); } - eles.resize(verts.size()); + eles.resize(point_topos.size()); // we need an extra 'inds' tag, in case the source and animation has different topo eles.append_channels(cudaPol,{{"inds",1},{"follow_weight",1}}); cudaPol(zs::range(eles.size()),[ - kverts = proxy({},kverts), + verts = proxy({},verts), + animaskOffset = verts.getPropertyOffset(animaskGroupName), do_constraint_topological_coloring = do_constraint_topological_coloring, + use_hard_constraint = use_hard_constraint, reordered_map = proxy(reordered_map), point_topos = proxy(point_topos), eles = proxy({},eles)] ZS_LAMBDA(int oei) mutable { auto ei = do_constraint_topological_coloring ? reordered_map[oei] : oei; auto pi = point_topos[ei][0]; - auto am = kverts("ani_mask",pi); + auto am = verts(animaskOffset,pi); am = am > 1 ? 1 : am; am = am < 0 ? 0 : am; - eles("follow_weight",oei) = 1 - am; + + if(use_hard_constraint) { + am = am > 0.5 ? 1 : 0; + } + eles("follow_weight",oei) = am; eles("inds",oei) = zs::reinterpret_bits(pi); + if(use_hard_constraint) + verts("minv",pi) = am > 0.5 ? 0 : verts("minv",pi); }); // not sure about effect by increasing the nodal mass - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - kverts = proxy({},kverts)] ZS_LAMBDA(int vi) mutable { - verts("minv",vi) = (1 - kverts("ani_mask",vi)) * verts("minv",vi); - }); + // cudaPol(zs::range(eles.size()),[ + // verts = proxy({},verts), + // use_hard_constraint = use_hard_constraint, + // followWeightOffset = eles.getPropertyOffset("follow_weight"), + // eles = proxy({},eles)] ZS_LAMBDA(int ei) mutable { + // auto vi = reinterpret_bits(eles("inds",ei)); + // if(use_hard_constraint) + // verts("minv",vi) = eles(followWeightOffset,ci) > 0.5 ? 0 : verts("minv",vi); + // }); // TILEVEC_OPS::copy(cudaPol,eles,"inds",eles,"vis_inds"); constraint->setMeta(CONSTRAINT_TARGET,target.get()); } if(type == "reference_dcd_collision_constraint") { + constexpr auto exec_tag = wrapv{}; constexpr auto eps = 1e-6; constexpr auto MAX_IMMINENT_COLLISION_PAIRS = 200000; auto dcd_source_xtag = get_input2("dcd_source_xtag"); @@ -232,6 +643,28 @@ virtual void apply() override { eles.append_channels(cudaPol,{{"inds",4},{"bary",4},{"type",1}}); eles.resize(MAX_IMMINENT_COLLISION_PAIRS); + if(!source->hasAuxData(DCD_COUNTER_BUFFER)) { + (*source)[DCD_COUNTER_BUFFER] = dtiles_t{verts.get_allocator(),{ + {"cnt",1} + },verts.size()}; + } + if(!source->hasAuxData(COLLISION_BUFFER)) { + (*source)[COLLISION_BUFFER] = dtiles_t{verts.get_allocator(),{ + {"inds",4},{"bary",4},{"type",1} + },MAX_IMMINENT_COLLISION_PAIRS}; + } + auto& collision_buffer = (*source)[COLLISION_BUFFER]; + auto& collision_counter = (*source)[DCD_COUNTER_BUFFER]; + // cudaPol(zs::range(collision_counter),[] ZS_LAMBDA(auto& cnt) {cnt = 0;}); + TILEVEC_OPS::fill(cudaPol,collision_counter,"cnt",0.f); + + auto among_same_group = get_input2("among_same_group"); + auto among_different_groups = get_input2("among_different_groups"); + + int group_strategy = 0; + group_strategy |= (among_same_group ? 1 : 0); + group_strategy |= (among_different_groups ? 2 : 0); + const auto &edges = (*source)[ZenoParticles::s_surfEdgeTag]; auto has_input_collider = has_input("target"); @@ -348,8 +781,19 @@ virtual void apply() override { }); } - zs::bht csPT{verts.get_allocator(),(size_t)MAX_IMMINENT_COLLISION_PAIRS};csPT.reset(cudaPol,true); - zs::bht csEE{edges.get_allocator(),(size_t)MAX_IMMINENT_COLLISION_PAIRS};csEE.reset(cudaPol,true); + if(!source->hasMeta(COLLSIION_CSPT_SET)) { + source->setMeta(COLLSIION_CSPT_SET,zs::bht{verts.get_allocator(),(size_t)MAX_IMMINENT_COLLISION_PAIRS}); + } + auto&csPT = source->readMeta &>(COLLSIION_CSPT_SET); + csPT.reset(cudaPol,true); + + if(!source->hasMeta(COLLISION_CSEE_SET)) { + source->setMeta(COLLISION_CSEE_SET,zs::bht{verts.get_allocator(),(size_t)MAX_IMMINENT_COLLISION_PAIRS}); + } + auto&csEE = source->readMeta &>(COLLISION_CSEE_SET); + csEE.reset(cudaPol,true); + + // zs::bht csEE{edges.get_allocator(),(size_t)MAX_IMMINENT_COLLISION_PAIRS};csEE.reset(cudaPol,true); auto triBvh = bvh_t{}; @@ -361,10 +805,11 @@ virtual void apply() override { imminent_collision_thickness, 0, triBvh, - eles, + collision_buffer, csPT, true, - true); + true, + group_strategy); // std::cout << "nm_imminent_csPT : " << csPT.size() << std::endl; @@ -378,33 +823,36 @@ virtual void apply() override { imminent_collision_thickness, csPT.size(), edgeBvh, - eles,csEE, + collision_buffer,csEE, true, - true); + true, + group_strategy); + + // std::cout << "nm_imminent_csEE : " << csEE.size() << std::endl; - // std::cout << "csEE + csPT = " << csPT.size() + csEE.size() << std::endl; - if(!verts.hasProperty("dcd_collision_tag")) - verts.append_channels(cudaPol,{{"dcd_collision_tag",1}}); - cudaPol(zs::range(verts.size()),[ + auto nm_dcd_collisions = csPT.size() + csEE.size(); + eles.resize(nm_dcd_collisions); + TILEVEC_OPS::copy(cudaPol,collision_buffer,"inds",eles,"inds"); + TILEVEC_OPS::copy(cudaPol,collision_buffer,"bary",eles,"bary"); + TILEVEC_OPS::copy(cudaPol,collision_buffer,"type",eles,"type"); + + + cudaPol(zs::range(nm_dcd_collisions),[ + nm_verts = verts.size(), + exec_tag = exec_tag, verts = proxy({},verts), - vtemp = proxy({},vtemp)] ZS_LAMBDA(int vi) mutable { - verts("dcd_collision_tag",vi) = vtemp("dcd_collision_tag",vi); + minvOffset = verts.getPropertyOffset("minv"), + indsOffset = eles.getPropertyOffset("inds"), + collision_counter = proxy({},collision_counter), + eles = proxy({},eles)] ZS_LAMBDA(const auto& ci) mutable { + auto inds = eles.pack(dim_c<4>,indsOffset,ci,int_c); + for(int i = 0;i != 4;++i) + if(inds[i] < nm_verts && verts(minvOffset,inds[i]) > 1e-5 && inds[i] > -1) { + atomic_add(exec_tag,&collision_counter("cnt",inds[i]),1.f); + } }); - if(has_input_collider) { - auto collider = get_input("target"); - auto& kverts = collider->getParticles(); - if(!kverts.hasProperty("dcd_collision_tag")) - kverts.append_channels(cudaPol,{{"dcd_collision_tag",1}}); - cudaPol(zs::range(kverts.size()),[ - kverts = proxy({},kverts), - voffset = verts.size(), - vtemp = proxy({},vtemp)] ZS_LAMBDA(int kvi) mutable { - kverts("dcd_collision_tag",kvi) = vtemp("dcd_collision_tag",kvi + voffset); - }); - } - - constraint->setMeta(NM_DCD_COLLISIONS,csEE.size() + csPT.size()); + constraint->setMeta(NM_DCD_COLLISIONS,(size_t)nm_dcd_collisions); constraint->setMeta(GLOBAL_DCD_THICKNESS,imminent_collision_thickness); } @@ -579,7 +1027,6 @@ virtual void apply() override { proximity_buffer(typeOffset,id + buffer_offset) = zs::reinterpret_bits((int)1); proximity_buffer.tuple(dim_c<3>,hitPointOffset,id + buffer_offset) = kp; proximity_buffer.tuple(dim_c<3>,hitVelocityOffset,id + buffer_offset) = kv; - // proximity_buffer.tuple(dim_c<3>,hitNormalOffset,id) = hit_normal; }); @@ -628,12 +1075,6 @@ virtual void apply() override { hit_point = bary[2] * kps[0] + bary[3] * kps[1]; hit_velocity = bary[2] * kvs[0] + bary[3] * kvs[1]; - // auto hit_normal = bary[0] * ps[0] + bary[1] * ps[1] + bary[2] * kps[0] + bary[3] * kps[1]; - // if(hit_normal.norm() > eps) - // hit_normal = hit_normal.normalized(); - // else - // hit_normal = (ps[1] - ps[0]).cross(kps[1] - kps[0]).normalized(); - proximity_buffer.tuple(dim_c<4>,baryOffset,id + buffer_offset) = bary; proximity_buffer.tuple(dim_c<4>,indsOffset,id + buffer_offset) = inds.reinterpret_bits(float_c); proximity_buffer(typeOffset,id + buffer_offset) = zs::reinterpret_bits((int)2); @@ -848,7 +1289,7 @@ virtual void apply() override { auto ws = compute_vertex_tetrahedron_barycentric_weights(p,ktps[0],ktps[1],ktps[2],ktps[3]); T epsilon = zs::limits::epsilon(); - if(ws[0] > epsilon && ws[1] > epsilon && ws[2] > epsilon && ws[3] > epsilon){ + if(ws[0] > -epsilon && ws[1] > -epsilon && ws[2] > -epsilon && ws[3] > -epsilon){ embed_kti = kti; bary = ws; found = true; @@ -869,8 +1310,11 @@ virtual void apply() override { } if(type == "point_cell_pin") { + auto use_hard_constraint = get_input2("use_hard_constraint"); + constexpr auto eps = 1e-6; constraint->setMeta(CONSTRAINT_KEY,category_c::vertex_pin_to_cell_constraint); + constraint->setMeta(PBD_USE_HARD_CONSTRAINT,use_hard_constraint); auto target = get_input("target"); @@ -889,6 +1333,7 @@ virtual void apply() override { if(!target->hasAuxData(TARGET_CELL_BUFFER)) { (*target)[TARGET_CELL_BUFFER] = dtiles_t{kverts.get_allocator(),{ {"cx",3}, + // {"px",3}, {"x",3}, {"v",3}, {"nrm",3} @@ -919,32 +1364,37 @@ virtual void apply() override { zs::bht binder_set{verts.get_allocator(),verts.size()}; binder_set.reset(cudaPol,true); + if(!verts.hasProperty("pinSuccess")) + verts.append_channels(cudaPol,{{"pinSuccess",1}}); + TILEVEC_OPS::fill(cudaPol,verts,"pinSuccess",0.f); + cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - has_pin_group = has_pin_group, - eps = eps, - pin_group_name = zs::SmallString(pin_group_name), - ktris = proxy({},ktris), - kverts = proxy({},kverts), - binder_set = proxy(binder_set), - binder_buffer = proxy({},binder_buffer), - cell_buffer = proxy({},cell_buffer), - cellBvh = proxy(cellBvh)] ZS_LAMBDA(int vi) mutable { - auto p = verts.pack(dim_c<3>,"x",vi); - if(verts("minv",vi) < eps) - return; + use_hard_constraint = use_hard_constraint, + verts = proxy({},verts), + has_pin_group = has_pin_group, + eps = eps, + pin_group_name = zs::SmallString(pin_group_name), + ktris = proxy({},ktris), + kverts = proxy({},kverts), + binder_set = proxy(binder_set), + binder_buffer = proxy({},binder_buffer), + cell_buffer = proxy({},cell_buffer), + cellBvh = proxy(cellBvh)] ZS_LAMBDA(int vi) mutable { + auto p = verts.pack(dim_c<3>,"x",vi); + if(verts("minv",vi) < eps && !use_hard_constraint) + return; + + if(has_pin_group && verts(pin_group_name,vi) < eps) { + // printf("ignore V[%d] excluded by pingroup\n",vi); + return; + } + auto bv = bv_t{p,p}; + int closest_kti = -1; + T closest_toc = std::numeric_limits::max(); + zs::vec closest_bary = {}; + T min_toc_dist = std::numeric_limits::max(); - if(has_pin_group && verts(pin_group_name,vi) < eps) { - // printf("ignore V[%d] excluded by pingroup\n",vi); - return; - } - auto bv = bv_t{p,p}; - int closest_kti = -1; - T closest_toc = std::numeric_limits::max(); - zs::vec closest_bary = {}; - T min_toc_dist = std::numeric_limits::max(); - - auto do_close_proximity_detection = [&](int kti) mutable { + auto do_close_proximity_detection = [&](int kti) mutable { auto ktri = ktris.pack(dim_c<3>,"inds",kti,int_c); vec3 as[3] = {}; vec3 bs[3] = {}; @@ -956,7 +1406,7 @@ virtual void apply() override { zs::vec prism_bary{}; T toc{}; - if(!compute_vertex_prism_barycentric_weights(p,as[0],as[1],as[2],bs[0],bs[1],bs[2],toc,prism_bary,(T)0.1)) + if(!compute_vertex_prism_barycentric_weights(p,as[0],as[1],as[2],bs[0],bs[1],bs[2],toc,prism_bary,(T)0.0001)) return; auto toc_dist = zs::abs(toc - (T)0.5); @@ -966,16 +1416,41 @@ virtual void apply() override { closest_bary = prism_bary; closest_kti = kti; } - }; - cellBvh.iter_neighbors(bv,do_close_proximity_detection); - if(closest_kti >= 0) { + }; + cellBvh.iter_neighbors(bv,do_close_proximity_detection); + if(closest_kti >= 0) { auto id = binder_set.insert(vec2i{vi,closest_kti}); binder_buffer.tuple(dim_c<6>,"bary",id) = closest_bary; + verts("pinSuccess",vi) = 1.f; + if(use_hard_constraint) + verts("minv",vi) = 0; + + auto ktri = ktris.pack(dim_c<3>,"inds",closest_kti,int_c); + vec3 as[3] = {}; + vec3 bs[3] = {}; + + for(int i = 0;i != 3;++i) { + as[i] = cell_buffer.pack(dim_c<3>,"x",ktri[i]); + bs[i] = cell_buffer.pack(dim_c<3>,"v",ktri[i]) + as[i]; + } + + auto tp = vec3::zeros(); + for(int i = 0;i != 3;++i) { + tp += as[i] * closest_bary[i]; + tp += bs[i] * closest_bary[i + 3]; + } + + auto diffp = (p - tp).norm(); + if(diffp > 0.1) { + printf("too big prism and point difference : %d %d %f\n",vi,closest_kti,diffp); + } } }); eles.append_channels(cudaPol,{{"inds",2},{"bary",6}}); eles.resize(binder_set.size()); + + std::cout << "nunber of cell pin anchors : " << eles.size() << std::endl; cudaPol(zip(zs::range(binder_set.size()),binder_set._activeKeys),[ binder_buffer = proxy({},binder_buffer), @@ -1016,8 +1491,8 @@ virtual void apply() override { point_topos[id] = pvec[0]; }); - std::cout << "binder name : " << pin_point_group_name << std::endl; - std::cout << "nm binder point : " << point_topos.size() << std::endl; + // std::cout << "binder name : " << pin_point_group_name << std::endl; + // std::cout << "nm binder point : " << point_topos.size() << std::endl; if(do_constraint_topological_coloring) { topological_coloring(cudaPol,point_topos,colors,false); @@ -1157,8 +1632,8 @@ virtual void apply() override { } // std::cout << "quads.size() = " << quads.size() << "\t" << "edge_topos.size() = " << edge_topos.size() << std::endl; - eles.append_channels(cudaPol,{{"inds",4},{"ra",1},{"sign",1},{"rest_scale",1}}); - TILEVEC_OPS::fill(cudaPol,eles,"rest_scale",rest_scale); + eles.append_channels(cudaPol,{{"inds",4},{"r",1}}); + // TILEVEC_OPS::fill(cudaPol,eles,"rest_scale",rest_scale); cudaPol(zs::range(eles.size()),[ eles = proxy({},eles), @@ -1177,8 +1652,8 @@ virtual void apply() override { float alpha{}; float alpha_sign{}; CONSTRAINT::init_DihedralBendingConstraint(x[0],x[1],x[2],x[3],rest_scale,alpha,alpha_sign); - eles("ra",oei) = alpha; - eles("sign",oei) = alpha_sign; + eles("r",oei) = alpha * rest_scale; + // eles("sign",oei) = alpha_sign; // auto topo = bd_topos[ei]; // zs::vec vis_inds { @@ -1260,6 +1735,7 @@ ZENDEFNODE(MakeSurfaceConstraintTopology, {{ {"string","dcd_source_xtag","px"}, {"string","dcd_collider_xtag","x"}, {"string","dcd_collider_pxtag","px"}, + {"float","search_radii","0.0"}, {"float","toc","0"}, {"bool","add_dcd_repulsion_force","1"}, {"float","relative_stiffness","1.0"}, @@ -1270,11 +1746,14 @@ ZENDEFNODE(MakeSurfaceConstraintTopology, {{ {"float","thickness","0.1"}, {"int","substep_id","0"}, {"int","nm_substeps","1"}, + {"int","max_constraint_pairs","1"}, {"bool","make_empty_constraint","0"}, {"bool","do_constraint_topological_coloring","1"}, {"float","damping_coeff","0.0"}, {"bool","enable_sliding","0"}, - {"bool","use_hard_constraint","0"} + {"bool","use_hard_constraint","0"}, + {"bool","among_same_group","1"}, + {"bool","among_different_groups","1"} }, {{"constraint"}}, { diff --git a/projects/CuLagrange/pbd/ConstraintsSolver.cu b/projects/CuLagrange/pbd/ConstraintsSolver.cu index cf92eafd50..7e18ac3065 100644 --- a/projects/CuLagrange/pbd/ConstraintsSolver.cu +++ b/projects/CuLagrange/pbd/ConstraintsSolver.cu @@ -21,6 +21,8 @@ #include "constraint_function_kernel/constraint_types.hpp" #include "../fem/collision_energy/evaluate_collision.hpp" +#include "zensim/math/matrix/QRSVD.hpp" + namespace zeno { @@ -170,7 +172,7 @@ struct XPBDSolve : INode { verts.tuple(dim_c<3>,ptag,pi) = kc + knrm * rd; } - if(category == category_c::edge_length_constraint || category == category_c::dihedral_spring_constraint) { + if(category == category_c::edge_length_constraint || category == category_c::dihedral_spring_constraint || category == category_c::long_range_attachment) { // printf("do xpbd solve\n"); auto edge = cquads.pack(dim_c<2>,"inds",coffset + gi,int_c); @@ -194,6 +196,7 @@ struct XPBDSolve : INode { // lambda, // dp0,dp1)) // return; + bool stretch_resistence_only = category == category_c::long_range_attachment; if(!CONSTRAINT::solve_DistanceConstraint( p0,minv0, p1,minv1, @@ -203,7 +206,8 @@ struct XPBDSolve : INode { kd, dt, lambda, - dp0,dp1)) + dp0,dp1, + stretch_resistence_only)) return; verts.tuple(dim_c<3>,ptag,edge[0]) = p0 + dp0; @@ -252,8 +256,8 @@ struct XPBDSolve : INode { minv[i] = verts("minv",quad[i]); } - auto ra = cquads("ra",coffset + gi); - auto ras = cquads("sign",coffset + gi); + auto ra = cquads("r",coffset + gi); + // auto ras = cquads("sign",coffset + gi); vec3 dp[4] = {}; // if(!CONSTRAINT::solve_DihedralConstraint( // p[0],minv[0], @@ -274,7 +278,7 @@ struct XPBDSolve : INode { p[3],minv[3], pp[0],pp[1],pp[2],pp[3], ra, - ras, + // ras, alpha, dt, kd, @@ -334,16 +338,20 @@ struct XPBDSolveSmooth : INode { // auto all_constraints = RETRIEVE_OBJECT_PTRS(ZenoParticles, "all_constraints"); auto constraints = get_input("constraints"); - // auto ptag = get_param("ptag"); + auto dptag = get_input2("dptag"); + auto ptag = get_input2("ptag"); + auto pptag = get_input2("pptag"); auto relaxs = get_input2("relaxation_strength"); auto& verts = zsparticles->getParticles(); + if(!verts.hasProperty(dptag)) + verts.append_channels(cudaPol,{{dptag,3}}); auto nm_smooth_iters = get_input2("nm_smooth_iters"); - 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;}); + // 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;}); @@ -369,91 +377,97 @@ struct XPBDSolveSmooth : INode { auto pw = (float)(substep_id) / (float)nm_substeps; auto nm_verts = verts.size(); - auto nm_tris = tris.size(); - auto nm_edges = edges.size(); - - if(has_input_collider) { - auto collider = constraints->readMeta(CONSTRAINT_TARGET,zs::wrapt{}); - nm_verts += collider->getParticles().size(); - nm_edges += (*collider)[ZenoParticles::s_surfEdgeTag].size(); - nm_tris += collider->getQuadraturePoints().size(); - } - - dtiles_t vtemp{verts.get_allocator(),{ - {"x",3}, - {"v",3}, - {"minv",1}, - {"m",1}, - // {"collision_cancel",1} - },nm_verts}; - - auto pptag = get_input2("pptag"); + // auto nm_tris = tris.size(); + // auto nm_edges = edges.size(); + + // if(has_input_collider) { + // auto collider = constraints->readMeta(CONSTRAINT_TARGET,zs::wrapt{}); + // nm_verts += collider->getParticles().size(); + // nm_edges += (*collider)[ZenoParticles::s_surfEdgeTag].size(); + // nm_tris += collider->getQuadraturePoints().size(); + // } + + // dtiles_t vtemp{verts.get_allocator(),{ + // {"x",3}, + // {"v",3}, + // {"minv",1}, + // {"m",1}, + // // {"collision_cancel",1} + // },nm_verts}; + + + + // TILEVEC_OPS::copy<3>(cudaPol,verts,pptag,vtemp,"x"); + // TILEVEC_OPS::copy(cudaPol,verts,"minv",vtemp,"minv"); + // TILEVEC_OPS::copy(cudaPol,verts,"m",vtemp,"m"); + + // cudaPol(zs::range(verts.size()),[ + // vtemp = proxy({},vtemp), + // pptag = zs::SmallString(pptag), + // verts = proxy({},verts)] ZS_LAMBDA(int vi) mutable { + // vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,"x",vi) - verts.pack(dim_c<3>,pptag,vi); + // }); + + // if(has_input_collider) { + // auto boundary_velocity_scale = get_input2("boundary_velocity_scale"); + + auto collider = constraints->readMeta(CONSTRAINT_TARGET,zs::wrapt{}); + const auto& kverts = collider->getParticles(); + const auto& kedges = (*collider)[ZenoParticles::s_surfEdgeTag]; + const auto& ktris = collider->getQuadraturePoints(); + + // auto voffset = verts.size(); + // cudaPol(zs::range(kverts.size()),[ + // kverts = proxy({},kverts), + // voffset = voffset, + // pw = pw, + // boundary_velocity_scale = boundary_velocity_scale, + // w = w, + // nm_substeps = nm_substeps, + // // hasKCollisionCancel = kverts.hasProperty("collision_cancel"), + // // kCollisionCancelOffset = kverts.getPropertyOffset("collision_cancel"), + // vtemp = proxy({},vtemp)] ZS_LAMBDA(int kvi) mutable { + // auto pre_kvert = kverts.pack(dim_c<3>,"px",kvi) * (1 - pw) + kverts.pack(dim_c<3>,"x",kvi) * pw; + // auto cur_kvert = kverts.pack(dim_c<3>,"px",kvi) * (1 - w) + kverts.pack(dim_c<3>,"x",kvi) * w; + // vtemp.tuple(dim_c<3>,"x",voffset + kvi) = pre_kvert; + // vtemp("minv",voffset + kvi) = 0; + // vtemp("m",voffset + kvi) = (T)1000; + // vtemp.tuple(dim_c<3>,"v",voffset + kvi) = (cur_kvert - pre_kvert) * boundary_velocity_scale; + // // if(hasKCollisionCancel) + // // vtemp("collision_cancel",voffset + kvi) = kverts("collision_cancel",kvi); + // }); + // } - TILEVEC_OPS::copy<3>(cudaPol,verts,pptag,vtemp,"x"); - TILEVEC_OPS::copy(cudaPol,verts,"minv",vtemp,"minv"); - TILEVEC_OPS::copy(cudaPol,verts,"m",vtemp,"m"); - // TILEVEC_OPS::fill(cudaPol,vtemp,"collision_cancel",0); - // if(verts.hasProperty("collision_cancel")) - // TILEVEC_OPS::copy(cudaPol,verts,"collision_cancel",vtemp,"collision_cancel"); - // else - // TILEVEC_OPS::fill(cudaPol,vtemp,"collision_cancel",0); - cudaPol(zs::range(verts.size()),[ - vtemp = proxy({},vtemp), - pptag = zs::SmallString(pptag), - verts = proxy({},verts)] ZS_LAMBDA(int vi) mutable { - vtemp.tuple(dim_c<3>,"v",vi) = verts.pack(dim_c<3>,"x",vi) - verts.pack(dim_c<3>,pptag,vi); - }); + auto add_repulsion_force = get_input2("add_repulsion_force"); - if(has_input_collider) { - auto boundary_velocity_scale = get_input2("boundary_velocity_scale"); + const auto& dp_count = (*zsparticles)[DCD_COUNTER_BUFFER]; - auto collider = constraints->readMeta(CONSTRAINT_TARGET,zs::wrapt{}); - const auto& kverts = collider->getParticles(); - const auto& kedges = (*collider)[ZenoParticles::s_surfEdgeTag]; - const auto& ktris = collider->getQuadraturePoints(); - - auto voffset = verts.size(); - cudaPol(zs::range(kverts.size()),[ - kverts = proxy({},kverts), - voffset = voffset, - pw = pw, - boundary_velocity_scale = boundary_velocity_scale, - w = w, - nm_substeps = nm_substeps, - // hasKCollisionCancel = kverts.hasProperty("collision_cancel"), - // kCollisionCancelOffset = kverts.getPropertyOffset("collision_cancel"), - vtemp = proxy({},vtemp)] ZS_LAMBDA(int kvi) mutable { - auto pre_kvert = kverts.pack(dim_c<3>,"px",kvi) * (1 - pw) + kverts.pack(dim_c<3>,"x",kvi) * pw; - auto cur_kvert = kverts.pack(dim_c<3>,"px",kvi) * (1 - w) + kverts.pack(dim_c<3>,"x",kvi) * w; - vtemp.tuple(dim_c<3>,"x",voffset + kvi) = pre_kvert; - vtemp("minv",voffset + kvi) = 0; - vtemp("m",voffset + kvi) = (T)1000; - vtemp.tuple(dim_c<3>,"v",voffset + kvi) = (cur_kvert - pre_kvert) * boundary_velocity_scale; - // if(hasKCollisionCancel) - // vtemp("collision_cancel",voffset + kvi) = kverts("collision_cancel",kvi); - }); - } - - auto add_repulsion_force = get_input2("add_repulsion_force"); + // std::cout << "nm_dcd_collisions : " << nm_dcd_collisions << std::endl; for(auto iter = 0;iter != nm_smooth_iters;++iter) { - cudaPol(zs::range(verts.size()),[ - dp_buffer = proxy(dp_buffer), - dp_count = proxy(dp_count)] ZS_LAMBDA(int vi) mutable { - for(int d = 0;d != 3;++d) - dp_buffer[vi * 3 + d] = 0; - dp_count[vi] = 0; - }); + + // cudaPol(zs::range(verts.size()),[ + // dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { + // for(int d = 0;d != 3;++d) + // dp_buffer[vi * 3 + d] = 0; + // // dp_count[vi] = 0; + // }); + TILEVEC_OPS::fill(cudaPol,verts,dptag,(T)0); cudaPol(zs::range(nm_dcd_collisions),[ cquads = proxy({},cquads), - vtemp = proxy({},vtemp), + verts = proxy({},verts), + dptagOffset = verts.getPropertyOffset(dptag), + ptagOffset = verts.getPropertyOffset(ptag), + pptagOffset = verts.getPropertyOffset(pptag), + kverts = proxy({},kverts), exec_tag = exec_tag, eps = eps, + pw = pw, + w = w, + nm_verts = nm_verts, add_repulsion_force = add_repulsion_force, - imminent_thickness = imminent_thickness, - dp_buffer = proxy(dp_buffer), - dp_count = proxy(dp_count)] ZS_LAMBDA(int ci) mutable { + imminent_thickness = imminent_thickness] ZS_LAMBDA(int ci) mutable { auto inds = cquads.pack(dim_c<4>,"inds",ci,int_c); auto bary = cquads.pack(dim_c<4>,"bary",ci); auto type = zs::reinterpret_bits(cquads("type",ci)); @@ -464,10 +478,21 @@ struct XPBDSolveSmooth : INode { vec4 ms{}; 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]); - minvs[i] = vtemp("minv",inds[i]); - ms[i] = vtemp("m",inds[i]); + if(inds[i] < nm_verts) { + ps[i] = verts.pack(dim_c<3>,pptagOffset,inds[i]); + vs[i] = verts.pack(dim_c<3>,ptagOffset,inds[i]) - verts.pack(dim_c<3>,pptagOffset,inds[i]); + minvs[i] = verts("minv",inds[i]); + ms[i] = verts("m",inds[i]); + } else { + auto kp = kverts.pack(dim_c<3>,"x",(size_t)(inds[i] - nm_verts)); + auto kpp = kverts.pack(dim_c<3>,"px",(size_t)(inds[i] - nm_verts)); + auto pre_kvert = kpp * (1 - pw) + kp * pw; + auto cur_kvert = kpp * (1 - w) + kp * w; + ps[i] = pre_kvert; + vs[i] = cur_kvert - pre_kvert; + minvs[i] = (T)0; + ms[i] = (T)1000; + } } vec3 imps[4] = {}; @@ -482,38 +507,51 @@ struct XPBDSolveSmooth : INode { add_repulsion_force)) return; for(int i = 0;i != 4;++i) { - if(minvs[i] < eps) + if(minvs[i] < eps || inds[i] >= nm_verts) continue; + - if(isnan(imps[i].norm())) { - printf("nan imps detected : %f %f %f %f %f %f %f\n", - (float)imps[i][0],(float)imps[i][1],(float)imps[i][2], - (float)bary[0],(float)bary[1],(float)bary[2],(float)bary[3]); - return; - } - atomic_add(exec_tag,&dp_count[inds[i]],(int)1); + // if(isnan(imps[i].norm())) { + // printf("nan imps detected : %f %f %f %f %f %f %f\nvs : %d %d %d %d\n%f\t%f\t%f\n%f\t%f\t%f\n%f\t%f\t%f\n%f\t%f\t%f\n", + // (float)imps[i][0],(float)imps[i][1],(float)imps[i][2], + // (float)bary[0],(float)bary[1],(float)bary[2],(float)bary[3], + // inds[0],inds[1],inds[2],inds[3], + // (float)ps[0][0],(float)ps[0][1],(float)ps[0][2], + // (float)ps[1][0],(float)ps[1][1],(float)ps[1][2], + // (float)ps[2][0],(float)ps[2][1],(float)ps[2][2], + // (float)ps[3][0],(float)ps[3][1],(float)ps[3][2]); + // return; + // } + // atomic_add(exec_tag,&dp_count[inds[i]],(int)1); + // printf("imps : %f\n",(float)imps[i].norm()); for(int d = 0;d != 3;++d) - atomic_add(exec_tag,&dp_buffer[inds[i] * 3 + d],imps[i][d]); + atomic_add(exec_tag,&verts(dptagOffset + d,inds[i]),imps[i][d]); } }); + // auto ndp = TILEVEC_OPS::dot<3>(cudaPol,verts,dptag,dptag); + // std::cout << "ndp : " << ndp << std::endl; + cudaPol(zs::range(verts.size()),[ - vtemp = proxy({},vtemp),relaxs = relaxs, - dp_count = proxy(dp_count), - dp_buffer = proxy(dp_buffer)] ZS_LAMBDA(int vi) mutable { - if(dp_count[vi] > 0) { - auto dp = relaxs * vec3{dp_buffer[vi * 3 + 0],dp_buffer[vi * 3 + 1],dp_buffer[vi * 3 + 2]}; - vtemp.tuple(dim_c<3>,"v",vi) = vtemp.pack(dim_c<3>,"v",vi) + dp / (T)dp_count[vi]; + verts = proxy({},verts), + relaxs = relaxs, + dp_count = proxy({},dp_count), + dptagOffset = verts.getPropertyOffset(dptag), + ptagOffset = verts.getPropertyOffset(ptag)] ZS_LAMBDA(int vi) mutable { + if(dp_count("cnt",vi) > 0.5) { + // auto dp = relaxs * vec3{dp_buffer[vi * 3 + 0],dp_buffer[vi * 3 + 1],dp_buffer[vi * 3 + 2]}; + auto dp = verts.pack(dim_c<3>,dptagOffset,vi) * relaxs; + // printf("update %d : %f %f\n",vi,(float)dp.norm(),dp_count("cnt",vi)); + verts.tuple(dim_c<3>,ptagOffset,vi) = verts.pack(dim_c<3>,ptagOffset,vi) + dp / (T)dp_count("cnt",vi); } }); - } - cudaPol(zs::range(verts.size()),[ - verts = proxy({},verts), - vtemp = proxy({},vtemp)] ZS_LAMBDA(int vi) mutable { - verts.tuple(dim_c<3>,"x",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); - }); + // cudaPol(zs::range(verts.size()),[ + // verts = proxy({},verts), + // vtemp = proxy({},vtemp)] ZS_LAMBDA(int vi) mutable { + // verts.tuple(dim_c<3>,"x",vi) = vtemp.pack(dim_c<3>,"x",vi) + vtemp.pack(dim_c<3>,"v",vi); + // }); } @@ -529,7 +567,9 @@ ZENDEFNODE(XPBDSolveSmooth, {{{"zsparticles"}, {"int","substep_id","0"}, {"bool","add_repulsion_force","0"}, {"float","boundary_velocity_scale","1"}, - {"string","pptag","px"} + {"string","ptag","x"}, + {"string","pptag","px"}, + {"string","dptag","dx"} }, {{"zsparticles"}}, {}, @@ -726,6 +766,7 @@ struct XPBDSolveSmoothAll : INode { using vec2 = zs::vec; using vec3 = zs::vec; using vec4 = zs::vec; + using mat3 = zs::vec; using vec2i = zs::vec; using vec3i = zs::vec; using vec4i = zs::vec; @@ -750,15 +791,143 @@ struct XPBDSolveSmoothAll : INode { if(!verts.hasProperty("w")) { verts.append_channels(cudaPol,{{"w",1}}); } - TILEVEC_OPS::fill(cudaPol,verts,"w",0); - + if(!verts.hasProperty(dptag)) + verts.append_channels(cudaPol,{{dptag,3}}); + TILEVEC_OPS::fill(cudaPol,verts,dptag,0); + + auto iter_id = get_input2("iter_id"); for(auto& constraint_ptr : constraint_ptr_list) { auto category = constraint_ptr->readMeta(CONSTRAINT_KEY,wrapt{}); const auto& cquads = constraint_ptr->getQuadraturePoints(); - if(category == category_c::edge_length_constraint || category == category_c::dihedral_bending_constraint) { + + if(constraint_ptr->userData().has("stride")) { + auto stride = objectToLiterial(constraint_ptr->userData().get("stride")); + // std::cout << "find constraint with stride = " << stride << std::endl; + // if(stride <= 0 && iter_id != 0) + // continue; + if(iter_id % stride != 0) { + // std::cout << "skip constraint solving due to stride-skipping" << std::endl; + continue; + } + } else { + // std::cout << "the constraint has no stride information" << std::endl; + } + + if(category == category_c::shape_matching_constraint) { + auto shape_matching_rest_cm = constraint_ptr->readMeta>(SHAPE_MATCHING_REST_CM); + auto shape_matching_weight_sum = constraint_ptr->readMeta>(SHAPE_MATCHING_WEIGHT_SUM); + auto shape_matching_offsets = constraint_ptr->readMeta>(SHAPE_MATCHING_SHAPE_OFFSET); + auto nm_shapes = shape_matching_rest_cm.size(); + + auto dAs = constraint_ptr->readMeta>(SHAPE_MATCHING_MATRIX_BUFFER); + + zs::Vector cmVec{verts.get_allocator(),1}; + + // auto wsum = constraint_ptr->readMeta(SHAPE_MATCHING_WEIGHT_SUM); + // auto restCM = constraint_ptr->readMeta(SHAPE_MATCHING_REST_CM); + for(int shape_id = 0;shape_id != nm_shapes;++shape_id) { + auto shape_size = shape_matching_offsets.getVal(shape_id + 1) - shape_matching_offsets.getVal(shape_id); + if(shape_size == 0) + continue; + + auto restCM = shape_matching_rest_cm[shape_id]; + auto wsum = shape_matching_weight_sum[shape_id]; + + cmVec.setVal(vec3::zeros()); + cudaPol(zs::range(shape_size),[ + exec_tag = exec_tag, + verts = proxy({},verts), + offset = shape_matching_offsets.getVal(shape_id), + ptagOffset = verts.getPropertyOffset(ptag), + minvOffset = verts.getPropertyOffset("minv"), + indsOffset = cquads.getPropertyOffset("inds"), + cquads = proxy({},cquads), + cmVec = proxy(cmVec)] ZS_LAMBDA(int ci) mutable { + auto vi = zs::reinterpret_bits(cquads(indsOffset,ci + offset)); + auto pi = verts.pack(dim_c<3>,ptagOffset,vi); + auto wi = static_cast(1.0) / (static_cast(1e-6) + verts(minvOffset,vi)); + auto pw = pi * wi; + for(int d = 0;d != 3;++d) + atomic_add(exec_tag,&cmVec[0][d],pw[d]); + }); + + auto cm = cmVec.getVal(0) / wsum; + // dAs.setVal(mat3::zeros()); + + cudaPol(zs::range(shape_size),[ + offset = shape_matching_offsets.getVal(shape_id), + cquads = proxy({},cquads), + indsOffset = cquads.getPropertyOffset("inds"), + verts = proxy({},verts), + XtagOffset = verts.getPropertyOffset("X"), + minvOffset = verts.getPropertyOffset("minv"), + ptagOffset = verts.getPropertyOffset(ptag), + restCM = restCM, + cm = cm, + dAs = proxy(dAs)] ZS_LAMBDA(int ci) mutable { + auto vi = zs::reinterpret_bits(cquads(indsOffset,ci + offset)); + auto q = verts.pack(dim_c<3>,XtagOffset,vi) - restCM; + auto p = verts.pack(dim_c<3>,ptagOffset,vi) - cm; + auto w = static_cast(1.0) / (verts(minvOffset,vi) + static_cast(1e-6)); + p *= w; + dAs[ci + offset] = dyadic_prod(p,q); + }); + + zs::Vector A{verts.get_allocator(),1}; + A.setVal(mat3::zeros()); + + cudaPol(zs::range(shape_size * 9),[ + exec_tag = exec_tag, + offset = shape_matching_offsets.getVal(shape_id), + A = proxy(A), + dAs = proxy(dAs)] ZS_LAMBDA(int dof) mutable { + auto dAid = dof / 9; + auto Aoffset = dof % 9; + auto r = Aoffset / 3; + auto c = Aoffset % 3; + const auto& dA = dAs[dAid + offset]; + atomic_add(exec_tag,&A[0][r][c],dA[r][c]); + }); + + auto Am = A.getVal(0); + Am /= wsum; + + auto [R,S] = math::polar_decomposition(Am); + cudaPol(zs::range(shape_size),[ + offset = shape_matching_offsets.getVal(shape_id), + cquads = proxy({},cquads), + stiffnessOffset = cquads.getPropertyOffset("relative_stiffness"), + R = R, + cm = cm, + restCM = restCM, + wOffset = verts.getPropertyOffset("w"), + dptagOffset = verts.getPropertyOffset(dptag), + verts = proxy({},verts), + XtagOffset = verts.getPropertyOffset("X"), + ptagOffset = verts.getPropertyOffset(ptag)] ZS_LAMBDA(int ci) mutable { + auto vi = zs::reinterpret_bits(cquads("inds",ci + offset)); + auto Xi = verts.pack(dim_c<3>,XtagOffset,vi); + auto w = cquads(stiffnessOffset,ci + offset); + auto goal = cm + R * (Xi - restCM); + + auto dp = goal - verts.pack(dim_c<3>,ptagOffset,vi); + + verts.tuple(dim_c<3>,dptagOffset,vi) = verts.pack(dim_c<3>,dptagOffset,vi) + dp * w; + verts(wOffset,vi) += w; + }); + + } + + } + + if(category == category_c::edge_length_constraint || category == category_c::dihedral_bending_constraint || category == category_c::long_range_attachment) { + // if(category == category_c::edge_length_constraint) + // std::cout << "solve edge length constraint" << std::endl; + // if(category == category_c::dihedral_bending_constraint) + // std::cout << "solve dihedral bending constraint" << std::endl; cudaPol(zs::range(cquads.size()),[ cquads = proxy({},cquads), dt = dt, @@ -766,8 +935,8 @@ struct XPBDSolveSmoothAll : INode { affiliationOffset = cquads.getPropertyOffset("xpbd_affiliation"), dampingOffset = cquads.getPropertyOffset("damping_coeff"), indsOffset = cquads.getPropertyOffset("inds"), - // rOffset = cquads.getPropertyOffset("r"), - restScaleOffset = cquads.getPropertyOffset("rest_scale"), + rOffset = cquads.getPropertyOffset("r"), + // restScaleOffset = cquads.getPropertyOffset("rest_scale"), // weight_sum = proxy(weight_sum), ptagOffset = verts.getPropertyOffset(ptag), pptagOffset = verts.getPropertyOffset(pptag), @@ -788,11 +957,13 @@ struct XPBDSolveSmoothAll : INode { auto pp1 = verts.pack(dim_c<3>,pptagOffset,edge[1]); auto minv0 = verts(minvOffset,edge[0]); auto minv1 = verts(minvOffset,edge[1]); - auto rest_scale = cquads(restScaleOffset,ci); - auto r = cquads("r",ci) * rest_scale; + // auto rest_scale = cquads(restScaleOffset,ci); + // auto r = cquads("r",ci) * rest_scale; + auto r = cquads(rOffset,ci); vec3 dp[2] = {}; auto lambda = (T)0; + bool do_stretch_resistence_only = (category == category_c::long_range_attachment); if(!CONSTRAINT::solve_DistanceConstraint( p0,minv0, p1,minv1, @@ -802,16 +973,17 @@ struct XPBDSolveSmoothAll : INode { kd, dt, lambda, - dp[0],dp[1])) + dp[0],dp[1], + do_stretch_resistence_only)) return; // printf("smooth stretch update : %f %f\n",(float)dp[0].norm(),(float)dp[1].norm()); for(int i = 0;i != 2;++i) { - if(isnan(dp[i].norm())) - printf("nan dp[%d] detected at stretch\n",i); + // if(isnan(dp[i].norm())) + // printf("nan dp[%d] detected at bending\n",i); // atomic_add(exec_tag,&weight_sum[edge[i]],w); atomic_add(exec_tag,&verts(wOffset,edge[i]),w); for(int d = 0;d != 3;++d) - atomic_add(exec_tag,&verts(dptagOffset + d,edge[i]),dp[i][d] * w); + atomic_add(exec_tag,&verts(dptagOffset + d,edge[i]),dp[i][d]); } } @@ -825,9 +997,10 @@ struct XPBDSolveSmoothAll : INode { pp[i] = verts.pack(dim_c<3>,pptagOffset,quad[i]); minv[i] = verts(minvOffset,quad[i]); } - auto rest_scale = cquads(restScaleOffset,ci); - auto ra = cquads("ra",ci) * rest_scale; - auto ras = cquads("sign",ci); + // auto rest_scale = cquads(restScaleOffset,ci); + // auto ra = cquads("ra",ci) * rest_scale; + // auto ras = cquads("sign",ci); + auto ra = cquads(rOffset,ci); vec3 dp[4] = {}; auto lambda = (T)0; if(!CONSTRAINT::solve_DihedralConstraint( @@ -837,7 +1010,7 @@ struct XPBDSolveSmoothAll : INode { p[3],minv[3], pp[0],pp[1],pp[2],pp[3], ra, - ras, + // ras, aff, dt, kd, @@ -845,8 +1018,8 @@ struct XPBDSolveSmoothAll : INode { dp[0],dp[1],dp[2],dp[3])) return; for(int i = 0;i != 4;++i) { - if(isnan(dp[i].norm())) - printf("nan dp[%d] detected at stretch\n",i); + // if(isnan(dp[i].norm())) + // printf("nan dp[%d] detected at bending\n",i); // atomic_add(exec_tag,&weight_sum[quad[i]],w); atomic_add(exec_tag,&verts(wOffset,quad[i]),w); for(int d = 0;d != 3;++d) @@ -855,6 +1028,81 @@ struct XPBDSolveSmoothAll : INode { } }); } + + if(category == category_c::vertex_pin_to_vertex_constraint) { + auto target = constraint_ptr->readMeta(CONSTRAINT_TARGET); + const auto& kverts = target->getParticles(); + + auto substep_id = get_input2("substep_id"); + auto nm_substeps = get_input2("nm_substeps"); + auto anim_w = (float)(substep_id + 1) / (float)nm_substeps; + auto anim_pw = (float)(substep_id) / (float)nm_substeps; + + cudaPol(zs::range(cquads.size()),[ + cquads = proxy({},cquads), + stiffnessOffset = cquads.getPropertyOffset("relative_stiffness"), + affiliationOffset = cquads.getPropertyOffset("xpbd_affiliation"), + dampingOffset = cquads.getPropertyOffset("damping_coeff"), + cquadsIndsOffset = cquads.getPropertyOffset("inds"), + cquadsROffset = cquads.getPropertyOffset("r"), + dt = dt, + anim_w = anim_w, + anim_pw = anim_pw, + verts = view(verts), + ptagOffset = verts.getPropertyOffset(ptag), + pptagOffset = verts.getPropertyOffset(pptag), + dptagOffset = verts.getPropertyOffset(dptag), + minvOffset = verts.getPropertyOffset("minv"), + wOffset = verts.getPropertyOffset("w"), + kverts = proxy({},kverts), + kptagOffset = kverts.getPropertyOffset("x"), + kpptagOffset = kverts.getPropertyOffset("px")] ZS_LAMBDA(int ei) mutable { + auto w = cquads(stiffnessOffset,ei); + auto inds = cquads.pack(dim_c<2>,cquadsIndsOffset,ei,int_c); + auto r = cquads(cquadsROffset,ei); + auto aff = cquads(affiliationOffset,ei); + auto kd = cquads(dampingOffset,ei); + + auto vi = inds[0]; + auto kvi = inds[1]; + if(kvi < 0) + return; + + auto p = verts.pack(dim_c<3>,ptagOffset,vi); + auto pp = verts.pack(dim_c<3>,pptagOffset,vi); + auto kp = anim_w * kverts.pack(dim_c<3>,kptagOffset,kvi) + (1.f - anim_w) * kverts.pack(dim_c<3>,kpptagOffset,kvi); + auto kpp = anim_pw * kverts.pack(dim_c<3>,kptagOffset,kvi) + (1.f - anim_pw) * kverts.pack(dim_c<3>,kpptagOffset,kvi); + + auto minv = verts(minvOffset,vi); + if(minv < 1e-6) + return; + auto kminv = 0.f; + + auto lambda = (T)0.0; + bool do_stretch_resistence_only = true; + vec3 dp[2] = {}; + + if(!CONSTRAINT::solve_DistanceConstraint( + p,minv, + kp,kminv, + pp,kpp, + r, + aff, + kd, + dt, + lambda, + dp[0],dp[1], + do_stretch_resistence_only)) + return; + + // atomic_add(exec_tag,&verts(wOffset,vi),w); + verts(wOffset,vi) += w; + verts.tuple(dim_c<3>,dptagOffset,vi) = verts.pack(dim_c<3>,dptagOffset,vi) + dp[0]; + // for(int d = 0;d != 3;++d) + // atomic_add(exec_tag,&verts(dptagOffset + d,vi),dp[0][d]); + }); + } + if(category == category_c::self_dcd_collision_constraint) { const auto& tris = zsparticles->getQuadraturePoints(); @@ -935,6 +1183,12 @@ struct XPBDSolveSmoothAll : INode { } if(category == category_c::vertex_pin_to_cell_constraint) { + // std::cout << "solve vertex cell pin constraint" << std::endl; + auto use_hard_constraint = constraint_ptr->readMeta(PBD_USE_HARD_CONSTRAINT); + if(use_hard_constraint && iter_id > 0) + continue; + + auto target = constraint_ptr->readMeta(CONSTRAINT_TARGET); const auto& kverts = target->getParticles(); const auto& ktris = target->getQuadraturePoints(); @@ -964,78 +1218,115 @@ struct XPBDSolveSmoothAll : INode { thickness); cudaPol(zs::range(cquads.size()),[ + use_hard_constraint = use_hard_constraint, cquads = proxy({},cquads), cell_buffer = proxy({},cell_buffer), dptagOffset = verts.getPropertyOffset(dptag), - ptagOffet = verts.getPropertyOffset(ptag), + ptagOffset = verts.getPropertyOffset(ptag), + cquadsIndsOffset = cquads.getPropertyOffset("inds"), + cquadsBaryOffset = cquads.getPropertyOffset("bary"), + cellBufferXOffset = cell_buffer.getPropertyOffset("x"), + cellBufferVOffset = cell_buffer.getPropertyOffset("v"), ktris = ktris.begin("inds",dim_c<3>,int_c), enable_sliding = enable_sliding, - // weight_sum = proxy(weight_sum), wOffset = verts.getPropertyOffset("w"), stiffnessOffset = cquads.getPropertyOffset("relative_stiffness"), verts = proxy({},verts)] ZS_LAMBDA(int ci) mutable { auto w = cquads(stiffnessOffset,ci); - auto pair = cquads.pack(dim_c<2>,"inds",ci,int_c); + auto pair = cquads.pack(dim_c<2>,cquadsIndsOffset,ci,int_c); auto vi = pair[0]; auto kti = pair[1]; auto ktri = ktris[kti]; - auto bary = cquads.pack(dim_c<6>,"bary",ci); + auto bary = cquads.pack(dim_c<6>,cquadsBaryOffset,ci); vec3 as[3] = {}; vec3 bs[3] = {}; for(int i = 0;i != 3;++i) { - as[i] = cell_buffer.pack(dim_c<3>,"x",ktri[i]); - bs[i] = cell_buffer.pack(dim_c<3>,"v",ktri[i]) + as[i]; + as[i] = cell_buffer.pack(dim_c<3>,cellBufferXOffset,ktri[i]); + bs[i] = cell_buffer.pack(dim_c<3>,cellBufferVOffset,ktri[i]) + as[i]; } - - auto tp = vec3::zeros(); for(int i = 0;i != 3;++i) { tp += as[i] * bary[i]; tp += bs[i] * bary[i + 3]; } - auto dp = tp - verts.pack(dim_c<3>,ptagOffet,vi); - - if(enable_sliding) { - auto avg_nrm = vec3::zeros(); - for(int i = 0;i != 3;++i) { - avg_nrm += cell_buffer.pack(dim_c<3>,"nrm",ktri[i]); + if(use_hard_constraint) { + verts.tuple(dim_c<3>,ptagOffset,vi) = tp; + } else { + auto dp = (tp - verts.pack(dim_c<3>,ptagOffset,vi)) * w; + atomic_add(exec_tag,&verts(wOffset,vi),w); + for(int d = 0;d != 3;++d){ + atomic_add(exec_tag,&verts(dptagOffset + d,vi),dp[d]); } - avg_nrm = avg_nrm.normalized(); - - auto dp_normal = dp.dot(avg_nrm) * avg_nrm; - auto dp_tangent = dp - dp_normal; - if(dp_tangent.norm() < static_cast(0.1)) - dp_tangent = vec3::zeros(); - else - dp_tangent *= static_cast(0.5); - // dp -= dp_tangent * 0.5; - dp = dp_tangent + dp_normal; } + }); + } + + if(category == category_c::follow_animation_constraint) { + auto use_hard_constraint = constraint_ptr->readMeta(PBD_USE_HARD_CONSTRAINT); + if(use_hard_constraint && iter_id > 0) + continue; + + auto animation = constraint_ptr->readMeta(CONSTRAINT_TARGET); + const auto& averts = animation->getParticles(); + auto substep_id = get_input2("substep_id"); + auto nm_substeps = get_input2("nm_substeps"); + auto anim_w = (float)(substep_id + 1) / (float)nm_substeps; - // atomic_add(exec_tag,&weight_sum[vi],w); - atomic_add(exec_tag,&verts(wOffset,vi),w); - for(int d = 0;d != 3;++d){ - atomic_add(exec_tag,&verts(dptagOffset + d,vi),dp[d] * w); + cudaPol(zs::range(cquads.size()),[ + cquads = proxy({},cquads), + verts = proxy({},verts), + alpha = anim_w, + wOffset = verts.getPropertyOffset("w"), + averts = proxy({},averts), + aPtagOffset = averts.getPropertyOffset("x"), + dptagOffset = verts.getPropertyOffset(dptag), + apPtagOffset = averts.getPropertyOffset("px"), + use_hard_constraint = use_hard_constraint, + ptagOffset = verts.getPropertyOffset(ptag), + followWeightOffset = cquads.getPropertyOffset("follow_weight"), + indsOffset = cquads.getPropertyOffset("inds")] ZS_LAMBDA(int ei) mutable { + auto vi = zs::reinterpret_bits(cquads(indsOffset,ei)); + auto w = cquads(followWeightOffset,ei); + auto p = verts.pack(dim_c<3>,ptagOffset,vi); + auto tp = averts.pack(dim_c<3>,apPtagOffset,vi) * (1.f - alpha) + averts.pack(dim_c<3>,aPtagOffset,vi) * alpha; + if(use_hard_constraint) + verts.tuple(dim_c<3>,ptagOffset,vi) = tp; + else { + auto bp = tp * w + p * (1.f - w); + auto dp = bp - p; + atomic_add(exec_tag,&verts(wOffset,vi),w); + for(int d = 0;d != 3;++d){ + atomic_add(exec_tag,&verts(dptagOffset + d,vi),dp[d] * w); + } } }); } if(category == category_c::volume_pin_constraint) { + // std::cout << "solve volume pin constraint " << std::endl; + auto use_hard_constraint = constraint_ptr->readMeta(PBD_USE_HARD_CONSTRAINT); + if(use_hard_constraint && iter_id > 0) + continue; + + auto embed_volume = constraint_ptr->readMeta(CONSTRAINT_TARGET); const auto& vverts = embed_volume->getParticles(); - const auto vtets = embed_volume->getQuadraturePoints(); + const auto& vtets = embed_volume->getQuadraturePoints(); - auto use_hard_constraint = constraint_ptr->readMeta(PBD_USE_HARD_CONSTRAINT); auto substep_id = get_input2("substep_id"); auto nm_substeps = get_input2("nm_substeps"); auto volume_anim_w = (float)(substep_id + 1) / (float)nm_substeps; // auto pw = (float)(substep_id) / (float)nm_substeps; + if(!vverts.hasProperty("px")) { + throw std::runtime_error("the vverts has no px channel"); + } + cudaPol(zs::range(cquads.size()),[ cquads = proxy({},cquads), verts = proxy({},verts), @@ -1254,17 +1545,32 @@ struct XPBDSolveSmoothAll : INode { } } + auto update_vertex_position = get_input2("update_vertex_position"); + + auto output_debug_inform = get_input2("output_debug_inform"); + cudaPol(zs::range(verts.size()),[ + update_vertex_position = update_vertex_position, verts = proxy({},verts), eps = eps, + // output_debug_inform = output_debug_inform, dptagOffset = verts.getPropertyOffset(dptag), + ptagOffset = verts.getPropertyOffset(ptag), wOffset = verts.getPropertyOffset("w")] ZS_LAMBDA(int vi) mutable { if(verts(wOffset,vi) > eps) - verts.tuple(dim_c<3>,dptagOffset,vi) = verts.pack(dim_c<3>,dptagOffset,vi) / verts(wOffset,vi); + verts.tuple(dim_c<3>,dptagOffset,vi) = 2.f * verts.pack(dim_c<3>,dptagOffset,vi) / verts(wOffset,vi); else verts.tuple(dim_c<3>,dptagOffset,vi) = vec3::zeros(); + if(update_vertex_position) + verts.tuple(dim_c<3>,ptagOffset,vi) = verts.pack(dim_c<3>,ptagOffset,vi) + verts.pack(dim_c<3>,dptagOffset,vi); }); + if(output_debug_inform) { + auto ndp = TILEVEC_OPS::dot<3>(cudaPol,verts,dptag,dptag); + std::cout << "ndp : " << ndp << std::endl; + } + + set_output("zsparticles",get_input("zsparticles")); set_output("constraints",get_input("constraints")); }; @@ -1279,11 +1585,528 @@ ZENDEFNODE(XPBDSolveSmoothAll, {{{"zsparticles"}, {"float","dt","1.0"}, {"int","nm_substeps","1"}, {"int","substep_id","0"}, + {"int","iter_id","0"}, + {"bool","update_vertex_position","1"}, + {"bool","output_debug_inform","0"} }, {{"zsparticles"},{"constraints"}}, {}, {"PBD"}}); + +// recalc target nodal normal and bvh structure before using this node +struct ProjectOntoSurface : INode { + + using bvh_t = ZenoLinearBvh::lbvh_t; + using bv_t = bvh_t::Box; + using dtiles_t = zs::TileVector; + + virtual void apply() override { + using namespace zs; + using namespace PBD_CONSTRAINT; + + using vec2 = zs::vec; + using vec3 = zs::vec; + using vec4 = zs::vec; + using mat3 = zs::vec; + using vec2i = zs::vec; + using vec3i = zs::vec; + using vec4i = zs::vec; + using mat4 = zs::vec; + using Box = AABBBox<3,float>; + + constexpr auto space = execspace_e::cuda; + auto cudaPol = cuda_exec(); + constexpr auto exec_tag = wrapv{}; + constexpr auto eps = 1e-6; + + auto target = get_input2("target"); + + const auto& tverts = target->getParticles(); + const auto& ttris = target->getQuadraturePoints(); + auto tptag = get_input2("tptag"); + + auto update_target_mesh = get_input2("update_target_mesh"); + + if(!target->hasBvh(TRIANGLE_MESH_BVH)) { + target->bvh(TRIANGLE_MESH_BVH) = LBvh<3,int,T>{}; + } + auto& ttri_bvh = target->bvh(TRIANGLE_MESH_BVH); + + if(!target->hasMeta(MESH_REORDER_KEYS)) { + update_target_mesh = true; + target->setMeta(MESH_REORDER_KEYS, + zs::Vector{tverts.get_allocator(),tverts.size()}); + } + auto& keys = target->readMeta&>(MESH_REORDER_KEYS); + + if(!target->hasMeta(MESH_REORDER_INDICES)) { + update_target_mesh = true; + target->setMeta(MESH_REORDER_INDICES, + zs::Vector{tverts.get_allocator(),tverts.size()}); + } + auto& indices = target->readMeta&>(MESH_REORDER_INDICES); + + if(!target->hasMeta(MESH_REORDER_VERTICES_BUFFER)) { + update_target_mesh = true; + target->setMeta(MESH_REORDER_VERTICES_BUFFER, + zs::Vector{tverts.get_allocator(),tverts.size()}); + } + auto& reorderedVBuffer = target->readMeta&>(MESH_REORDER_VERTICES_BUFFER); + + if(!target->hasMeta(MESH_MAIN_AXIS)) { + update_target_mesh = true; + target->setMeta(MESH_MAIN_AXIS,0); + } + auto& axis = target->readMeta(MESH_MAIN_AXIS); + + if(!target->hasMeta(MESH_GLOBAL_BOUNDING_BOX)) { + update_target_mesh = true; + target->setMeta(MESH_GLOBAL_BOUNDING_BOX,Box{}); + } + auto& gbv = target->readMeta(MESH_GLOBAL_BOUNDING_BOX); + + // std::cout << "before update_target_mesh" << std::endl; + + if(update_target_mesh) { + auto tbvs = retrieve_bounding_volumes(cudaPol,tverts,ttris,wrapv<3>{},(T)0,tptag); + ttri_bvh.build(cudaPol,tbvs); + + zs::Vector gmins{tverts.get_allocator(),tverts.size()},gmaxs{tverts.get_allocator(),tverts.size()}; + // Box gbv; + zs::Vector ret{tverts.get_allocator(),1}; + + for(int d = 0;d != 3;++d) { + cudaPol(enumerate(gmins,gmaxs),[ + tverts = proxy({},tverts), + tptagOffset = tverts.getPropertyOffset(tptag), + d = d] ZS_LAMBDA(int i,float& gmin,float& gmax) mutable { + auto p = tverts.pack(dim_c<3>,tptagOffset,i); + gmin = p[d]; + gmax = p[d]; + }); + + reduce(cudaPol,std::begin(gmins),std::end(gmins),std::begin(ret),limits::max(),getmin{}); + gbv._min[d] = ret.getVal(); + reduce(cudaPol,std::begin(gmaxs),std::end(gmaxs),std::begin(ret),limits::min(),getmax{}); + gbv._max[d] = ret.getVal(); + } + axis = 0; + auto dis = gbv._max[0] - gbv._min[0]; + for(int d = 1;d != 3;++d) { + if(auto tmp = gbv._max[d] - gbv._min[d];tmp > dis) { + dis = tmp; + axis = d; + } + } + + zs::Vector keys{tverts.get_allocator(),tverts.size()}; + zs::Vector indices{tverts.get_allocator(),tverts.size()}; + cudaPol(enumerate(keys,indices),[tverts = proxy({},tverts), + tptagOffset = tverts.getPropertyOffset(tptag), + axis] ZS_LAMBDA(int id,float& key,int &idx) mutable { + auto p = tverts.pack(dim_c<3>,tptagOffset,id); + key = p[axis]; + idx = id; + }); + + merge_sort_pair(cudaPol,std::begin(keys),std::begin(indices),tverts.size(),std::less{}); + cudaPol(zip(indices,reorderedVBuffer),[ + tverts = proxy({},tverts), + tptagOffset = tverts.getPropertyOffset(tptag)] ZS_LAMBDA(int oid,vec3& p) mutable { + p = tverts.pack(dim_c<3>,tptagOffset,oid); + }); + } + + auto zsparticles = get_input("zsparticles"); + auto& verts = zsparticles->getParticles(); + auto ptag = get_input2("ptag"); + + auto npcheck = TILEVEC_OPS::dot<3>(cudaPol,verts,ptag,ptag); + if(isnan(npcheck)){ + std::cout << "nan np detected" << std::endl; + // throw std::runtime_error("nan np detected"); + } + + zs::Vector locs{verts.get_allocator(),verts.size()}; + cudaPol(zip(zs::range(verts.size()),locs),[axis = axis, + keys = proxy(keys), + minvOffset = verts.getPropertyOffset("minv"), + verts = proxy({},verts), + ptagOffset = verts.getPropertyOffset(ptag)] ZS_LAMBDA(int vi,int& loc) mutable { + auto locate = [&keys](float v) -> int { + int left = 0, right = keys.size(); + while (left < right) { + auto mid = left + (right - left) / 2; + if (keys[mid] > v) + right = mid; + else + left = mid + 1; + } + if (left < keys.size()) { + if (keys[left] > v) + left--; + } else + left = keys.size() - 1; + // left could be -1 + return left; + }; + if(verts(minvOffset,vi) < 0.00001) + return; + + auto xi = verts.pack(dim_c<3>,ptagOffset,vi); + loc = locate(xi[axis]); + }); + + zs::Vector search_radii{verts.get_allocator(),verts.size()}; + auto default_search_radius = get_input2("default_search_radius"); + + // std::cout << "before projection" << std::endl; + + cudaPol(zs::range(verts.size()),[ + verts = proxy({},verts), + minvOffset = verts.getPropertyOffset("minv"), + ptagOffset = verts.getPropertyOffset(ptag), + reorderedVBuffer = proxy(reorderedVBuffer), + locs = proxy(locs), + keys = proxy(keys), + search_radii = proxy(search_radii), + axis = axis, + default_search_radius = default_search_radius, + dd2 = default_search_radius * default_search_radius, + indices = proxy(indices)] ZS_LAMBDA(int vi) mutable { + if(verts(minvOffset,vi) < 0.00001) + return; + auto loc = locs[vi]; + auto p = verts.pack(dim_c<3>,ptagOffset,vi); + int l = loc + 1; + // auto d2 = limits::max(); + auto d2 = dd2; + int j = -1; + int cnt = 0; + while(l < verts.size() && cnt++ < 128) { + if(auto tmp = zs::sqr(reorderedVBuffer[l][axis] - p[axis]);tmp > d2 || tmp > dd2) + break; + if(auto tmp = (reorderedVBuffer[l] - p).l2NormSqr();tmp < d2) { + d2 = tmp; + j = l; + } + ++l; + } + cnt = 0; + l = loc; + while(l >= 0 && cnt++ < 128) { + if(auto tmp = zs::sqr(reorderedVBuffer[l][axis] - p[axis]);tmp > d2 || tmp > dd2) + break; + if(auto tmp = (reorderedVBuffer[l] - p).l2NormSqr();tmp < d2) { + d2 = tmp; + j = l; + } + l--; + } + + if(j != -1) { + search_radii[vi] = zs::sqrt(d2 + 0.000001f) * 1.0001f; + } else { + search_radii[vi] = default_search_radius * 1.0001f; + } + }); + + auto do_moton_ordering = get_input2("do_moton_ordering"); + + + if(!zsparticles->hasMeta(MESH_REORDER_INDICES)) { + do_moton_ordering = true; + zsparticles->setMeta(MESH_REORDER_INDICES,zs::Vector{verts.get_allocator(),verts.size()}); + } + auto& is = zsparticles->readMeta&>(MESH_REORDER_INDICES); + + // std::cout << "do motor ordering" << std::endl; + if(do_moton_ordering) { + if(!zsparticles->hasMeta(MESH_REORDER_KEYS)) { + do_moton_ordering = true; + zsparticles->setMeta(MESH_REORDER_KEYS,zs::Vector{verts.get_allocator(),verts.size()}); + } + auto& ks = zsparticles->readMeta&>(MESH_REORDER_KEYS); + + cudaPol(enumerate(ks,is),[ + gbv = gbv, + ptagOffset = verts.getPropertyOffset(ptag), + verts = proxy({},verts)] ZS_LAMBDA(int i,u32& key,int& idx) mutable { + auto p = verts.pack(dim_c<3>,ptagOffset,i); + for (int d = 0; d != 3; ++d) { + if (p[d] < gbv._min[d]) + p[d] = gbv._min[d]; + else if (p[d] > gbv._max[d]) + p[d] = gbv._max[d]; + } + auto coord = gbv.getUniformCoord(p).template cast(); + key = morton_code<3>(coord); + idx = i; + }); + + merge_sort_pair(cudaPol, std::begin(ks), std::begin(is), verts.size(), std::less{}); + } else { + cudaPol(enumerate(is),[] ZS_LAMBDA(int i,int& idx) mutable {idx = i;}); + } + + auto dptag = get_input2("dptag"); + auto update_vertex_position = get_input2("update_vertex_position"); + + if(!update_vertex_position && !verts.hasProperty(dptag)) { + verts.append_channels(cudaPol,{{dptag,3}}); + TILEVEC_OPS::fill(cudaPol,verts,dptag,0.f); + } + + // std::cout << "before doing projection" << std::endl; + + cudaPol(is,[verts = proxy({},verts), + ptagOffset = verts.getPropertyOffset(ptag), + dptagOffset = verts.getPropertyOffset(dptag), + minvOffset = verts.getPropertyOffset("minv"), + tverts = proxy({},tverts), + tptagOffset = tverts.getPropertyOffset(tptag), + ttris = proxy({},ttris), + update_vertex_position = update_vertex_position, + default_search_radius = default_search_radius, + is = proxy(is), + tindsOffset = ttris.getPropertyOffset("inds"), + search_radii = proxy(search_radii), + ttri_bvh = proxy(ttri_bvh)] ZS_LAMBDA(int qid) mutable { + if(verts(minvOffset,qid) < 0.00001) + return; + auto rad = search_radii[qid]; + auto p = verts.pack(dim_c<3>,ptagOffset,qid); + auto bv = Box{get_bounding_box(p - rad,p + rad)}; + + auto closest_dist = limits::max(); + int closest_ti = -1; + // auto closest_bary = vec3{1.f,0.f,0.f}; + auto closest_cp = vec3::zeros(); + + auto find_closest_triangles = [&](int ti) { + auto ttri = ttris.pack(dim_c<3>,tindsOffset,ti,int_c); + vec3 tps[3] = {}; + for(int i = 0;i != 3;++i) + tps[i] = tverts.pack(dim_c<3>,tptagOffset,ttri[i]); + vec3 bary{}; + vec3 project_cp{}; + auto dist = LSL_GEO::get_vertex_triangle_distance(tps[0],tps[1],tps[2],p,bary,project_cp); + if(project_cp.norm() < 1e-6) { + { + auto v0 = tps[0]; + auto v1 = tps[1]; + auto v2 = tps[2]; + auto v = p; + vec3 barycentric{}; + vec3 project_point{}; + + const vec3 e1 = v1 - v0; + const vec3 e2 = v2 - v0; + const vec3 e3 = v2 - v1; + const vec3 n = e1.cross(e2); + const vec3 na = (v2 - v1).cross(v - v1); + const vec3 nb = (v0 - v2).cross(v - v2); + const vec3 nc = (v1 - v0).cross(v - v0); + // barycentric = vec3(n.dot(na) / n.l2NormSqr(), + // n.dot(nb) / n.l2NormSqr(), + // n.dot(nc) / n.l2NormSqr()); + auto n2 = n.l2NormSqr(); + barycentric = vec3(n.dot(na),n.dot(nb),n.dot(nc)); + const float barySum = zs::abs(barycentric[0]) + zs::abs(barycentric[1]) + zs::abs(barycentric[2]); + + // if the point projects to inside the triangle, it should sum to 1 + if (zs::abs(barySum - n2) < static_cast(1e-6) && n2 > static_cast(1e-6)) + { + const vec3 nHat = n / n.norm(); + const float normalDistance = (nHat.dot(v - v0)); + barycentric /= n2; + // project_bary = barycentric; + // project_point = vec3::zeros(); + + project_point = barycentric[0] * v0 + barycentric[1] * v1 + barycentric[2] * v2; + + printf("wierd——000 project center[%d]->[%d] : %f %f %f : %f %f %f : A : %f D : %f\n",qid,ti, + (float)project_cp[0], + (float)project_cp[1], + (float)project_cp[2], + (float)tps[0].norm(), + (float)tps[1].norm(), + (float)tps[2].norm(), + (float)LSL_GEO::area(tps[0],tps[1],tps[2]), + (float)dist); + + return; + + // return zs::abs(normalDistance); + } + + vec3 vs[3] = {v0,v1,v2}; + + vec3 es[3] = {}; + + // project onto each edge, find the distance to each edge + + const vec3 ev = v - v0; + const vec3 ev3 = v - v1; + + // const vec3 e2Hat = e2 / e2.norm(); + // const vec3 e3Hat = e3 / e3.norm(); + vec3 edgeDistances{1e8, 1e8, 1e8}; + + // see if it projects onto the interval of the edge + // if it doesn't, then the vertex distance will be smaller, + // so we can skip computing anything + // vec3 e1Hat = e1; + float e1dot = e1.dot(ev); + // vec3 projected_e[3] = {}; + // auto e1n = e1.norm(); + auto e1n2 = e1.l2NormSqr(); + if (e1dot > 0.0 && e1dot < e1n2 && e1n2 > static_cast(1e-6)) + { + // e1Hat /= e1.norm(); + const vec3 projected = v0 + e1 * e1dot / e1n2; + es[0] = projected; + edgeDistances[0] = (v - projected).norm(); + } + + const float e2dot = e2.dot(ev); + auto e2n2 = e2.l2NormSqr(); + if (e2dot > 0.0 && e2dot < e2n2 && e2n2 > static_cast(1e-6)) + { + const vec3 projected = v0 + e2 * e2dot / e2n2; + es[1] = projected; + edgeDistances[1] = (v - projected).norm(); + } + const float e3dot = e3.dot(ev3); + auto e3n2 = e3.l2NormSqr(); + if (e3dot > 0.0 && e3dot < e3n2 && e3n2 > static_cast(1e-6)) + { + const vec3 projected = v1 + e3 * e3dot / e3n2; + es[2] = projected; + edgeDistances[2] = (v - projected).norm(); + } + + // get the distance to each vertex + const vec3 vertexDistances{(v - v0).norm(), + (v - v1).norm(), + (v - v2).norm()}; + + // get the smallest of both the edge and vertex distances + float vertexMin = 1e8; + float edgeMin = 1e8; + + int min_e_idx = 0; + int min_v_idx = 0; + // vec3 project_v_min{}; + // vec3 project_e_min{}; + + for(int i = 0;i < 3;++i){ + if(vertexMin > vertexDistances[i]){ + vertexMin = vertexDistances[i]; + min_v_idx = i; + } + if(edgeMin > edgeDistances[i]){ + edgeMin = edgeDistances[i]; + min_e_idx = i; + } + // vertexMin = vertexMin > vertexDistances[i] ? vertexDistances[i] : vertexMin; + // edgeMin = edgeMin > edgeDistances[i] ? edgeDistances[i] : edgeMin; + } + // vec3 project_v{}; + if(vertexMin < edgeMin) + project_point = vs[min_v_idx]; + else + project_point = es[min_e_idx]; + + printf("wierd-111 project center[%d]->[%d] : PCP : %f %f %f : V %f %f %f %f : VD : %f %f %f : A : %f Vmin : %f\n",qid,ti, + (float)project_cp[0], + (float)project_cp[1], + (float)project_cp[2], + (float)v.norm(), + (float)v0.norm(), + (float)v1.norm(), + (float)v2.norm(), + (float)vertexDistances[0], + (float)vertexDistances[1], + (float)vertexDistances[2], + (float)LSL_GEO::area(tps[0],tps[1],tps[2]), + (float)vertexMin); + + return; + + } + + + // printf("wierd project center[%d]->[%d] : %f %f %f : %f %f %f : A : %f D : %f\n",qid,ti, + // (float)project_cp[0], + // (float)project_cp[1], + // (float)project_cp[2], + // (float)tps[0].norm(), + // (float)tps[1].norm(), + // (float)tps[2].norm(), + // (float)LSL_GEO::area(tps[0],tps[1],tps[2]), + // (float)dist); + } + // if(isnan(dist) || isnan(project_bary.norm())) + // return; + if(dist < closest_dist) { + closest_dist = dist; + closest_ti = ti; + closest_cp = project_cp; + } + }; + + ttri_bvh.iter_neighbors(bv,find_closest_triangles); + if(closest_ti < 0) { + return; + } else { + auto cp = closest_cp; + auto dp = cp - verts.pack(dim_c<3>,ptagOffset,qid); + + // if(isnan(dp.norm()) || dp.norm() > default_search_radius) { + // printf("too big projection update detected %d(%f) -> %d [%f %f %f]\n",qid,(float)rad,closest_ti, + // (float)closest_cp[0], + // (float)closest_cp[1], + // (float)closest_cp[2]); + // return; + // } + + if(update_vertex_position) + verts.tuple(dim_c<3>,ptagOffset,qid) = cp; + else + verts.tuple(dim_c<3>,dptagOffset,qid) = dp; + } + }); + + auto np = TILEVEC_OPS::dot<3>(cudaPol,verts,ptag,ptag); + if(isnan(np)) { + std::cout << "nan update detected after surface project" << std::endl; + throw std::runtime_error("nan update detected after surface project"); + } + + + set_output("zsparticles",get_input("zsparticles")); + set_output("target",get_input("target")); + }; +}; + + +ZENDEFNODE(ProjectOntoSurface, {{{"zsparticles"}, + {"target"}, + {"string","ptag","x"}, + {"string","dptag","dx"}, + {"string","tptag","x"}, + {"bool","update_target_mesh","1"}, + {"bool","do_moton_ordering","1"}, + {"bool","update_vertex_position","1"}, + {"float","default_search_radius","0.0"} + }, + {{"zsparticles"},{"target"}}, + {}, + {"PBD"}}); + + struct VisualizeDCDProximity : zeno::INode { virtual void apply() override { diff --git a/projects/CuLagrange/pbd/ConstraintsUpdator.cu b/projects/CuLagrange/pbd/ConstraintsUpdator.cu new file mode 100644 index 0000000000..2cf732504f --- /dev/null +++ b/projects/CuLagrange/pbd/ConstraintsUpdator.cu @@ -0,0 +1,110 @@ +#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 "../../Utils.hpp" + +#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 "../geometry/kernel/bary_centric_weights.hpp" +#include "constraint_function_kernel/constraint_types.hpp" +#include "../fem/collision_energy/evaluate_collision.hpp" + + +namespace zeno { + +struct UpdateConstraintTarget : INode { + +virtual void apply() override { + using namespace zs; + using namespace PBD_CONSTRAINT; + + using vec2 = zs::vec; + using vec3 = zs::vec; + using vec4 = zs::vec; + using vec9 = zs::vec; + using mat3 = 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 = get_input("constraint"); + + auto update_weight = get_input2("update_weight"); + auto new_uniform_weight = get_input2("new_uniform_weight"); + + if(update_weight) { + auto& cquads = constraint->getQuadraturePoints(); + TILEVEC_OPS::fill(cudaPol,cquads,"w",new_uniform_weight); + } + + // auto target = get_input("target"); + + auto type = constraint->readMeta(CONSTRAINT_KEY,wrapt{}); + // auto do_frame_interpolation = get_input2("do_frame_interpolation"); + + // if(type == category_c::vertex_pin_to_cell_constraint || type == category_c) { + std::cout << "update constraint " << type << std::endl; + auto target = get_input("target"); + // switch(type) { + // // case category_c::follow_animation_constraint : break; + // // case category_c::dcd_collision_constraint : break; + // case category_c::vertex_pin_to_cell_constraint || category_c::volume_pin_constraint : + auto ctarget = constraint->readMeta(CONSTRAINT_TARGET,zs::wrapt{}); + if(target->getParticles().size() != ctarget->getParticles().size()) { + std::cout << "the input update target and contraint target has different number of particles" << std::endl; + throw std::runtime_error("the input update target and constraint target has different number of particles"); + } + if(target->getQuadraturePoints().size() != ctarget->getQuadraturePoints().size()) { + std::cout << "the input update target and constraint target has different number of quadratures" << std::endl; + throw std::runtime_error("the input update target and constraint target has different number of quadratures"); + } + + const auto& kverts = target->getParticles(); + auto& ckverts = ctarget->getParticles(); + if(!ckverts.hasProperty("px")) { + ckverts.append_channels(cudaPol,{{"px",3}}); + } + TILEVEC_OPS::copy(cudaPol,ckverts,"x",ckverts,"px"); + TILEVEC_OPS::copy(cudaPol,kverts,"x",ckverts,"x"); + // break; + // } + + // } + set_output("constraint",constraint); + set_output("source",source); +} + +}; + +ZENDEFNODE(UpdateConstraintTarget, {{ + {"source"}, + {"target"}, + {"constraint"}, + {"bool","update_weight","0"}, + {"float","new_uniform_weight","1.0"} +}, +{{"source"},{"constraint"}}, +{ + // {"string","groupID",""}, +}, +{"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 2837974cab..3572e62c43 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint.cuh @@ -5,6 +5,8 @@ #include "zensim/math/DihedralAngle.hpp" namespace zeno { namespace CONSTRAINT { + using namespace zs; + // FOR CLOTH SIMULATION template constexpr bool solve_DistanceConstraint( @@ -76,14 +78,14 @@ namespace zeno { namespace CONSTRAINT { constexpr bool solve_DistanceConstraint( const VECTOR3d &p0, const SCALER& invMass0, const VECTOR3d &p1, const SCALER& invMass1, - const VECTOR3d &pp0, - const VECTOR3d &pp1, + const VECTOR3d &pp0,const VECTOR3d &pp1, const SCALER& restLength, const SCALER& xpbd_affiliation, const SCALER& kdamp_ratio, const SCALER& dt, SCALER& lambda, - VECTOR3d &corr0, VECTOR3d &corr1) + VECTOR3d &corr0, VECTOR3d &corr1, + bool stretch_resistence_only = false) { SCALER wsum = invMass0 + invMass1; if(wsum < static_cast(1e-6)) { @@ -92,42 +94,47 @@ namespace zeno { namespace CONSTRAINT { VECTOR3d n = p0 - p1; SCALER d = n.norm(); - SCALER C = d - restLength; - - if (d > static_cast(1e-6)) + if(d > static_cast(1e-6)) { n /= d; - else - { + } + else { corr0 = VECTOR3d::uniform(0); corr1 = VECTOR3d::uniform(0); return false; - } + } + SCALER C = d - restLength; + SCALER alpha = 0.0; - if (xpbd_affiliation != 0.0) - { + if (xpbd_affiliation != 0.0){ alpha = static_cast(1.0) / (xpbd_affiliation * dt * dt); } - const auto& gradC = n; - SCALER dsum = 0.0, gamma = 1.0; + if(stretch_resistence_only && C < 0) { + corr0 = VECTOR3d::uniform(0); + corr1 = VECTOR3d::uniform(0); + return false; + } + + auto gradC = n; + + SCALER dsum = 0.0, gamma = 0.0; if(kdamp_ratio > 0) { auto beta = kdamp_ratio * dt * dt; gamma = alpha * beta / dt; - dsum = gamma * n.dot((p0 - pp0) - (p1 - pp1)); + dsum = gamma * gradC.dot((p0 - pp0) - (p1 - pp1)); // gamma += 1.0; } - const SCALER delta_lambda = -(C + alpha * lambda + dsum) / ((gamma + static_cast(1.0)) * wsum + alpha); + const SCALER dL = -(C + alpha * lambda + dsum) / ((gamma + static_cast(1.0))* wsum + alpha); + const VECTOR3d dp = n * dL; - lambda += delta_lambda; - - const VECTOR3d pt = n * delta_lambda; + lambda += dL; - corr0 = invMass0 * pt; - corr1 = -invMass1 * pt; + corr0 = invMass0 * dp; + corr1 = -invMass1 * dp; return true; } @@ -254,20 +261,6 @@ namespace zeno { namespace CONSTRAINT { ds[2] = VECTOR3d{grad[1 * 3 + 0],grad[1 * 3 + 1],grad[1 * 3 + 2]}; ds[3] = VECTOR3d{grad[2 * 3 + 0],grad[2 * 3 + 1],grad[2 * 3 + 2]}; ds[1] = VECTOR3d{grad[3 * 3 + 0],grad[3 * 3 + 1],grad[3 * 3 + 2]}; - // for(int i = 0;i != 4;++i) - // ds[i] = VECTOR3d{grad[i * 3 + 0],grad[i * 3 + 1],grad[i * 3 + 2]}; - - // for(int i = 0;i != 4;++i){ - // printf("ds[%d] : %f %f %f\n",i, - // (float)ds[i][0], - // (float)ds[i][1], - // (float)ds[i][2]); - // } - - - // SCALER alpha = 0.0; - // if (stiffness != 0.0) - // alpha = static_cast(1.0) / (stiffness * dt * dt); SCALER sum_normGradC = invMass0 * ds[0].l2NormSqr() + @@ -384,7 +377,7 @@ namespace zeno { namespace CONSTRAINT { const VECTOR3d& pp2, const VECTOR3d& pp3, const SCALER& restAngle, - const SCALER& restAngleSign, + // const SCALER& restAngleSign, const SCALER& xpbd_affliation, const SCALER& dt, const SCALER& kdamp_ratio, @@ -408,7 +401,7 @@ namespace zeno { namespace CONSTRAINT { alpha = static_cast(1.0) / (xpbd_affliation * dt * dt); SCALER dsum = 0.0; - SCALER gamma = 1.0; + SCALER gamma = 0.0; if(kdamp_ratio > 0) { auto beta = kdamp_ratio * dt * dt; gamma = alpha * beta / dt; @@ -906,96 +899,109 @@ namespace zeno { namespace CONSTRAINT { // FOR ELASTIC RODS SIMULATION // ---------------------------------------------------------------------------------------------- -// 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(); -// } +template, + std::is_convertible_v, + VECTOR3d::dim == 1,VECTOR3d::extent == 3, + QUATERNION::dim == 1,QUATERNION::extent == 4> = 0> +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) +{ + constexpr auto eps = zs::limits::epsilon(); + 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 (zs::abs(stretchingAndShearingKs[0] - stretchingAndShearingKs[1]) < eps && zs::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(); + auto R = quaternion2matrix(q0); + gamma = R.transpose() * gamma; + for (int i = 0; i<3; i++) gamma[i] *= stretchingAndShearingKs[i]; + gamma = R * gamma; + } -// corr0 = invMass0 * gamma; -// corr1 = -invMass1 * gamma; + 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; + QUATERNION q_e_3_bar{-q0.y(), q0.x(), -q0.w(), q0.z()}; //compute q*e_3.conjugate (cheaper than quaternion product) + corrq0 = quaternionMultiply(QUATERNION{gamma.x(), gamma.y(), gamma.z(), 0.0 },q_e_3_bar); + + corrq0 *= static_cast(2.0) * invMassq0 * restLength; -// return true; -// } + 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 +template, + std::is_convertible_v, + VECTOR3d::dim == 1,VECTOR3d::extent == 3, + QUATERNION::dim == 1,QUATERNION::extent == 4> = 0> +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 = quaternionConjugateMultiply(q0,q1); //darboux vector + + QUATERNION omega_plus; + omega_plus = omega + restDarbouxVector; //delta Omega with -Omega_0 + omega = omega - restDarbouxVector; //delta Omega with + omega_0 + if (omega.l2NormSqr() > omega_plus.l2NormSqr()) omega = omega_plus; + + for (int i = 0; i < 3; i++) omega[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; + corrq0 = quaternionMultiply(q1,omega); + // corrq1 = q0 * omega; + corrq1 = quaternionMultiply(q0,omega); + corrq0 *= invMassq0; + corrq1 *= -invMassq1; + return true; +} -// 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; +// ---------------------------------------------------------------------------------------------- +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; + const SCALER lambda = stiffness * p2pm.dot(p1p0) / wSum; -// corr0 = -invMass0 * lambda * p0p2; -// corr1 = -invMass1 * lambda * p2p1; -// corr2 = -invMass2 * lambda * p1p0; + corr0 = -invMass0 * lambda * p0p2; + corr1 = -invMass1 * lambda * p2p1; + corr2 = -invMass2 * lambda * p1p0; -// return true; -// } + return true; +} // // ---------------------------------------------------------------------------------------------- // template diff --git a/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp b/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp index eb277a49c3..b25eac7832 100644 --- a/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp +++ b/projects/CuLagrange/pbd/constraint_function_kernel/constraint_types.hpp @@ -5,8 +5,16 @@ namespace zeno { namespace PBD_CONSTRAINT { constexpr auto CONSTRAINT_KEY = "XPBD_CONSTRAINT"; constexpr auto CONSTRAINT_TARGET = "XPBD_CONSTRAINT_TARGET"; constexpr auto CONSTRAINT_COLOR_OFFSET = "XPBD_CONSTRAINT_OFFSET"; +constexpr auto CONSTRAINT_UPDATE_INDEX_BUFFER = "CONSTRAINT_UPDATE_INDEX_BUFFER"; +constexpr auto CONSTRAINT_UPDATE_OFFSETS = "CONSTRAINT_UPDATE_OFFSETS"; +constexpr auto CONSTRAINT_UPDATE_BUFFER = "CONSTRAINT_Update_BUFFER"; + constexpr auto NM_DCD_COLLISIONS = "NM_DCD_COLLISIONS"; +constexpr auto DCD_COUNTER_BUFFER = "DCD_COUNTER_BUFFER"; +constexpr auto COLLISION_BUFFER = "COLLLISION_BUFFER"; +constexpr auto COLLISION_CSEE_SET = "COLLISION_CSEE_SET"; +constexpr auto COLLSIION_CSPT_SET = "COLLISION_CSPT_SET"; constexpr auto GLOBAL_DCD_THICKNESS = "GLOBAL_DCD_THICKNESS"; constexpr auto ENABLE_SLIDING = "ENABLE_SLIDING"; @@ -18,9 +26,30 @@ constexpr auto TARGET_CELL_BUFFER = "TARGET_CELL_BUFFER"; constexpr auto PBD_USE_HARD_CONSTRAINT = "PBD_USE_HARD_CONSTRAINT"; +constexpr auto SHAPE_MATCHING_REST_CM = "SHAPE_MATCHING_REST_CM"; +constexpr auto SHAPE_MATCHING_WEIGHT_SUM = "SHAPE_MATCHING_WEIGHT_SUM"; + +constexpr auto SHAPE_MATCHING_SHAPE_OFFSET = "SHAPE_MATCHING_SHAPE_OFFSET"; + +constexpr auto SHAPE_MATCHING_MATRIX_BUFFER = "SHAPE_MATCHING_MATRIX_BUFFER"; + +constexpr auto VERTEX_BV = "VERTEX_BV"; +constexpr auto VERTEX_BVH = "VERTEX_BV"; +constexpr auto TRIANGLE_MESH_BV = "TRIANGLE_MESH_BV"; +constexpr auto TRIANGLE_MESH_BVH = "TRIANGLE_MESH_BVH"; + +constexpr auto MESH_REORDER_KEYS = "MESH_REORDER_KEYS"; +constexpr auto MESH_REORDER_INDICES = "MESH_REORDER_INDICES"; +constexpr auto MESH_REORDER_VERTICES_BUFFER = "MESH_REORDER_VERTICES_BUFFER"; +constexpr auto MESH_MAIN_AXIS = "MESH_MAIN_AXIS"; +constexpr auto MESH_GLOBAL_BOUNDING_BOX = "MESH_GLOBAL_BOUNDING_BOX"; + // constexpr auto DCD_COLLISIONS_MESH_COLLIDER = "DCD_COLLISION_MESH_COLLIDER"; enum category_c : int { + shape_matching_constraint, + long_range_attachment, + vertex_pin_to_vertex_constraint, edge_length_constraint, isometric_bending_constraint, dihedral_bending_constraint, diff --git a/projects/ZenoFX/nw.cpp b/projects/ZenoFX/nw.cpp index b8e078550e..340f4b22f6 100644 --- a/projects/ZenoFX/nw.cpp +++ b/projects/ZenoFX/nw.cpp @@ -85,7 +85,17 @@ struct NumericWrangle : zeno::INode { auto par = zeno::objectToLiterial(obj); auto dim = std::visit([&] (auto const &v) { using T = std::decay_t; - if constexpr (std::is_convertible_v) { + if constexpr (std::is_convertible_v) { + parvals.push_back(v[0]); + parvals.push_back(v[1]); + parvals.push_back(v[2]); + parvals.push_back(v[3]); + parnames.emplace_back(key, 0); + parnames.emplace_back(key, 1); + parnames.emplace_back(key, 2); + parnames.emplace_back(key, 3); + return 4; + } else if constexpr (std::is_convertible_v) { parvals.push_back(v[0]); parvals.push_back(v[1]); parvals.push_back(v[2]); diff --git a/zeno/src/nodes/StringNodes.cpp b/zeno/src/nodes/StringNodes.cpp index 143f49a373..ac789eaf31 100644 --- a/zeno/src/nodes/StringNodes.cpp +++ b/zeno/src/nodes/StringNodes.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -87,7 +88,7 @@ ZENDEFNODE(StringEqual, { struct PrintString : zeno::INode { virtual void apply() override { auto str = get_input2("str"); - printf("PrintString: %s\n", str.c_str()); + printf("%s\n", str.c_str()); } }; @@ -231,9 +232,18 @@ ZENDEFNODE(StringFormatNumStr, { struct StringRegexMatch : zeno::INode { virtual void apply() override { + using namespace std::regex_constants; + auto str = get_input2("str"); auto regex_str = get_input2("regex"); - std::regex self_regex(regex_str); + + auto case_sensitive = get_input2("case_sensitive"); + + auto default_flags = ECMAScript; + if(!case_sensitive) + default_flags |= icase; + + std::regex self_regex(regex_str,default_flags); int output = std::regex_match(str, self_regex); set_output2("output", output); @@ -244,6 +254,7 @@ ZENDEFNODE(StringRegexMatch, { { {"string", "str", ""}, {"string", "regex", ""}, + {"bool","case_sensitive","1"} }, { {"int", "output"} @@ -252,6 +263,65 @@ ZENDEFNODE(StringRegexMatch, { {"string"}, }); +struct StringRegexSearch : zeno::INode { + virtual void apply() override { + using namespace std::regex_constants; + + auto str = get_input2("str"); + + auto regex_str = get_input2("regex"); + auto case_sensitive = get_input2("case_sensitive"); + + std::smatch res{}; + + auto flags = ECMAScript; + if(!case_sensitive) + flags |= icase; + + std::regex self_regex(regex_str,flags); + + auto matched_substr_list = std::make_shared(); + + // int search_success = std::regex_search(str,res,self_regex); + int search_success = 0; + + + while(std::regex_search(str,res,self_regex)) { + search_success = 1; + auto is_first_matched = true; + for(auto w : res) { + if(is_first_matched) { + is_first_matched = false; + continue; + } + auto zstr = std::make_shared(); + zstr->set(w.str()); + matched_substr_list->arr.push_back(std::move(zstr)); + } + + str = res.suffix().str(); + } + + set_output2("search_success",search_success); + set_output("res",std::move(matched_substr_list)); + } +}; + +ZENDEFNODE(StringRegexSearch, { + { + {"string", "str", ""}, + {"string", "regex", ""}, + {"bool","case_sensitive","1"} + }, + { + {"int", "search_success"}, + {"res"} + }, + {}, + {"string"}, +}); + + struct StringSplitAndMerge: zeno::INode{ std::vector split(const std::string& s, std::string seperator) diff --git a/zeno/src/nodes/prim/PrimitiveAttrPicker.cpp b/zeno/src/nodes/prim/PrimitiveAttrPicker.cpp index 9ce12642cd..73cc0dad77 100644 --- a/zeno/src/nodes/prim/PrimitiveAttrPicker.cpp +++ b/zeno/src/nodes/prim/PrimitiveAttrPicker.cpp @@ -64,8 +64,8 @@ ZENDEFNODE(PrimitiveAttrPicker, { }, // outputs { - {"list"}, - {"PrimitiveObject", "outPrim"} + {"PrimitiveObject", "outPrim"}, + {"list"} }, // params {{"string", "selected", ""}},