FCL in, just need to compute correct dPdQ

dilevin · Apr 16, 2018 · 3d72727 · 3d72727
1 parent a387bf9
commit 3d72727
Show file tree

Hide file tree

Showing 13 changed files with 250 additions and 55 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -173,6 +173,7 @@ set(FCL_BUILD_TESTS OFF CACHE INTERNAL "" FORCE)
 set(FCL_WITH_OCTOMAP OFF CACHE INTERNAL "" FORCE)
 set(BUILD_SHARED_LIBS OFF CACHE INTERNAL "" FORCE) 
 set(FCL_STATIC_LIBRARY ON CACHE INTERNAL "" FORCE)
+set(ENABLE_DOUBLE_PRECISION ON CACHE INTERNAL "" FORCE)
 #set ccd include directory 
 #set(octomap_DIR ${PROJECT_SOURCE_DIR}/ThirdParty/octomap/octomap CACHE INTERNAL  "" FORCE)
 

diff --git a/ThirdParty/fcl b/ThirdParty/fcl
diff --git a/src/Collisions/include/CollisionDetector.h b/src/Collisions/include/CollisionDetector.h
@@ -68,22 +68,54 @@ namespace Gauss {
 
                         //Build Jacobian differently for vertex collisions and in element collision events
                         static_if<std::remove_reference<decltype(collisionInfo)>::type::collisionType == 0>([&](auto f){
-                            auto J = sharedInfo[collisionInfo.getShared()].getNormal().transpose()*a->getDPDQ(state, collisionInfo.getData(collisionInfo.collisionType));
+
+                            auto J = (sharedInfo[collisionInfo.getShared()].getNormal().transpose()*a->getDPDQ(state, collisionInfo.getData(collisionInfo.collisionType))).eval();
                             assign(assembler, J, std::array<ConstraintIndex,1>{{indexInc}},a->getQDot(collisionInfo.getData(collisionInfo.collisionType)));
                         }).else_([&](auto f) {
-                            std::cout<<"getGradient: Shouldn't be here yet, no continous jacobian implemented \n";
-                            exit(1);
-                            //auto J = sharedInfo[collisionInfo.getShared()].getNormal().transpose()*a->getDVDQ(sharedInfo[collisionInfo.getShared()].getPosition(), collisionInfo.getData(collisionInfo.collisionType));
-                            //assign(assembler, J, std::array<ConstraintIndex,1>{{indexInc}},a->getQDot(collisionInfo.getData(collisionInfo.collisionType)));
+
+                            //something is wrong here
+                            auto J = (sharedInfo[collisionInfo.getShared()].getNormal().transpose()*a->getDPDQ(state, collisionInfo.getData(collisionInfo.collisionType),sharedInfo[collisionInfo.getShared()].getPosition())).eval();
+                            auto q = a->getQ(sharedInfo[collisionInfo.getShared()].getPosition(), collisionInfo.getData(collisionInfo.collisionType));
+                            //std::cout<<"Q SIZE: "<<q.size()<<"\n";
+                            assign(assembler, J, std::array<ConstraintIndex,1>{{indexInc}},q);
                         });
 
 
                     });
-
+
+                    //need another loop of objectB
                     indexInc.offsetGlobalId(1);
 
                 });
 
