| 4 | using namespace PyMesh; |
| 5 | |
| 6 | ZSparseMatrix RigidMotionAssembler::assemble(FESettingPtr setting) { |
| 7 | typedef FESetting::FEMeshPtr FEMeshPtr; |
| 8 | |
| 9 | typedef Eigen::Triplet<Float> T; |
| 10 | std::vector<T> entries; |
| 11 | |
| 12 | FEMeshPtr mesh = setting->get_mesh(); |
| 13 | |
| 14 | const size_t dim = mesh->getDim(); |
| 15 | const size_t num_nodes = mesh->getNbrNodes(); |
| 16 | |
| 17 | // Translation part |
| 18 | for (size_t i=0; i<num_nodes; i++) { |
| 19 | for (size_t j=0; j<dim; j++) { |
| 20 | entries.push_back(T(j,i*dim+j, 1.0)); |
| 21 | } |
| 22 | } |
| 23 | |
| 24 | // Rotation part |
| 25 | size_t rot_degrees = 0; |
| 26 | if (dim == 2) { |
| 27 | rot_degrees = 1; |
| 28 | for (size_t i=0; i<num_nodes; i++) { |
| 29 | VectorF x = mesh->getNode(i); |
| 30 | entries.push_back(T(dim , i*dim , -x[1])); |
| 31 | entries.push_back(T(dim , i*dim+1, x[0])); |
| 32 | } |
| 33 | } else { |
| 34 | rot_degrees = 3; |
| 35 | for (size_t i=0; i<num_nodes; i++) { |
| 36 | VectorF x = mesh->getNode(i); |
| 37 | entries.push_back(T(dim , i*dim+1, -x[2])); |
| 38 | entries.push_back(T(dim , i*dim+2, x[1])); |
| 39 | entries.push_back(T(dim+1, i*dim , x[2])); |
| 40 | entries.push_back(T(dim+1, i*dim+2, -x[0])); |
| 41 | entries.push_back(T(dim+2, i*dim , -x[1])); |
| 42 | entries.push_back(T(dim+2, i*dim+1, x[0])); |
| 43 | } |
| 44 | } |
| 45 | |
| 46 | ZSparseMatrix Ru(dim + rot_degrees, dim * num_nodes); |
| 47 | Ru.setFromTriplets(entries.begin(), entries.end()); |
| 48 | return Ru; |
| 49 | } |