diff --git a/src/simulation/fem/fem_2d_mesh.cc b/src/simulation/fem/fem_2d_mesh.cc
index 2e2218d..f046028 100644
--- a/src/simulation/fem/fem_2d_mesh.cc
+++ b/src/simulation/fem/fem_2d_mesh.cc
@@ -22,6 +22,33 @@ FEM2DMesh::FEM2DMesh()
     fixedIndicator_.getMaterials()[0].setColor(vec3(1.0f, 0.0f, 0.0f), true);
 }
 
+bool FEM2DMesh::is_well_defined()
+{
+  bool oneFixed = false;
+  vec3 fixedpos;
+  for (auto& p : elems0d_)
+    if (p.flags == FIXED)
+    {
+      oneFixed = true;
+      fixedpos = p.coord;
+      break;
+    }
+  if (!oneFixed)
+    return false;
+  vec3 dir = vec3(0);
+  for (auto& p : elems0d_)
+  {
+    if (p.flags == NONE)
+      continue;
+    if (dir.x == 0 && dir.y == 0)
+      dir = fixedpos - p.coord;
+    else if(glm::length(cross(dir, fixedpos - p.coord)) <= 0.01f)
+      continue;
+    return true;
+  }
+  return false;
+}
+
 int find_or_append(vec3 e, std::vector<FEMPoint> &v) {
     for (unsigned int i = 0; i < v.size(); i++) {
         if (v[i].coord == e)
diff --git a/src/simulation/fem/fem_2d_mesh.hh b/src/simulation/fem/fem_2d_mesh.hh
index f56869d..4495d97 100644
--- a/src/simulation/fem/fem_2d_mesh.hh
+++ b/src/simulation/fem/fem_2d_mesh.hh
@@ -31,6 +31,7 @@ class FEM2DMesh {
     bool setMode(vec3 pt, FEMFlag flag);
     FEMPoint *getPoint(vec3 pt);
     void resetForces();
+    bool is_well_defined();
 
   private:
     void updateFlags();
diff --git a/src/simulation/fem/fem_computation.cc b/src/simulation/fem/fem_computation.cc
index d1fa9dc..2b1d5d5 100644
--- a/src/simulation/fem/fem_computation.cc
+++ b/src/simulation/fem/fem_computation.cc
@@ -60,7 +60,7 @@ void gaussianElimination(float *A, float *B, int n) { // return value is in B
 }
 
 float calculate_theta(const glm::vec2 &node1, const glm::vec2 &node2) {
-    glm::vec2 diff = node2 - node1;
+    glm::vec2 diff = node1 - node2;
     return atan2(diff.y, diff.x); // Calculate angle in radians
 }
 
@@ -72,10 +72,10 @@ void build_rotation_matrix(float theta, glm::mat4 &rotation_matrix) {
     float C = cos(theta);
     float S = sin(theta);
 
-    rotation_matrix[0] = glm::vec4(C * C, C * S, -C * C, -C * S);
-    rotation_matrix[1] = glm::vec4(C * S, S * S, -C * S, -S * S);
-    rotation_matrix[2] = glm::vec4(-C * C, -C * S, C * C, C * S);
-    rotation_matrix[3] = glm::vec4(-C * S, -S * S, C * S, S * S);
+    rotation_matrix[0] = glm::vec4(C, S, 0, 0);
+    rotation_matrix[1] = glm::vec4(-S, C, 0, 0);
+    rotation_matrix[2] = glm::vec4(0, 0, C, S);
+    rotation_matrix[3] = glm::vec4(0, 0, -S, C);
 }
 
 void calculate_global_stiffness_beam(const glm::vec2 &node1,
@@ -91,7 +91,9 @@ void calculate_global_stiffness_beam(const glm::vec2 &node1,
     std::cout << "theta = " << theta << std::endl;
     glm::mat4 rotation_matrix;
     build_rotation_matrix(theta, rotation_matrix);
+    print_matrix(rotation_matrix);
     glm::mat4 temp_k = local_k_init * rotation_matrix;
+    print_matrix(temp_k);
     global_k = glm::transpose(rotation_matrix) * temp_k;
     print_matrix(global_k);
 }
@@ -131,25 +133,19 @@ void compute_displacement(std::vector<FEMPoint> &points,
     for (auto &p : points) {
         /* std::cout << p.coord.z << " " << p.coord.y << " " << p.flags */
         /*           << std::endl; */
+        known_forces[2 * i] = p.forceApplied.x;
+        known_forces[2 * i + 1] = p.forceApplied.y;
         switch (p.flags) {
         case NONE:
-            std::cout << "set " << (2 * i + 1) << " and " << (2 * i)
-                      << std::endl;
-            known_forces[2 * i + 1] = p.forceApplied.y;
-            known_forces[2 * i] = p.forceApplied.z;
             ids_knowns.push_back(2 * i);
             ids_knowns.push_back(2 * i + 1);
             break;
         case FIXED:
             break;
         case ROLLING_X:
-            /* std::cout << "set " << (2 * i) << std::endl; */
-            known_forces[2 * i] = 0;
             ids_knowns.push_back(2 * i);
             break;
         case ROLLING_Y:
-            /* std::cout << "set " << (2 * i + 1) << std::endl; */
-            known_forces[2 * i + 1] = 0;
             ids_knowns.push_back(2 * i + 1);
             break;
         }