several bugfixes but I think I'm going to need to change the way I handle constrained solve

1 files changed, 73 insertions, 23 deletions

diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index f49a314db47..f9c6363b89f 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -21,13 +21,16 @@ struct VFCollisionPair {
     VFCollisionPair();
 };
 
-// I'll update this class/structure another day.
-// For now let's get something in place to do floor collisions.
-// Probably will work better to use uber-collision class for
-// all self and obstacle collisions, reducing the amount of
-// for-all vertices loops.
 class Collision {
 public:
+    // Obstacle data created in set_obstacles
+    struct ObstacleData {
+        Eigen::MatrixXd V0, V1;
+        Eigen::MatrixXi F;
+        std::vector<Eigen::AlignedBox<double,3> > aabbs;
+        AABBTree<double,3> tree;
+    } obsdata;
+
     virtual ~Collision() {}
 
     // Performs collision detection.
@@ -37,12 +40,14 @@ public:
         const Eigen::MatrixXd *x1) = 0;
 
     // Set the soup of obstacles for this time step.
+    // I don't really like having to switch up interface style, but we'll
+    // do so here to avoid copies that would happen in admmpd_api.
     virtual void set_obstacles(
         const float *v0,
         const float *v1,
         int nv,
         const unsigned int *faces,
-        int nf) = 0;
+        int nf);
 
     // Special case for floor since it's common.
     virtual void set_floor(double z) = 0;
@@ -56,6 +61,21 @@ public:
         std::vector<Eigen::Triplet<double> > *trips_y,
     	std::vector<Eigen::Triplet<double> > *trips_z,
 		std::vector<double> *l) = 0;
+
+    // Given a point and a surface mesh,
+    // perform discrete collision and create
+    // a vertex-face collision pair if colliding.
+    // Also adds collision pairs if below floor.
+    static void detect_discrete_vf(
+        const Eigen::Vector3d &pt,
+        int pt_idx,
+        bool pt_is_obs,
+        const AABBTree<double,3> *mesh_tree,
+        const Eigen::MatrixXd *mesh_x,
+        const Eigen::MatrixXi *mesh_tris,
+        bool mesh_is_obs,
+        double floor_z,
+        std::vector<VFCollisionPair> *pairs);
 };
 
 // Collision detection against multiple meshes
@@ -66,23 +86,6 @@ public:
         floor_z(-std::numeric_limits<double>::max())
         {}
 
-    // Obstacle data created in set_obstacles
-    struct ObstacleData {
-        Eigen::MatrixXd V0, V1;
-        Eigen::MatrixXi F;
-        std::vector<Eigen::AlignedBox<double,3> > aabbs;
-        AABBTree<double,3> tree;
-    } obsdata;
-
-    // I don't really like having to switch up interface style, but we'll
-    // do so here to avoid copies that would happen in admmpd_api.
-    void set_obstacles(
-        const float *v0,
-        const float *v1,
-        int nv,
-        const unsigned int *faces,
-        int nf);
-
     // A floor is so common that it makes sense to hard
     // code floor collision instead of using a floor mesh.
     void set_floor(double z)
@@ -121,6 +124,53 @@ protected:
     { (void)(x0); (void)(x1); }
 };
 
+/*
+class TetMeshCollision : public Collision {
+public:
+    TetMeshCollision(const TetMeshData *mesh_) :
+        mesh(mesh_),
+        floor_z(-std::numeric_limits<double>::max())
+        {}
+
+    // Performs collision detection.
+    // Returns the number of active constraints.
+    int detect(
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1);
+
+    // Special case for floor since it's common.
+    void set_floor(double z)
+    {
+        floor_z = z;
+    }
+
+    // Linearize the constraints and return Jacobian tensor.
+    // Constraints are linearized about x for constraint
+    // K x = l
+    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;
+
+protected:
+    const TetMeshData *mesh;
+    double floor_z;
+
+    // Pairs are compute on detect
+    std::vector<VFCollisionPair> vf_pairs;
+
+    // Updates the tetmesh BVH for self collisions.
+    // Called by detect()
+    // TODO
+    void update_bvh(
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1)
+    { (void)(x0); (void)(x1); }
+};
+*/
+
 } // namespace admmpd
 
 #endif // ADMMPD_COLLISION_H_