private static double calculateAngle(double x1, double y1,
                                        double x2, double y2) {
      double dx = x2 - x1;
      double dy = y2 - y1;
      double angle = 0.0; // Horisontal to the right

      if (dx == 0.0) { // Vertical
         if (dy == 0.0) { // No angle
            angle = 0.0;
         }
         else if (dy > 0.0) { // Points vertical up
            angle = Math.PI/2.0;
         }
         else {// dy < 0, Points down
            angle = Math.PI/2.0*3.0;
         }
      }
      else if (dy == 0.0) { // Horisontal
         if (dx > 0.0) { // Points right
            angle = 0.0;
         }
         else { // dx < 0.0 Points left
            angle = Math.PI;
         }
      }
      else {
         if (dx < 0.0) { // Points (up and left) or (down and left)
            angle = Math.atan(dy/dx) + Math.PI;
         }
         else if (dy < 0.0) { // Points down and right
            angle = Math.atan(dy/dx) + 2 * Math.PI;
         }
         else {
            angle = Math.atan(dy/dx);
         }
      }
      return angle;
   }