Utils { public static double distance(double x1, double y1, double x2, double y2) { return Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)); }  static double getHeading(Quaternion quaternion); static double angleDifference(double angle1, double angle2); static double pointDirection(double x1, double y1, double x2, double y2); static double distance(double x1, double y1, double x2, double y2); static double distanceSquared(double x1, double y1, double x2, double y2); static double distanceToLine(double x, double y, Vector3 pt1, Vector3 pt2); static void drawPoint(GL10 gl, float x, float y, float size, int color); static void drawShape(GL10 gl, org.ros.android.view.visualization.Color color); static void drawPoints(GL10 gl, FloatBuffer vertices, float size, boolean fan); static String readText(Context context, int resId);  }
@Test public void testDistance() throws Exception { assertEquals(Utils.distance(-2.0, -1.0, -1.0, 0.0), 1.41, 0.1); }