+                indexInc = index;
+                indexInc.setNumRows(1);
+
+                //forEach loop for multivector
+                forEach(objBList, [&assembler, &sharedInfo, &indexInc,&world, &state](auto &collisionInfo) {
+                    apply(world.getSystemList(), collisionInfo.getObject(), [&assembler, &collisionInfo, &indexInc, &world, &sharedInfo, &state](auto &a) {
+
+                        //Build Jacobian differently for vertex collisions and in element collision events
+                        static_if<std::remove_reference<decltype(collisionInfo)>::type::collisionType == 0>([&](auto f){
+                            auto J = (-sharedInfo[collisionInfo.getShared()].getNormal().transpose()*a->getDPDQ(state, collisionInfo.getData(collisionInfo.collisionType))).eval();
+                            assign(assembler, J, std::array<ConstraintIndex,1>{{indexInc}},a->getQ(collisionInfo.getData(collisionInfo.collisionType)));
+                        }).else_([&](auto f) {
+
+                            //something is wrong here
+                            auto J = (-sharedInfo[collisionInfo.getShared()].getNormal().transpose()*a->getDPDQ(state, collisionInfo.getData(collisionInfo.collisionType),sharedInfo[collisionInfo.getShared()].getPosition())).eval();
+                            auto q = a->getQ(sharedInfo[collisionInfo.getShared()].getPosition(), collisionInfo.getData(collisionInfo.collisionType));
+                            //std::cout<<"Q SIZE: "<<q.size()<<"\n";
+                            assign(assembler, J, std::array<ConstraintIndex,1>{{indexInc}},q);
+                        });
+
+
+                    });
+
+                    //need another loop of objectB
+                    indexInc.offsetGlobalId(1);
+
+                });
+
                 //forLoop<IsParallel<Assembler>::value>(objAList, assembler, [&world, &index, &cd](auto &assemble, auto &constraint) {
 
                     //TODO add static if inside apply to handle different constraint types.

diff --git a/src/Collisions/include/CollisionsFCL.h b/src/Collisions/include/CollisionsFCL.h
@@ -11,8 +11,9 @@
 //GAUSS Stuff
 #include <GaussIncludes.h>
 #include <UtilitiesContact.h>
+#define MAX_CONTACTS 1000
 
