Xpbd

projects/CuLagrange/CMakeLists.txt

@@ -35,8 +35,10 @@ target_sources(zeno PRIVATE
 
 # pbd
 target_sources(zeno PRIVATE
-  pbd/PBDInit.cu
-  pbd/PBDPipeline.cu
+  pbd/Constraints.cu
+  pbd/SelfCollision.cu
+  pbd/KinematicCollision.cu
+  # pbd/Pipeline.cu
 )
 
 # fem-cloth

projects/CuLagrange/fem/FleshDynamicStepping.cu

@@ -681,7 +681,7 @@ struct FleshDynamicStepping : INode {
                             dt = dt,offset = offset] ZS_LAMBDA(int ei) mutable {
                     auto m = eles("m",ei)/(T)4.0;
                     auto inds = eles.pack(dim_c<4>,"inds",ei).reinterpret_bits(int_c);
-                    auto pgrad = zs::vec<T,12>::zeros();
+                    // auto pgrad = zs::vec<T,12>::zeros();
                     // auto H  = zs::vec<T,12,12>::zeros();
                     // if(eles.hasProperty("dt")) {
                     //     dt2 = eles("dt",ei) * eles("dt",ei);
@@ -701,8 +701,13 @@ struct FleshDynamicStepping : INode {
                         auto x0 = vtemp.pack(dim_c<3>,"xp",inds[i]);
                         auto v0 = vtemp.pack(dim_c<3>,"vp",inds[i]);
 
-                        auto alpha = inertia * m/dt2 * A;
+                        auto alpha = (inertia * m / dt2) * A;
                         auto nodal_pgrad = -alpha * (x1 - x0 - v0 * dt);
+
+                        if(isnan(nodal_pgrad.norm())) {
+                            printf("nan nodal pgrad detected : %f %f %f %f\n",(float)alpha.norm(),(float)x1.norm(),(float)x0.norm(),(float)v0.norm());
+                        }
+
                         for(int d = 0;d != 3;++d){
                             auto idx = i * 3 + d;
                             gh_buffer("grad",idx,ei + offset) = nodal_pgrad[d];
@@ -719,6 +724,9 @@ struct FleshDynamicStepping : INode {
                 });
             }
 
+            // auto gradn_after_inertia = TILEVEC_OPS::dot<12>(cudaPol,gh_buffer,"grad","grad");
+            // std::cout << "gradn_after_inertia : " << gradn_after_inertia << std::endl;
+
             cudaPol(zs::range(eles.size()), [dt = dt,dt2 = dt2,aniso_strength = aniso_strength,
                             verts = proxy<space>({},verts),
                             vtemp = proxy<space>({}, vtemp),
@@ -767,6 +775,7 @@ struct FleshDynamicStepping : INode {
                 auto dFdXT = dFdX.transpose();
                 auto vf = -vole * (dFdXT * vecP);     
 
+
                 auto mg = volf * vole / (T)4.0;
                 for(int i = 0;i != 4;++i)
                     for(int d = 0;d !=3 ;++d){
@@ -784,25 +793,34 @@ struct FleshDynamicStepping : INode {
                     auto fiber = eles.pack(dim_c<3>,"fiber",ei);
                     if(zs::abs(fiber.norm() - 1.0) < 1e-3) {
                         fiber /= fiber.norm();
-                        // if(eles.hasProperty("mu")) {
-                        //     amodel.mu = eles("mu",ei);
-                        //     // amodel.lam = eles("lam",ei);
+                        if(eles.hasProperty("mu") && eles.hasProperty("lam")) {
+                            amodel.mu = eles("mu",ei);
+                            // amodel.lam = eles("lam",ei);
 
-                        // }
+                        }
+                        // COMMIT FIND A ANISOTROPIC BUG HERE
                         auto aP = amodel.do_first_piola(FAct,fiber);
-                        auto vecAP = flatten(P);
-                        vecAP = dFActdF.transpose() * vecP;
+                        auto vecAP = flatten(aP);
+                        vecAP = dFActdF.transpose() * vecAP;
                         vf -= vole  * dFdXT * vecAP *aniso_strength;
 
-                        // auto aHq = amodel.do_first_piola_derivative(FAct,fiber);
-                        auto aHq = zs::vec<T,9,9>::zeros();
+                        auto aHq = amodel.do_first_piola_derivative(FAct,fiber);
+                        // auto aHq = zs::vec<T,9,9>::zeros();
                         H += dFdAct_dFdX.transpose() * aHq * dFdAct_dFdX * vole * aniso_strength;
                         // if((int)eles("Muscle_ID",ei) == 0){
                         //     printf("fiber : %f %f %f,Fa = %f,aP = %f,aHq = %f,H = %f\n",fiber[0],fiber[1],fiber[2],(float)FAct.norm(),(float)aP.norm(),(float)aHq.norm(),(float)H.norm());
                         // }
+
+                        if(isnan(vf.norm())) {
+                            printf("nan nodal aniso_vf detected : %f %f %f %f\n",(float)vecP.norm(),(float)volf.norm(),(float)vecAP.norm(),(float)aHq.norm());
+                        }
                     }
                 }
 
+                // if(isnan(vf.norm())) {
+                //     printf("nan nodal vf detected : %f %f %f %f\n",(float)vecP.norm(),(float)volf.norm(),(float)P.norm(),(float)FAct.norm());
+                // }
+
                 gh_buffer.tuple(dim_c<12>,"grad",ei + offset) = gh_buffer.pack(dim_c<12>,"grad",ei + offset) + vf/* - rdamping*/; 
                 // gh_buffer.tuple(dim_c<12>,"grad",ei + offset) = gh_buffer.pack(dim_c<12>,"grad",ei + offset) - rdamping; 
                 // H += kd_beta*H/dt;
@@ -813,6 +831,10 @@ struct FleshDynamicStepping : INode {
             // T lambda = model.lam;
             // T mu = model.mu;
 
+
+            // auto gradn_after_elastic = TILEVEC_OPS::dot<12>(cudaPol,gh_buffer,"grad","grad");
+            // std::cout << "gradn_after_elastic : " << gradn_after_elastic << std::endl;
+
             auto nmEmbedVerts = b_verts.size();
 
             cudaPol(zs::range(nmEmbedVerts), [
@@ -882,6 +904,8 @@ struct FleshDynamicStepping : INode {
 
             });
 
+            // auto gradn_after_driver = TILEVEC_OPS::dot<12>(cudaPol,gh_buffer,"grad","grad");
+            // std::cout << "gradn_after_driver : " << gradn_after_driver << std::endl;
 
         }
 
@@ -1997,6 +2021,17 @@ struct FleshDynamicStepping : INode {
             },[](...) {
                 throw std::runtime_error("unsupported anisotropic elasticity model");
             })(models.getElasticModel(),models.getAnisoElasticModel());
+
+            // {
+            //     auto gradn = TILEVEC_OPS::dot<12>(cudaPol,gh_buffer,"grad","grad");
+            //     std::cout << "gradn after elastic and inertia : " << gradn << std::endl;
+            //     if(std::isnan(gradn)) {
+            //         printf("nan gradn = %f detected after compute computeGradientAndHessian\n",gradn);
+            //         // printf("Hn = ")
+            //         throw std::runtime_error("nan gradn detected after compute computeGradientAndHessian");
+            //     }
+            // }
+
             // std::cout << "computePositionConstraintGradientAndHessian : " << kverts.size() << std::endl;
             // the binder constraint gradient and hessian
             if(use_binder_constraint) {
@@ -2270,7 +2305,7 @@ struct FleshDynamicStepping : INode {
             timer.tick();
             spmat._vals.reset(0);  
 
-            std::cout << "update gh_buffer spmat" << std::endl;
+            // std::cout << "update gh_buffer spmat" << std::endl;
 
             cudaPol(zs::range(eles.size()),
                 [gh_buffer = proxy<space>({},gh_buffer),
@@ -2285,7 +2320,7 @@ struct FleshDynamicStepping : INode {
                     update_hessian(spmat,inds,H,true);
             });
 
-            std::cout << "update sttemp spmat" << std::endl;
+            // std::cout << "update sttemp spmat" << std::endl;
 
              cudaPol(zs::range(sttemp.size()),
                 [sttemp = proxy<space>({},sttemp),vsize = verts.size(),spmat = proxy<space>(spmat)] ZS_LAMBDA(int vi) mutable {
@@ -2297,7 +2332,7 @@ struct FleshDynamicStepping : INode {
                     update_hessian(spmat,inds,H,true);
             });
 
-            std::cout << "update vtemp spmat" << std::endl;
+            // std::cout << "update vtemp spmat" << std::endl;
 
             cudaPol(zs::range(vtemp.size()),
                 [vtemp = proxy<space>({},vtemp),vsize = verts.size(),spmat = proxy<space>(spmat)] ZS_LAMBDA(int vi) mutable {
@@ -2308,7 +2343,7 @@ struct FleshDynamicStepping : INode {
                     update_hessian(spmat,inds,H,true);
             });
 
-            std::cout << "update kinematics spmat" << std::endl;
+            // std::cout << "update kinematics spmat" << std::endl;
 
             if(use_kinematics_collision && kinematics.size() > 0)
                 cudaPol(zs::range(ktris_vert_collision_buffer.size()),[
@@ -2319,7 +2354,7 @@ struct FleshDynamicStepping : INode {
                         update_hessian(spmat,inds,H,true);
                 });
 
-            std::cout << "finish upate hessian" << std::endl;
+            // std::cout << "finish upate hessian" << std::endl;
 
             timer.tock("spmat evaluation");
             #endif
@@ -2341,6 +2376,13 @@ struct FleshDynamicStepping : INode {
             // std::cout << "Hn : " << Hn << std::endl;
             int nm_CG_iters = 0;
             #ifdef USE_SPARSE_MATRIX
+                // auto gradn = TILEVEC_OPS::dot<3>(cudaPol,vtemp,"grad","grad");
+                // if(std::isnan(gradn)) {
+                //     printf("nan gradn = %f detected\n",gradn);
+                //     // printf("Hn = ")
+                //     throw std::runtime_error("nan gradn detected");
+                // }
+
                 if(turn_on_self_collision)
                     nm_CG_iters = PCG::pcg_with_fixed_sol_solve<3>(cudaPol,vtemp,spmat,self_collision_fp_buffer,"dir","bou_tag","grad","P","inds","H",(T)cg_res,max_cg_iters,100);
                 else

projects/CuLagrange/fem/collision_energy/collision_utils.hpp

@@ -21,6 +21,8 @@ namespace COLLISION_UTILS {
     using MATRIX3x12 = typename zs::vec<REAL,3,12>;
     using MATRIX12 = typename zs::vec<REAL,12,12>;
 
+
+
     ///////////////////////////////////////////////////////////////////////
     // should we reverse the direction of the force?
     ///////////////////////////////////////////////////////////////////////
@@ -981,5 +983,137 @@ namespace COLLISION_UTILS {
                                 normal[0], normal[1], normal[2], intersect);
     }
 
+// ps = [p_ea_0,p_ea_1,p_eb_0,p_eb_1]
+    constexpr bool compute_imminent_EE_collision_impulse(const VECTOR3 ps[4],
+        const VECTOR3 vs[4],
+        const REAL& scale,
+        const REAL& imminent_thickness,
+        const REAL minv[4],
+        VECTOR3 imps[4]) {
+            constexpr auto eps = 1e-7;
+
+            VECTOR3 int_a{},int_b{};
+            COLLISION_UTILS::IntersectLineSegments(ps[0],ps[1],ps[2],ps[3],int_a,int_b);
+            auto ra = (ps[0] - int_a).norm() / (ps[0] - ps[1]).norm();
+            auto rb = (ps[2] - int_b).norm() / (ps[2] - ps[3]).norm(); 
+
+            VECTOR4 bary{};
+            bary[0] = ra - 1;
+            bary[1] = -ra;
+            bary[2] = 1 - rb;
+            bary[3] = rb;
+
+            auto cm = (REAL).0;
+            for(int i = 0;i != 4;++i)
+                cm += bary[i] * bary[i] * minv[i];
+            if(cm < eps){
+                // for(int i = 0;i != 4;++i)
+                //     imps[i] = VECTOR3::zeros();
+                return false;;  
+            }
+
+            auto pr = VECTOR3::zeros();
+            auto vr = VECTOR3::zeros();
+            for(int i = 0;i != 4;++i) {
+                pr += bary[i] * ps[i];
+                vr += bary[i] * vs[i];
+            }
+
+            if(pr.norm() > imminent_thickness) {
+                return false;
+            }
+
+            VECTOR3 collision_nrm{};
+            if(pr.norm() < 100 * eps)
+                  collision_nrm = (ps[0] - ps[1]).cross(ps[2] - ps[3]).normalized();
+            else
+                collision_nrm = pr.normalized();    
+
+            if(collision_nrm.dot(vr) > 0)
+                collision_nrm = (REAL)-1 * collision_nrm;
+
+            auto vr_nrm = collision_nrm.dot(vr);           
+            auto impulse = -collision_nrm * vr_nrm * scale;
+
+            for(int i = 0;i != 4;++i)
+                imps[i] = bary[i] * (minv[i] / cm) * impulse;
+
+            return true;
+    }
+
+// ps = [p_ea_0,p_ea_1,p_eb_0,p_eb_1]
+    constexpr bool compute_continous_EE_collision_impulse(
+        const VECTOR3 ps[4],
+        const VECTOR3 vs[4],
+        const REAL minv[4],
+        VECTOR3 imps[4]) {
+            constexpr auto eps = 1e-7;
+
+            auto alpha = (REAL)1.0;
+
+            auto has_dynamic_points = false;
+            for(int i = 0;i != 4;++i)
+                if(minv[i] > eps)
+                    has_dynamic_points = true;
+
+
+            if(!has_dynamic_points || !ee_accd(ps[0],ps[1],ps[2],ps[3],vs[0],vs[1],vs[2],vs[3],(REAL)0.2,(REAL)0.0,alpha)) {
+                for(int i = 0;i != 4;++i)
+                    imps[i] = VECTOR3::zeros();
+                return false;
+            }
+
+            VECTOR3 nps[4] = {};
+            for(int i = 0;i != 4;++i)
+                nps[i] = ps[i] + alpha * vs[i];
+
+// there will surely be an imminent collision
+            return compute_imminent_EE_collision_impulse(nps,vs,(REAL)1 - alpha,std::numeric_limits<REAL>::max(),minv,imps);
+    }
+
+// ps = [t0,t1,t2,p]
+    constexpr bool compute_imminent_PT_collision_impulse(const VECTOR3 ps[4],
+        const VECTOR3 vs[4],
+        const REAL& scale,
+        const REAL& imminent_thickness,
+        const REAL minv[4],
+        VECTOR3 imps[4]) {
+            constexpr auto eps = 1e-7;
+            auto tnrm = LSL_GEO::facet_normal(ps[0],ps[1],ps[2]);
+
+            auto seg = ps[3] - ps[0];
+            auto project_dist = zs::abs(seg.dot(tnrm));
+            if(project_dist > imminent_thickness) {
+                return false;
+            }
+
+            VECTOR3 bary_centric{};
+            LSL_GEO::pointBaryCentric(ps[0],ps[1],ps[2],ps[3],bary_centric);
+
+            // if(distance > imminent_thickness)
+            //     return false;
+
+            auto bary_sum = zs::abs(bary_centric[0]) + zs::abs(bary_centric[1]) + zs::abs(bary_centric[2]);
+            if(bary_sum > (REAL)(1.0 + eps * 1000)) {
+                return false;
+            }
+
+            VECTOR4 bary{-bary_centric[0],-bary_centric[1],-bary_centric[2],1};
+
+            auto pr = VECTOR3::zeros();
+            auto vr = VECTOR3::zeros();
+            for(int i = 0;i != 4;++i) {
+                pr += bary[i] * ps[i];
+                vr += bary[i] * vs[i];
+            }
+
+            auto collision_nrm = LSL_GEO::facet_normal(ps[0],ps[1],ps[2]);
+            auto align = collision_nrm.dot(pr);
+            if(align < 0)
+                collision_nrm *= -1;
+
+            auto relative_normal_velocity = vr.dot(collision_nrm);
+    }
+
 };
 };