[Bf-blender-cvs] [a125171beca] soc-2020-soft-body: obstacle collisions. REALLY need to improve mass computation
over0219
noreply at git.blender.org
Tue Jun 23 19:45:52 CEST 2020
Commit: a125171beca714c2bf9e71347da56b14c7195153
Author: over0219
Date: Tue Jun 23 12:45:46 2020 -0500
Branches: soc-2020-soft-body
https://developer.blender.org/rBa125171beca714c2bf9e71347da56b14c7195153
obstacle collisions. REALLY need to improve mass computation
===================================================================
M extern/softbody/src/admmpd_collision.cpp
M extern/softbody/src/admmpd_collision.h
M extern/softbody/src/admmpd_embeddedmesh.cpp
M extern/softbody/src/admmpd_math.cpp
M extern/softbody/src/admmpd_math.h
M extern/softbody/src/admmpd_solver.cpp
M extern/softbody/src/admmpd_solver.h
M extern/softbody/src/admmpd_types.h
===================================================================
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index 78675ea3a0b..98c5653e7f7 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -3,18 +3,24 @@
#include "admmpd_collision.h"
#include "admmpd_bvh_traverse.h"
+#include "admmpd_math.h"
+
#include "BLI_assert.h"
#include "BLI_task.h"
#include "BLI_threads.h"
+#include <iostream>
+
namespace admmpd {
using namespace Eigen;
VFCollisionPair::VFCollisionPair() :
- p(-1), // point
+ p_idx(-1), // point
p_is_obs(0), // 0 or 1
- q(-1), // face
- q_is_obs(0) // 0 or 1
+ q_idx(-1), // face
+ q_is_obs(0), // 0 or 1
+ pt_on_q(0,0,0),
+ q_n(0,0,0)
{}
void EmbeddedMeshCollision::set_obstacles(
@@ -66,24 +72,26 @@ typedef struct DetectThreadData {
const EmbeddedMeshCollision::ObstacleData *obsdata;
const Eigen::MatrixXd *x0;
const Eigen::MatrixXd *x1;
+ double floor_z;
std::vector<std::vector<VFCollisionPair> > *pt_vf_pairs; // per thread pairs
} DetectThreadData;
-static void parallel_detect(
+static void parallel_detect_obstacles(
void *__restrict userdata,
const int i,
const TaskParallelTLS *__restrict tls)
{
+ DetectThreadData *td = (DetectThreadData*)userdata;
+
// Comments say "don't use this" but how else am I supposed
- // to get the thread ID?
+ // to get the thread ID? It appears to return the right thing...
int thread_idx = BLI_task_parallel_thread_id(tls);
- DetectThreadData *td = (DetectThreadData*)userdata;
std::vector<VFCollisionPair> &tl_pairs = td->pt_vf_pairs->at(thread_idx);
int tet_idx = td->mesh->vtx_to_tet[i];
RowVector4i tet = td->mesh->tets.row(tet_idx);
Vector4d bary = td->mesh->barys.row(i);
-
+
// First, get the surface vertex
Vector3d pt =
bary[0] * td->x1->row(tet[0]) +
@@ -91,6 +99,23 @@ static void parallel_detect(
bary[2] * td->x1->row(tet[2]) +
bary[3] * td->x1->row(tet[3]);
+ // Special case, check if we are below the floor
+ if (pt[2] < td->floor_z)
+ {
+ tl_pairs.emplace_back();
+ VFCollisionPair &pair = tl_pairs.back();
+ pair.p_idx = i;
+ pair.p_is_obs = 0;
+ pair.q_idx = -1;
+ pair.q_is_obs = 1;
+ pair.pt_on_q = Vector3d(pt[0],pt[1],td->floor_z);
+ pair.q_n = Vector3d(0,0,1);
+ }
+
+ // Any obstacles?
+ if (td->obsdata->F.rows()==0)
+ return;
+
// TODO
// This won't work for overlapping obstacles.
// We would instead need something like a signed distance field
@@ -103,9 +128,51 @@ static void parallel_detect(
return;
// If we are inside an obstacle, we
- // have to project to the nearest surface
+ // have to project to the nearest surface.
+
+ // TODO
+ // Consider replacing this with BLI codes:
+ // BLI_bvhtree_find_nearest in BLI_kdopbvh.h
+ // But since it doesn't have a point-in-mesh
+ // detection, we may as use our own BVH/traverser
+ // for now.
+
+ NearestTriangleTraverse<double> find_nearest_tri(
+ pt, &td->obsdata->V1, &td->obsdata->F);
+ td->obsdata->tree.traverse(find_nearest_tri);
+ if (find_nearest_tri.output.prim < 0) // error
+ return;
+
+ // Create collision pair
+ tl_pairs.emplace_back();
+ VFCollisionPair &pair = tl_pairs.back();
+ pair.p_idx = i;
+ pair.p_is_obs = 0;
+ pair.q_idx = find_nearest_tri.output.prim;
+ pair.q_is_obs = 1;
+ pair.pt_on_q = find_nearest_tri.output.pt_on_tri;
+
+ // Compute face normal
+ BLI_assert(pair.q_idx >= 0);
+ BLI_assert(pair.q_idx < td->obsdata->F.rows());
+ RowVector3i tri_inds = td->obsdata->F.row(pair.q_idx);
+ Vector3d tri[3] = {
+ td->obsdata->V1.row(tri_inds[0]),
+ td->obsdata->V1.row(tri_inds[1]),
+ td->obsdata->V1.row(tri_inds[2])
+ };
+ pair.q_n = ((tri[1]-tri[0]).cross(tri[2]-tri[0])).normalized();
+
+} // end parallel detect against obstacles
-} // end parallel lin solve
+static void parallel_detect(
+ void *__restrict userdata,
+ const int i,
+ const TaskParallelTLS *__restrict tls)
+{
+ parallel_detect_obstacles(userdata,i,tls);
+ //parallel_detect_self(userdata,i,tls);
+} // end parallel detect
int EmbeddedMeshCollision::detect(
const Eigen::MatrixXd *x0,
@@ -120,18 +187,36 @@ int EmbeddedMeshCollision::detect(
std::vector<std::vector<VFCollisionPair> > pt_vf_pairs
(max_threads, std::vector<VFCollisionPair>());
+
+
+
+
+
+floor_z = -2;
+
+
+
+
+
+
+
+
+
+
+
DetectThreadData thread_data = {
.mesh = mesh,
.obsdata = &obsdata,
.x0 = x0,
.x1 = x1,
+ .floor_z = floor_z,
.pt_vf_pairs = &pt_vf_pairs
};
- int nv = x1->rows();
+ int nev = mesh->x_rest.rows();
TaskParallelSettings settings;
BLI_parallel_range_settings_defaults(&settings);
- BLI_task_parallel_range(0, nv, &thread_data, parallel_detect, &settings);
+ BLI_task_parallel_range(0, nev, &thread_data, parallel_detect, &settings);
// Combine thread-local results
vf_pairs.clear();
@@ -142,80 +227,55 @@ int EmbeddedMeshCollision::detect(
}
return vf_pairs.size();
-}
- /*
-void EmbeddedMeshCollision::detect(const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1)
-{
-
- // First, update the positions of the embedded vertex
- // and perform collision detection against obstacles
- int n_ev = emb_V0.rows();
- std::vector<int> colliding(n_ev,0);
- for (int i=0; i<n_ev; ++i)
- {
- int t = vtx_to_tet->operator[](i);
- BLI_assert(t >= 0);
- BLI_assert(t < tets->rows());
- RowVector4i tet = tets->row(t);
- Vector4d bary = emb_barys->row(i);
-// emb_V0.row(i) =
-// bary[0] * x0->row(tet[0]) +
-// bary[1] * x0->row(tet[1]) +
-// bary[2] * x0->row(tet[2]) +
-// bary[3] * x0->row(tet[3]);
- Vector3d pt =
- bary[0] * x1->row(tet[0]) +
- bary[1] * x1->row(tet[1]) +
- bary[2] * x1->row(tet[2]) +
- bary[3] * x1->row(tet[3]);
-// emb_V1.row(i) =
-
- // Check if we are inside the mesh.
- // If so, find the nearest face in the rest pose.
- PointInTriangleMeshTraverse<double> pt_in_mesh(pt, &V1, &F);
- obs_tree.traverse(pt_in_mesh);
- if (pt_in_mesh.output.num_hits() % 2 == 1)
- {
- // Find
-// hit = true;
- }
-
-// colliding[i] = hit;
- }
+} // end detect
- // Only bother with self collision if it
- // is not colliding with an obstacle.
- // This is only useful for discrete tests.
-
-} // end emb collision detect
-*/
-/*
-void FloorCollider::detect(const Eigen::MatrixXd *x)
-{
- (void)(x);
- // Can just do detection in jacobian I guess.
-}
-
-void FloorCollider::jacobian(
+void EmbeddedMeshCollision::jacobian(
const Eigen::MatrixXd *x,
std::vector<Eigen::Triplet<double> > *trips_x,
std::vector<Eigen::Triplet<double> > *trips_y,
std::vector<Eigen::Triplet<double> > *trips_z,
std::vector<double> *l)
{
- const double floor_z = -2.0;
+ BLI_assert(x != NULL);
+ BLI_assert(x->cols() == 3);
+
+ int np = vf_pairs.size();
+ if (np==0)
+ return;
+
+ l->reserve((int)l->size() + np);
+ trips_x->reserve((int)trips_x->size() + np*3);
+ trips_y->reserve((int)trips_y->size() + np*3);
+ trips_z->reserve((int)trips_z->size() + np*3);
- int nx = x->rows();
- for (int i=0; i<nx; ++i)
+ for (int i=0; i<np; ++i)
{
- Eigen::Vector3d xi = x->row(i);
- if (xi[2]>floor_z)
+ VFCollisionPair &pair = vf_pairs[i];
+
+ int emb_p_idx = pair.p_idx;
+ if (pair.p_is_obs)
+ { // TODO: obstacle point intersecting mesh
continue;
+ }
+
+ // Obstacle collision
+ if (pair.q_is_obs)
+ {
+ RowVector4d bary = mesh->barys.row(emb_p_idx);
+ int tet_idx = mesh->vtx_to_tet[emb_p_idx];
+ RowVector4i tet = mesh->tets.row(tet_idx);
+ int c_idx = l->size();
+ for( int j=0; j<4; ++j ){
+ trips_x->emplace_back(c_idx, tet[j], bary[j]*pair.q_n[0]);
+ trips_y->emplace_back(c_idx, tet[j], bary[j]*pair.q_n[1]);
+ trips_z->emplace_back(c_idx, tet[j], bary[j]*pair.q_n[2]);
+ }
+ l->emplace_back( pair.q_n.dot(pair.pt_on_q));
+ continue;
+ }
+
+ } // end loop pairs
+
+} // end jacobian
- int c_idx = l->size();
- trips_z->emplace_back(c_idx,i,1.0);
- l->emplace_back(floor_z);
- }
-} // end floor collider Jacobian
-*/
} // namespace admmpd
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index 34b870a30f3..f49a314db47 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -10,12 +10,15 @@
namespace admmpd {
struct VFCollisionPair {
- int p; // point
+ int p_idx; // point
int p_is_obs; // 0 or 1
- int q; // face
+ int q_idx; // face
int q_is_obs; // 0 or 1
+ Eigen::Vector3d pt_on_q; // point of collision on q
+// int floor; // 0 or 1, special case
+// Eigen::Vector3d barys;
+ Eigen::Vector3d q_n; // face normal
VFCollisionPair();
- Eigen::Vector3d barys;
};
// I'll update this class/structure another day.
@@ -27,11 +30,13 @@ class Collision {
public:
virtual ~Collision() {}
- // Returns the number of active constraints
+ // Performs collision detection.
+ // Returns the number of active constraints.
virtual int detect(
const Eigen::MatrixXd *x0,
const Eigen::MatrixXd *x1) = 0;
+ // Set the soup of obstacles for this time step.
virtual void set_obstacles(
const float *v0,
const float *v1,
@@ -39,12 +44,18 @@ public:
const unsigned int *faces,
int nf) = 0;
-// virtual void jacobian(
-// const Eigen::MatrixXd *x,
-// std::vector<Eigen::Triplet<double> > *trips_x,
-// std::vector<Eigen::Triplet<double> > *trips_y,
-// std::vector<Eigen::Triplet<double> > *trips_z,
-// std::vector<double> *l) = 0;
+ // Special case for floor since it's common.
+ virtual void set_floor(double z) = 0;
+
+ // Linearize the constraints and return Jacobian tensor.
+ // Constraints are linearized about x for constraint
+ // K x = l
+ virtual void jacobian(
+ const Eigen::MatrixXd *x,
+ std::vector<Eigen::Triplet<double> > *trips_x,
+ std::vector<Eigen::Triplet<double> > *trips_y,
+ std::vector<Eigen::Triplet<double> > *trips_z,
+ std::vector<double> *l) = 0;
};
// Collision detection against multiple meshes
@@ -91,10 +102,7 @@ public:
std::vector<Eigen::Triplet<double> > *trips_x,
std::vector<Eigen::Triplet<double> > *trips_y,
std::vector<Eigen::Triplet<double> > *trips_z,
- std::vector<double>
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list