-// Collision detector for triangle meshes using the flexibile collision library (https://github.com/flexible-collision-library/fcl)
+// Collision detector fo    r triangle meshes using the flexibile collision library (https://github.com/flexible-collision-library/fcl)
 namespace Gauss {
     namespace Collisions {
 
@@ -26,19 +27,10 @@ namespace Gauss {
             CollisionFCLImpl(World &world) {
 
                 //scan through the world and put all the object into FCL collision structures
-                std::vector< fcl::BVHModel<fcl::KDOP<DataType, 24> > *> &bvhList = m_bvhList;
-                std::vector< SystemIndex> &indexList = m_indexList;
+                //std::vector< fcl::BVHModel<fcl::OBBRSS<DataType> > *> &bvhList = m_bvhList;
+                //std::vector< SystemIndex> &indexList = m_indexList;
+                m_firstTime = true;
 
-                forEachIndex(world.get().getSystemList(), [&bvhList, &indexList](auto type, auto index, auto &a) {
-                    fcl::BVHModel<fcl::KDOP<DataType, 24> > *bvh = new fcl::BVHModel<fcl::KDOP<DataType, 24> >();
-
-                    bvh->beginModel();
-                    bvh->addSubModel(a->getGeometry().first, a->getGeometry().second);
-                    bvh->endModel();
-
-                    bvhList.push_back(bvh);
-                    indexList.push_back(SystemIndex(type, index));
-                });
 
             }
 
@@ -69,9 +61,11 @@ namespace Gauss {
             std::vector<SharedCollisionInfo<DataType>> m_sharedList; //normals and world space positions;
 
             //FCL collision data structures
-            std::vector< fcl::BVHModel<fcl::KDOP<DataType, 24> > * > m_bvhList;
+            std::vector< fcl::BVHModel<fcl::OBBRSS<DataType> > > m_bvhList;
             std::vector< SystemIndex > m_indexList; 
 
+            bool m_firstTime;
+
         private:
 
         };
@@ -90,38 +84,76 @@ void Gauss::Collisions::CollisionFCLImpl<DataType>::detectCollisions(World &worl
     //MultiVector<Gauss::Collisions::ObjectCollisionInfo<DataType, 0> > &objBList = m_objBList;
     //std::vector<Gauss::Collisions::SharedCollisionInfo<DataType> > &sharedList = m_sharedList;
 
-    //update bvhs
-    std::vector< fcl::BVHModel<fcl::KDOP<DataType, 24> > *> &bvhList = m_bvhList;
+    std::vector< fcl::BVHModel<fcl::OBBRSS<DataType> > > &bvhList = m_bvhList;
+    std::vector< SystemIndex > &indexList = m_indexList;
 
-    unsigned int bvhIndex = 0;
+    if(m_firstTime) {
+
+        m_bvhList.clear();
+        m_indexList.clear();
+
+        forEachIndex(world.getSystemList(), [&world, &bvhList, &indexList](auto type, auto index, auto &a) {
+
+            Eigen::MatrixXx<DataType> positions;
+            positions.resize(a->getGeometry().first.rows(),3);
+
+            for(unsigned int ii=0; ii<a->getGeometry().first.rows(); ++ii) {
+                positions.row(ii) = a->getPosition(world.getState(),ii);
+            }
+
+            bvhList.push_back(fcl::BVHModel<fcl::OBBRSS<DataType> >());
+
+            bvhList[bvhList.size()-1].beginModel();
+            bvhList[bvhList.size()-1].addSubModel(positions, a->getGeometry().second);
+            bvhList[bvhList.size()-1].endModel();
+
+
+            //bvhList.push_back(bvh);
+            indexList.push_back(SystemIndex(type, index));
+        });
+
+        m_firstTime = false;
+    }
+
+    //update bvhs
+     unsigned int bvhIndex = 0;
     //fill in collision data using retrived indices and collision data
     forEachIndex(world.getSystemList(), [&bvhList, &bvhIndex, &world](auto type, auto index, auto &a) {
-        bvhList[bvhIndex]->beginReplaceModel();
+
+        bvhList[bvhIndex].beginUpdateModel();
         for(unsigned int ii=0; ii<a->getGeometry().first.rows(); ++ii) {
-            bvhList[bvhIndex]->replaceVertex(a->getPosition(world.getState(), ii));
+            bvhList[bvhIndex].updateVertex(a->getPosition(world.getState(), ii));
         }
-        bvhList[bvhIndex]->endReplaceModel(true, true);
+
+        bvhList[bvhIndex].endUpdateModel(true, true);
+
         bvhIndex++;
+
     });
 
-
     //n^2 collision detection
-    fcl::Transform3<double> pose = fcl::Transform3<double>::Identity();
-    fcl::CollisionRequest<double> request;
-    fcl::CollisionResult<double> result;
+    fcl::Transform3<DataType> pose0 = fcl::Transform3<DataType>::Identity();
+    fcl::Transform3<DataType> pose1 = fcl::Transform3<DataType>::Identity();
+    fcl::CollisionRequest<DataType> request(MAX_CONTACTS,true);
+    fcl::CollisionResult<DataType> result;
 
     for(unsigned int obj0=0; obj0<m_bvhList.size(); ++obj0) {
-        for(unsigned int obj1=obj0; obj1<m_bvhList.size(); ++obj1) {
+        for(unsigned int obj1=obj0+1; obj1<m_bvhList.size(); ++obj1) {
 
             //do collision detection
-            int numContacts = fcl::collide(m_bvhList[obj0],pose,m_bvhList[obj1], pose, request, result);
+            fcl::collide(&m_bvhList[obj0],pose0, &m_bvhList[obj1], pose1, request, result);
+
+            int numContacts = result.numContacts();
 
             for(unsigned int ii=0; ii<numContacts; ++ii) {
+
             //add each new contact to the contact list here
             //b1, b2 = collision primitive in obj1 and obj2
             //normal = contact normal
             //x = world space contact position
-                m_sharedList.push_back(SharedCollisionInfo<DataType>(result.getContact(ii).normal,result.getContact(ii).pos));
+
+                //std::cout<<"Position: \n"<<result.getContact(ii).pos<<", \nNormal: \n"<<result.getContact(ii).normal<<"\n";
+                m_sharedList.push_back(SharedCollisionInfo<DataType>(-result.getContact(ii).normal,result.getContact(ii).pos));
                 m_objAList.add(ObjectCollisionInfo<DataType,1>(m_sharedList.size()-1,m_indexList[obj0], result.getContact(ii).b1));
                 m_objBList.add(ObjectCollisionInfo<DataType,1>(m_sharedList.size()-1,m_indexList[obj1], result.getContact(ii).b2));
             }

diff --git a/src/Collisions/include/CollisionsFloor.h b/src/Collisions/include/CollisionsFloor.h
@@ -69,7 +69,7 @@ namespace Gauss {
                         //store collision
                         info.push_back(SharedCollisionInfo<DataType>(normal,obj->getPosition(state, iv)));
                         listA.add(ObjectCollisionInfo<DataType,0>(info.size()-1, SystemIndex(type,index), iv));
-                        listB.add(ObjectCollisionInfo<DataType,0>(info.size()-1, SystemIndex(-1,0), 0));
+                        //listB.add(ObjectCollisionInfo<DataType,0>(info.size()-1, SystemIndex(-1,0), 0));
 
                     }
                 }

diff --git a/src/Collisions/include/TimeStepperEulerImplicitLinearCollisions.h b/src/Collisions/include/TimeStepperEulerImplicitLinearCollisions.h
@@ -127,7 +127,7 @@ namespace Gauss {
         Eigen::VectorXd lx;
         Eigen::VectorXd ux;
         igl::active_set_params params;
-
+        params.max_iter = 0;
         Eigen::VectorXd qDotTmp = qDot;
         active_set(systemMatrix, (*forceVector), known, bKnown, Aeq, beq, Aineq, b, lx, ux, params, qDotTmp);
         qDot = qDotTmp;

diff --git a/src/Embeddings/include/Embedding.h b/src/Embeddings/include/Embedding.h
@@ -98,23 +98,63 @@ namespace Gauss {
 
 
             //per vertex accessors
-            inline decltype(auto) getPosition(const PhysicalSystemImpl &fem, const State<DataType> &state, unsigned int vertexId) const {
+            inline Eigen::Vector3x<DataType> getPosition(const PhysicalSystemImpl &fem, const State<DataType> &state, unsigned int vertexId) const {
                 //return m_V.row(vertexId).transpose() + m_N.block(3*vertexId, 0, 3, m_N.cols())*mapDOFEigen(fem.getQ(), state);
                 return m_V.row(vertexId).transpose() + m_N.block(3*vertexId, 0, 3, m_N.cols())*fem.getElement(m_elements[vertexId])->q(state);
             }
 
             //this is also surface J
-            inline decltype(auto) getDPDQ(const PhysicalSystemImpl &fem, const State<DataType> &state, unsigned int vertexId) {
+            inline Eigen::MatrixXx<DataType>  getDPDQ(const PhysicalSystemImpl &fem, const State<DataType> &state, unsigned int vertexId) const {
                 return m_N.block(3*vertexId, 0, 3, m_N.cols());
             }
 
-            inline void getVelocity(const PhysicalSystemImpl &fem, const State<DataType> &state, unsigned int vertexId) {
+            inline Eigen::Vector3x<DataType> getVelocity(const PhysicalSystemImpl &fem, const State<DataType> &state, unsigned int vertexId) {
                 return m_N.block(3*vertexId, 0, 3, m_N.cols())*fem.getElement(m_elements[vertexId])->qDot(state);
             }
 
             //per spatial point accessors (nothing implemented for these yet)
-
-
+            inline Eigen::MatrixXx<DataType> getDPDQ(const PhysicalSystemImpl &fem, const State<DataType> &state, unsigned int elementId, const Eigen::Vector3x<DataType> &x) const {
+
+                //Eigen::MatrixXx<DataType> J;
+                //J.resize(3, 3*m_N.cols());
+
+                //need barycnetric coords for contact point
+                Eigen::MatrixXx<DataType> J;
+                J.resize(3, 3*m_N.cols());
+
+                J.block(0,0, 3, m_N.cols()) = getDPDQ(fem, state, m_F(elementId, 0));
+                J.block(0,m_N.cols(), 3, m_N.cols()) = getDPDQ(fem, state, m_F(elementId, 1));
+                J.block(0,2*m_N.cols(), 3, m_N.cols()) = getDPDQ(fem, state, m_F(elementId, 2));
+
+                return J;
+
+           }
+
+           template<typename Vector>
+           inline decltype(auto) getQ(const PhysicalSystemImpl &fem, Vector &x, unsigned int elementId) const {
+
+               std::array<DOFBase<DataType> *, 3*NUM> q;
+               unsigned int kk=0;
+               for(unsigned int ii=0; ii< 3; ++ii) {
+                   auto e = fem.getElement(m_elements[m_F(elementId, ii)])->q();
+                   for(unsigned int jj=0 ;jj< e.size(); ++jj) {
+                       q[kk] = e[jj];
+                       kk++;
+                   }
+               }
+               return q;
+           }
+
+           inline decltype(auto) getQ(const PhysicalSystemImpl &fem, unsigned int vertexId) const {
+
+               return fem.getElement(m_elements[vertexId])->q();
+           }
+
+           inline decltype(auto) getQDot(const PhysicalSystemImpl &fem, unsigned int vertexId)  {
+
+               return fem.getElement(m_elements[vertexId])->qDot();
+           }
+
         protected:
 
            Eigen::MatrixXx<DataType> m_V;
@@ -172,6 +212,22 @@ namespace Gauss {
                 return m_embedding.getDVDQ(*this, state, params...);
             }
 
+            //Get function supporing point in space
+            template<typename Vector>
+            inline  decltype(auto) getQ(const Vector &x, unsigned int elementId) const {
+                return m_embedding.getQ(*this, x, elementId);
+            }
+
+            //Get function supporing vertex in space
+            inline decltype(auto) getQ(unsigned int elementId) const {
+                return m_embedding.getQ(*this, elementId);
+            }
+
+            //Get function supporing vertex in space
+            inline decltype(auto) getQDot(unsigned int elementId)  {
+
+                return m_embedding.getQDot(*this, elementId);
+            }
             inline decltype(auto) getGeometry() { return m_embedding.getGeometry(); }
 
         protected:

diff --git a/src/Examples/example17.cpp b/src/Examples/example17.cpp
@@ -58,7 +58,7 @@ int main(int argc, char **argv) {
     //Embeddings::EmbeddingFunction<double, FEMLinearTets> embeddingFunction;
     Embeddings::PhysicalSystemEmbeddedMesh<double, FEMLinearTets> *test = new Embeddings::PhysicalSystemEmbeddedMesh<double, FEMLinearTets>(Vs,Fs, V, F);
 
-    MyCollisionDetector cd(std::ref(world), Eigen::Vector3d(-0.2,1.0,0.0), Eigen::Vector3d(0.0,-5.0,0.0));
+    MyCollisionDetector cd(std::ref(world), Eigen::Vector3d(-1,1.0,0.0), Eigen::Vector3d(0.0, -5,0.0));
     world.addSystem(test);
     world.addInequalityConstraint(&cd);
     world.finalize();

diff --git a/src/Examples/exampleCollisionsFloor.cpp b/src/Examples/exampleCollisionsFloor.cpp
@@ -44,7 +44,7 @@ int main(int argc, char **argv) {
     readTetgen(V, F, dataDir()+"/meshesTetgen/Beam/Beam.node", dataDir()+"/meshesTetgen/Beam/Beam.ele");
 
     FEMLinearTets *test = new FEMLinearTets(V,F);
-    MyCollisionDetector cd(std::ref(world), Eigen::Vector3d(-0.2,1.0,0.0), Eigen::Vector3d(0.0,-5.0,0.0));
+    MyCollisionDetector cd(std::ref(world), Eigen::Vector3d(-0.2,1.0,0.0), Eigen::Vector3d(0.0, .0,0.0));
     world.addSystem(test);
     world.addInequalityConstraint(&cd);
     world.finalize();