@Test public void testDistanceToLine() throws Exception { assertEquals(Utils.distanceToLine(0.0, 0.0, new Vector3(1.0, 1.0, 0.0), new Vector3(-1.0, 1.0, 0.0)), 1.0, 0.01); }
public static double distanceToLine(double x, double y, Vector3 pt1, Vector3 pt2) { final double A, B, C, D; A = pt1.getX(); B = pt2.getX(); C = pt1.getY(); D = pt2.getY(); double t = (A * (A - B) + C * (C - D) + x * (B - A) + y * (D - C)) / ((A - B) * (A - B) + (C - D) * (C - D)); if (t < 0.0 || t > 1.0) return Double.MAX_VALUE; double c1 = B - A; double c2 = D - C; c1 = A + t * c1; c2 = C + t * c2; return distance(x, y, c1, c2); }
Utils { public static double distanceToLine(double x, double y, Vector3 pt1, Vector3 pt2) { final double A, B, C, D; A = pt1.getX(); B = pt2.getX(); C = pt1.getY(); D = pt2.getY(); double t = (A * (A - B) + C * (C - D) + x * (B - A) + y * (D - C)) / ((A - B) * (A - B) + (C - D) * (C - D)); if (t < 0.0 || t > 1.0) return Double.MAX_VALUE; double c1 = B - A; double c2 = D - C; c1 = A + t * c1; c2 = C + t * c2; return distance(x, y, c1, c2); } }
Utils { public static double distanceToLine(double x, double y, Vector3 pt1, Vector3 pt2) { final double A, B, C, D; A = pt1.getX(); B = pt2.getX(); C = pt1.getY(); D = pt2.getY(); double t = (A * (A - B) + C * (C - D) + x * (B - A) + y * (D - C)) / ((A - B) * (A - B) + (C - D) * (C - D)); if (t < 0.0 || t > 1.0) return Double.MAX_VALUE; double c1 = B - A; double c2 = D - C; c1 = A + t * c1; c2 = C + t * c2; return distance(x, y, c1, c2); }  }
Utils { public static double distanceToLine(double x, double y, Vector3 pt1, Vector3 pt2) { final double A, B, C, D; A = pt1.getX(); B = pt2.getX(); C = pt1.getY(); D = pt2.getY(); double t = (A * (A - B) + C * (C - D) + x * (B - A) + y * (D - C)) / ((A - B) * (A - B) + (C - D) * (C - D)); if (t < 0.0 || t > 1.0) return Double.MAX_VALUE; double c1 = B - A; double c2 = D - C; c1 = A + t * c1; c2 = C + t * c2; return distance(x, y, c1, c2); }  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); }
Utils { public static double distanceToLine(double x, double y, Vector3 pt1, Vector3 pt2) { final double A, B, C, D; A = pt1.getX(); B = pt2.getX(); C = pt1.getY(); D = pt2.getY(); double t = (A * (A - B) + C * (C - D) + x * (B - A) + y * (D - C)) / ((A - B) * (A - B) + (C - D) * (C - D)); if (t < 0.0 || t > 1.0) return Double.MAX_VALUE; double c1 = B - A; double c2 = D - C; c1 = A + t * c1; c2 = C + t * c2; return distance(x, y, c1, c2); }  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);  }