diff options
author | over0219 <over0219@umn.edu> | 2020-06-26 04:44:31 +0300 |
---|---|---|
committer | over0219 <over0219@umn.edu> | 2020-06-26 04:44:31 +0300 |
commit | fd7bcb209aa7e888a32f980a01d17f79c59b0c51 (patch) | |
tree | 00fbe05ea6a523932d14a88f42bc32a613b2ddf2 | |
parent | 77ca7ab9b3bee0a2dc1ea0fecaafabc2d2c506ac (diff) |
ope forgot some files
-rw-r--r-- | extern/softbody/src/admmpd_bvh_traverse.cpp | 300 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_bvh_traverse.h | 191 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_collision.cpp | 5 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_embeddedmesh.cpp | 2 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_embeddedmesh.h | 2 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_solver.cpp | 10 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_types.h | 6 |
7 files changed, 504 insertions, 12 deletions
diff --git a/extern/softbody/src/admmpd_bvh_traverse.cpp b/extern/softbody/src/admmpd_bvh_traverse.cpp new file mode 100644 index 00000000000..0695aeef2b5 --- /dev/null +++ b/extern/softbody/src/admmpd_bvh_traverse.cpp @@ -0,0 +1,300 @@ +// Copyright Matt Overby 2020. +// Distributed under the MIT License. + +#ifndef ADMMPD_BVH_H_ +#define ADMMPD_BVH_H_ 1 + +#include "admmpd_bvh_traverse.h" +#include "admmpd_math.h" +#include "BLI_assert.h" +#include "BLI_math_geom.h" + +namespace admmpd { +using namespace Eigen; + +template <typename T> +void PointInTetMeshTraverse<T>::traverse( + const AABB &left_aabb, bool &go_left, + const AABB &right_aabb, bool &go_right, + bool &go_left_first ) +{ + if (left_aabb.contains(point)) + go_left = true; + if (right_aabb.contains(point)) + go_right = true; + (void)(go_left_first); // doesn't matter for point-in-tet +} + +template <typename T> +bool PointInTetMeshTraverse<T>::stop_traversing( + const AABB &aabb, int prim ) +{ + BLI_assert(prim_verts->cols()==3); + BLI_assert(prim_inds->cols()==4); + + if (!aabb.contains(point)) + return false; + + RowVector4i t = prim_inds->row(prim); + VecType v[4] = { + prim_verts->row(t[0]), + prim_verts->row(t[1]), + prim_verts->row(t[2]), + prim_verts->row(t[3]) + }; + + bool hit = point_in_tet( + point.template cast<double>(), + v[0].template cast<double>(), + v[1].template cast<double>(), + v[2].template cast<double>(), + v[3].template cast<double>()); + + if (hit) + output.prim = prim; + + return hit; // stop traversing if hit +} + +template <typename T> +PointInTriangleMeshTraverse<T>::PointInTriangleMeshTraverse( + const VecType &point_, + const MatrixXType *prim_verts_, + const Eigen::MatrixXi *prim_inds_) : + point(point_), + dir(0,0,1), + prim_verts(prim_verts_), + prim_inds(prim_inds_) +{ + BLI_assert(prim_verts->rows()>=0); + BLI_assert(prim_inds->rows()>=0); + BLI_assert(prim_inds->cols()==3); + dir.normalize(); // TODO random unit vector + for (int i=0; i<3; ++i) + { + o[i] = (float)point[i]; + d[i] = (float)dir[i]; + } +} + +template <typename T> +void PointInTriangleMeshTraverse<T>::traverse( + const AABB &left_aabb, bool &go_left, + const AABB &right_aabb, bool &go_right, + bool &go_left_first ) +{ + float tmin = 0; + float tmax = std::numeric_limits<float>::max(); + float l_bb_min[3] = { (float)left_aabb.min()[0], (float)left_aabb.min()[1], (float)left_aabb.min()[2] }; + float l_bb_max[3] = { (float)left_aabb.max()[0], (float)left_aabb.max()[1], (float)left_aabb.max()[2] }; + go_left = isect_ray_aabb_v3_simple(o,d,l_bb_min,l_bb_max,&tmin,&tmax); + tmin = 0; + tmax = std::numeric_limits<float>::max(); + float r_bb_min[3] = { (float)right_aabb.min()[0], (float)right_aabb.min()[1], (float)right_aabb.min()[2] }; + float r_bb_max[3] = { (float)right_aabb.max()[0], (float)right_aabb.max()[1], (float)right_aabb.max()[2] }; + go_right = isect_ray_aabb_v3_simple(o,d,r_bb_min,r_bb_max,&tmin,&tmax); +} // end point in mesh traverse + +template <typename T> +bool PointInTriangleMeshTraverse<T>::stop_traversing( + const AABB &aabb, int prim ) +{ + // Check if the tet box doesn't intersect the triangle box + { + float tmin = 0; + float tmax = std::numeric_limits<float>::max(); + float bb_min[3] = { (float)aabb.min()[0], (float)aabb.min()[1], (float)aabb.min()[2] }; + float bb_max[3] = { (float)aabb.max()[0], (float)aabb.max()[1], (float)aabb.max()[2] }; + if (!isect_ray_aabb_v3_simple(o,d,bb_min,bb_max,&tmin,&tmax)) + return false; + } + + // Get the vertices of the face in float arrays + // to interface with Blender kernels. + BLI_assert(prim >= 0 && prim < prim_inds->rows()); + RowVector3i q_f = prim_inds->row(prim); + BLI_assert(q_f[0] < prim_verts->rows()); + BLI_assert(q_f[1] < prim_verts->rows()); + BLI_assert(q_f[2] < prim_verts->rows()); + float q0[3], q1[3], q2[3]; + for (int i=0; i<3; ++i) + { + q0[i] = (float)prim_verts->operator()(q_f[0],i); + q1[i] = (float)prim_verts->operator()(q_f[1],i); + q2[i] = (float)prim_verts->operator()(q_f[2],i); + } + + // If we didn't have a triangle-triangle intersection + // then record if it was a ray-hit. + float lambda = 0; + float uv[2] = {0,0}; + bool hit = isect_ray_tri_watertight_v3_simple( + o, d, q0, q1, q2, &lambda, uv); + + if (hit) + output.hits.emplace_back(std::make_pair(prim,lambda)); + + return false; // multi-hit, so keep traversing + +} // end point in mesh stop traversing + +template <typename T> +void NearestTriangleTraverse<T>::traverse( + const AABB &left_aabb, bool &go_left, + const AABB &right_aabb, bool &go_right, + bool &go_left_first) +{ + T l_d2 = left_aabb.exteriorDistance(point); + go_left = l_d2 < output.dist; + T r_d2 = right_aabb.exteriorDistance(point); + go_right = r_d2 < output.dist; + go_left_first = go_left < go_right; +} + +template <typename T> +bool NearestTriangleTraverse<T>::stop_traversing(const AABB &aabb, int prim) +{ + BLI_assert(prim >= 0); + BLI_assert(prim < prim_inds->rows()); + BLI_assert(prim_inds->cols()==3); + + T b_dist = aabb.exteriorDistance(point); + if (b_dist > output.dist) + return false; + + float p[3] = { (float)point[0], (float)point[1], (float)point[2] }; + float r[3] = { p[0], p[1], p[2] }; + RowVector3i tri = prim_inds->row(prim); + float t1[3], t2[3], t3[3]; + for (int i=0; i<3; ++i) + { + t1[i] = (float)prim_verts->operator()(tri[0],i); + t2[i] = (float)prim_verts->operator()(tri[1],i); + t3[i] = (float)prim_verts->operator()(tri[2],i); + } + + // I was hoping there would be kernels that are a bit faster + // to get point-triangle distance, but I guess this does what I need. + closest_on_tri_to_point_v3(r, p, t1, t2, t3); + VecType pt_on_tri((T)r[0], (T)r[1], (T)r[2]); + double d2 = (point-pt_on_tri).norm(); + if (d2 < output.dist) + { + output.prim = prim; + output.dist = d2; + output.pt_on_tri = pt_on_tri; + } + + return false; +} + +template <typename T> +TetIntersectsMeshTraverse<T>::TetIntersectsMeshTraverse( + const VecType points_[4], + const MatrixXType *prim_verts_, + const Eigen::MatrixXi *prim_inds_) : + prim_verts(prim_verts_), + prim_inds(prim_inds_) +{ + for (int i=0; i<4; ++i) + points[i] = points_[i]; + + BLI_assert(prim_verts->cols()==3); + BLI_assert(prim_inds->cols()==3); + + for(int i=0; i<3; ++i) + { + p0[i] = (float)points[0][i]; + p1[i] = (float)points[1][i]; + p2[i] = (float)points[2][i]; + p3[i] = (float)points[3][i]; + } + + tet_faces.resize(4,std::vector<float*>()); + tet_faces[0] = {p0,p1,p2}; + tet_faces[1] = {p0,p2,p3}; + tet_faces[2] = {p0,p3,p1}; + tet_faces[3] = {p1,p2,p3}; + + tet_aabb.setEmpty(); + for (int i=0; i<4; ++i) + tet_aabb.extend(points[i]); + +} // end point in mesh constructor + +template <typename T> +void TetIntersectsMeshTraverse<T>::traverse( + const AABB &left_aabb, bool &go_left, + const AABB &right_aabb, bool &go_right, + bool &go_left_first ) +{ + go_left = false; + go_right = false; + + if (tet_aabb.intersects(left_aabb)) + go_left = true; + if (tet_aabb.intersects(right_aabb)) + go_right = true; + + go_left_first = true; + if (go_right && !go_left) + go_left_first = false; + +} // end point in mesh traverse + +template <typename T> +bool TetIntersectsMeshTraverse<T>::stop_traversing( + const AABB &aabb, int prim ) +{ + bool tet_hits_aabb = tet_aabb.intersects(aabb); + if(!tet_hits_aabb) + { + return false; + } + + // Get the vertices of the face in float arrays + // to interface with Blender kernels. + BLI_assert(prim >= 0 && prim < prim_inds->rows()); + RowVector3i q_f = prim_inds->row(prim); + float q0[3], q1[3], q2[3]; + for (int i=0; i<3; ++i) + { + q0[i] = (float)prim_verts->operator()(q_f[0],i); + q1[i] = (float)prim_verts->operator()(q_f[1],i); + q2[i] = (float)prim_verts->operator()(q_f[2],i); + } + + // If the tet-aabb intersects the triangle-aabb, then test + // the four faces of the tet against the triangle. + for (int i=0; i<4; ++i) + { + float r_i1[3] = {0,0,0}; + float r_i2[3] = {0,0,0}; + const std::vector<float*> &f = tet_faces[i]; + bool hit = isect_tri_tri_epsilon_v3( + f[0], f[1], f[2], q0, q1, q2, r_i1, r_i2, 1e-8); + if (hit) + { + output.hit_face = prim; + return true; + } + } + + return false; // multi-hit, so keep traversing + +} // end point in mesh stop traversing + +// Compile template types +template class admmpd::PointInTetMeshTraverse<double>; +template class admmpd::PointInTetMeshTraverse<float>; +template class admmpd::PointInTriangleMeshTraverse<double>; +template class admmpd::PointInTriangleMeshTraverse<float>; +template class admmpd::NearestTriangleTraverse<double>; +template class admmpd::NearestTriangleTraverse<float>; +template class admmpd::TetIntersectsMeshTraverse<double>; +template class admmpd::TetIntersectsMeshTraverse<float>; + +} // namespace admmpd + +#endif // ADMMPD_BVH_H_ + diff --git a/extern/softbody/src/admmpd_bvh_traverse.h b/extern/softbody/src/admmpd_bvh_traverse.h new file mode 100644 index 00000000000..774eb6a6b10 --- /dev/null +++ b/extern/softbody/src/admmpd_bvh_traverse.h @@ -0,0 +1,191 @@ +// Copyright Matt Overby 2020. +// Distributed under the MIT License. + +#ifndef ADMMPD_BVH_TRAVERSE_H_ +#define ADMMPD_BVH_TRAVERSE_H_ 1 + +#include <Eigen/Geometry> +#include <vector> + +namespace admmpd { + +// Traverse class for traversing the structures. +template <typename T, int DIM> +class Traverser +{ +protected: + typedef Eigen::AlignedBox<T,DIM> AABB; +public: + // Set the boolean flags if we should go left, right, or both. + // Default for all booleans is true if left unchanged. + // Note that if stop_traversing ever returns true, it may not + // go left/right, even if you set go_left/go_right. + virtual void traverse( + const AABB &left_aabb, bool &go_left, + const AABB &right_aabb, bool &go_right, + bool &go_left_first) = 0; + + // Return true to stop traversing. + // I.e., returning true is equiv to "hit anything stop checking", + // finding a closest object should return false (continue traversing). + virtual bool stop_traversing(const AABB &aabb, int prim) = 0; +}; + +// Point in tet mesh traversal +template <typename T> +class PointInTetMeshTraverse : public Traverser<T,3> +{ +protected: + using typename Traverser<T,3>::AABB; + typedef Eigen::Matrix<T,3,1> VecType; + typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType; + + VecType point; + const MatrixXType *prim_verts; + const Eigen::MatrixXi *prim_inds; + +public: + struct Output { + int prim; // -1 if no intersections + Output() : prim(-1) {} + } output; + + PointInTetMeshTraverse( + const VecType &point_, + const MatrixXType *prim_verts_, + const Eigen::MatrixXi *prim_inds_) : + point(point_), + prim_verts(prim_verts_), + prim_inds(prim_inds_) + {} + + void traverse( + const AABB &left_aabb, bool &go_left, + const AABB &right_aabb, bool &go_right, + bool &go_left_first); + + bool stop_traversing(const AABB &aabb, int prim); +}; + +// Point in triangle mesh traversal. +// Determined by launching a ray in a random direction from +// the point and counting the number of (watertight) intersections. If +// the number of intersections is odd, the point is inside th mesh. +template <typename T> +class PointInTriangleMeshTraverse : public Traverser<T,3> +{ +protected: + using typename Traverser<T,3>::AABB; + typedef Eigen::Matrix<T,3,1> VecType; + typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType; + + VecType point, dir; + const MatrixXType *prim_verts; // triangle mesh verts + const Eigen::MatrixXi *prim_inds; // triangle mesh inds + float o[3], d[3]; // pt and dir casted to float for Blender kernels + +public: + struct Output { + std::vector< std::pair<int,double> > hits; // [prim,t] + int num_hits() const { return hits.size(); } + } output; + + PointInTriangleMeshTraverse( + const VecType &point_, + const MatrixXType *prim_verts_, + const Eigen::MatrixXi *prim_inds_); + + void traverse( + const AABB &left_aabb, bool &go_left, + const AABB &right_aabb, bool &go_right, + bool &go_left_first); + + // Always returns false (multi-hit, so it doesn't stop) + bool stop_traversing(const AABB &aabb, int prim); +}; + +// Search for the nearest triangle to a given point +template <typename T> +class NearestTriangleTraverse : public Traverser<T,3> +{ +protected: + using typename Traverser<T,3>::AABB; + typedef Eigen::Matrix<T,3,1> VecType; + typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType; + + VecType point; + const MatrixXType *prim_verts; // triangle mesh verts + const Eigen::MatrixXi *prim_inds; // triangle mesh inds + +public: + struct Output { + int prim; + T dist; + VecType pt_on_tri; + Output() : + prim(-1), + dist(std::numeric_limits<T>::max()), + pt_on_tri(0,0,0) + {} + } output; + + NearestTriangleTraverse( + const VecType &point_, + const MatrixXType *prim_verts_, + const Eigen::MatrixXi *prim_inds_) : + point(point_), + prim_verts(prim_verts_), + prim_inds(prim_inds_) + {} + + void traverse( + const AABB &left_aabb, bool &go_left, + const AABB &right_aabb, bool &go_right, + bool &go_left_first); + + // Always returns false + bool stop_traversing(const AABB &aabb, int prim); +}; + +// Check if a tet intersects a triangle mesh +// with tri-tri collision tests +template <typename T> +class TetIntersectsMeshTraverse : public Traverser<T,3> +{ +protected: + using typename Traverser<T,3>::AABB; + typedef Eigen::Matrix<T,3,1> VecType; + typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType; + + const MatrixXType *prim_verts; + const Eigen::MatrixXi *prim_inds; + VecType points[4]; // verts of the tet with proper winding (0,1,2,3) + float p0[3], p1[3], p2[3], p3[3]; // points casted to floats + std::vector<std::vector<float*> > tet_faces; + AABB tet_aabb; + +public: + struct Output { + int hit_face; // first found + int ray_hit_count; + Output() : hit_face(-1) {} + } output; + + TetIntersectsMeshTraverse( + const VecType points_[4], + const MatrixXType *prim_verts_, + const Eigen::MatrixXi *prim_inds_); + + void traverse( + const AABB &left_aabb, bool &go_left, + const AABB &right_aabb, bool &go_right, + bool &go_left_first); + + // Returns true if intersection + bool stop_traversing(const AABB &aabb, int prim); +}; + +} // namespace admmpd + +#endif // ADMMPD_BVH_TRAVERSE_H_ + diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp index be6bd0a21b7..bbda72f2428 100644 --- a/extern/softbody/src/admmpd_collision.cpp +++ b/extern/softbody/src/admmpd_collision.cpp @@ -247,8 +247,6 @@ int EmbeddedMeshCollision::detect( return vf_pairs.size(); } // end detect -static Vector3d vx0(0,0,0); - void EmbeddedMeshCollision::jacobian( const Eigen::MatrixXd *x, std::vector<Eigen::Triplet<double> > *trips_x, @@ -286,7 +284,8 @@ void EmbeddedMeshCollision::jacobian( int tet_idx = mesh->vtx_to_tet[emb_p_idx]; RowVector4i tet = mesh->tets.row(tet_idx); - // Basically a pin constraint + // Okay this is pretty ugly. I'm going to have to think about + // whether or not this is reasonable. for (int j=0; j<3; ++j) { int c_idx = l->size(); diff --git a/extern/softbody/src/admmpd_embeddedmesh.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp index 3476c34c720..68cd38b27d5 100644 --- a/extern/softbody/src/admmpd_embeddedmesh.cpp +++ b/extern/softbody/src/admmpd_embeddedmesh.cpp @@ -71,7 +71,7 @@ bool EmbeddedMesh::generate( { // How big the grid cells are as a fraction // of the total mesh. - static const double GRID_FRAC = 0.2; + static const double GRID_FRAC = 0.15; if (emb_mesh==NULL) return false; diff --git a/extern/softbody/src/admmpd_embeddedmesh.h b/extern/softbody/src/admmpd_embeddedmesh.h index 8b0cdc5bb04..85c64c7b0a5 100644 --- a/extern/softbody/src/admmpd_embeddedmesh.h +++ b/extern/softbody/src/admmpd_embeddedmesh.h @@ -30,7 +30,7 @@ public: const Eigen::MatrixXd *x_embed, // embedded vertices, p x 3 const Eigen::MatrixXd *x_tets, // lattice vertices, n x 3 Eigen::VectorXd *masses_tets, // masses of the lattice verts - double density_kgm3 = 1100); + double density_kgm3 = 2100); protected: diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp index 7e0c177ea18..ef1c1b50ca4 100644 --- a/extern/softbody/src/admmpd_solver.cpp +++ b/extern/softbody/src/admmpd_solver.cpp @@ -46,6 +46,8 @@ bool Solver::init( if (!compute_matrices(options,data)) return false; + printf("Solver::init:\n\tNum tets: %d\n\tNum verts: %d\n",T.rows(),V.rows()); + return true; } // end init @@ -304,21 +306,21 @@ void Solver::solve_conjugate_gradients( for (int iter=0; iter<options->max_cg_iters; ++iter) { - for( int i=0; i<3; ++i ) + for (int i=0; i<3; ++i) cgdata->Ap.col(i).noalias() = cgdata->A[i]*cgdata->p.col(i); double p_dot_Ap = mat_inner(cgdata->p,cgdata->Ap); - if( p_dot_Ap==0.0 ) + if (p_dot_Ap==0.0) break; double zk_dot_rk = mat_inner(cgdata->z,cgdata->r); - if( zk_dot_rk==0.0 ) + if (zk_dot_rk==0.0) break; double alpha = zk_dot_rk / p_dot_Ap; data->x.noalias() += alpha * cgdata->p; cgdata->r.noalias() -= alpha * cgdata->Ap; - if( cgdata->r.lpNorm<Infinity>() < eps ) + if (cgdata->r.lpNorm<Infinity>() < eps) break; solve_Ax_b(data,&cgdata->z,&cgdata->r); diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h index 42277180da7..13edb93c3da 100644 --- a/extern/softbody/src/admmpd_types.h +++ b/extern/softbody/src/admmpd_types.h @@ -25,11 +25,11 @@ struct Options { Eigen::Vector3d grav; Options() : timestep_s(1.0/24.0), - max_admm_iters(100), + max_admm_iters(50), max_cg_iters(10), mult_k(1), min_res(1e-6), - youngs(10000000), + youngs(1000000), poisson(0.299), grav(0,0,-9.8) {} @@ -76,7 +76,7 @@ struct SolverData { Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > ldltA; struct CGData { // Temporaries used in conjugate gradients RowSparseMatrix<double> A[3]; // (M + D'W^2D) + k * Kt K - Eigen::MatrixXd b; // M xbar + DtW2(z-u) + Kt l + Eigen::MatrixXd b; // M xbar + DtW2(z-u) + k Kt l Eigen::MatrixXd r; // residual Eigen::MatrixXd z; Eigen::MatrixXd p; |