I've got a program that allows users to draw curves. But these curves don't look nice - they look wobbly and hand-drawn.
So I want an algorithm that will automatically smooth them. I know there are inherent ambiguities in the smoothing process, so it won't be perfect every time, but such algorithms do seem to exist in several drawing packages and they work quite well.
Are there any code samples for something like this? C# would be perfect, but I can translate from other languages.
You can reduce the number of points using the Ramer–Douglas–Peucker algorithm there is a C# implementation here. I gave this a try using WPFs PolyQuadraticBezierSegment and it showed a small amount of improvement depending on the tolerance.
After a bit of searching sources (1,2) seem to indicate that using the curve fitting algorithm from Graphic Gems by Philip J Schneider works well, the C code is available. Geometric Tools also has some resources that could be worth investigating.
This is a rough sample I made, there are still some glitches but it works well a lot of the time. Here is the quick and dirty C# port of FitCurves.c. One of the issues is that if you don't reduce the original points the calculated error is 0 and it terminates early the sample uses the point reduction algorithm beforehand.
/*
An Algorithm for Automatically Fitting Digitized Curves
by Philip J. Schneider
from "Graphics Gems", Academic Press, 1990
*/
public static class FitCurves
{
/* Fit the Bezier curves */
private const int MAXPOINTS = 10000;
public static List<Point> FitCurve(Point[] d, double error)
{
Vector tHat1, tHat2; /* Unit tangent vectors at endpoints */
tHat1 = ComputeLeftTangent(d, 0);
tHat2 = ComputeRightTangent(d, d.Length - 1);
List<Point> result = new List<Point>();
FitCubic(d, 0, d.Length - 1, tHat1, tHat2, error,result);
return result;
}
private static void FitCubic(Point[] d, int first, int last, Vector tHat1, Vector tHat2, double error,List<Point> result)
{
Point[] bezCurve; /*Control points of fitted Bezier curve*/
double[] u; /* Parameter values for point */
double[] uPrime; /* Improved parameter values */
double maxError; /* Maximum fitting error */
int splitPoint; /* Point to split point set at */
int nPts; /* Number of points in subset */
double iterationError; /*Error below which you try iterating */
int maxIterations = 4; /* Max times to try iterating */
Vector tHatCenter; /* Unit tangent vector at splitPoint */
int i;
iterationError = error * error;
nPts = last - first + 1;
/* Use heuristic if region only has two points in it */
if(nPts == 2)
{
double dist = (d[first]-d[last]).Length / 3.0;
bezCurve = new Point[4];
bezCurve[0] = d[first];
bezCurve[3] = d[last];
bezCurve[1] = (tHat1 * dist) + bezCurve[0];
bezCurve[2] = (tHat2 * dist) + bezCurve[3];
result.Add(bezCurve[1]);
result.Add(bezCurve[2]);
result.Add(bezCurve[3]);
return;
}
/* Parameterize points, and attempt to fit curve */
u = ChordLengthParameterize(d, first, last);
bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2);
/* Find max deviation of points to fitted curve */
maxError = ComputeMaxError(d, first, last, bezCurve, u,out splitPoint);
if(maxError < error)
{
result.Add(bezCurve[1]);
result.Add(bezCurve[2]);
result.Add(bezCurve[3]);
return;
}
/* If error not too large, try some reparameterization */
/* and iteration */
if(maxError < iterationError)
{
for(i = 0; i < maxIterations; i++)
{
uPrime = Reparameterize(d, first, last, u, bezCurve);
bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
maxError = ComputeMaxError(d, first, last,
bezCurve, uPrime,out splitPoint);
if(maxError < error)
{
result.Add(bezCurve[1]);
result.Add(bezCurve[2]);
result.Add(bezCurve[3]);
return;
}
u = uPrime;
}
}
/* Fitting failed -- split at max error point and fit recursively */
tHatCenter = ComputeCenterTangent(d, splitPoint);
FitCubic(d, first, splitPoint, tHat1, tHatCenter, error,result);
tHatCenter.Negate();
FitCubic(d, splitPoint, last, tHatCenter, tHat2, error,result);
}
static Point[] GenerateBezier(Point[] d, int first, int last, double[] uPrime, Vector tHat1, Vector tHat2)
{
int i;
Vector[,] A = new Vector[MAXPOINTS,2];/* Precomputed rhs for eqn */
int nPts; /* Number of pts in sub-curve */
double[,] C = new double[2,2]; /* Matrix C */
double[] X = new double[2]; /* Matrix X */
double det_C0_C1, /* Determinants of matrices */
det_C0_X,
det_X_C1;
double alpha_l, /* Alpha values, left and right */
alpha_r;
Vector tmp; /* Utility variable */
Point[] bezCurve = new Point[4]; /* RETURN bezier curve ctl pts */
nPts = last - first + 1;
/* Compute the A's */
for (i = 0; i < nPts; i++) {
Vector v1, v2;
v1 = tHat1;
v2 = tHat2;
v1 *= B1(uPrime[i]);
v2 *= B2(uPrime[i]);
A[i,0] = v1;
A[i,1] = v2;
}
/* Create the C and X matrices */
C[0,0] = 0.0;
C[0,1] = 0.0;
C[1,0] = 0.0;
C[1,1] = 0.0;
X[0] = 0.0;
X[1] = 0.0;
for (i = 0; i < nPts; i++) {
C[0,0] += V2Dot(A[i,0], A[i,0]);
C[0,1] += V2Dot(A[i,0], A[i,1]);
/* C[1][0] += V2Dot(&A[i][0], &A[i][9]);*/
C[1,0] = C[0,1];
C[1,1] += V2Dot(A[i,1], A[i,1]);
tmp = ((Vector)d[first + i] -
(
((Vector)d[first] * B0(uPrime[i])) +
(
((Vector)d[first] * B1(uPrime[i])) +
(
((Vector)d[last] * B2(uPrime[i])) +
((Vector)d[last] * B3(uPrime[i]))))));
X[0] += V2Dot(A[i,0], tmp);
X[1] += V2Dot(A[i,1], tmp);
}
/* Compute the determinants of C and X */
det_C0_C1 = C[0,0] * C[1,1] - C[1,0] * C[0,1];
det_C0_X = C[0,0] * X[1] - C[1,0] * X[0];
det_X_C1 = X[0] * C[1,1] - X[1] * C[0,1];
/* Finally, derive alpha values */
alpha_l = (det_C0_C1 == 0) ? 0.0 : det_X_C1 / det_C0_C1;
alpha_r = (det_C0_C1 == 0) ? 0.0 : det_C0_X / det_C0_C1;
/* If alpha negative, use the Wu/Barsky heuristic (see text) */
/* (if alpha is 0, you get coincident control points that lead to
* divide by zero in any subsequent NewtonRaphsonRootFind() call. */
double segLength = (d[first] - d[last]).Length;
double epsilon = 1.0e-6 * segLength;
if (alpha_l < epsilon || alpha_r < epsilon)
{
/* fall back on standard (probably inaccurate) formula, and subdivide further if needed. */
double dist = segLength / 3.0;
bezCurve[0] = d[first];
bezCurve[3] = d[last];
bezCurve[1] = (tHat1 * dist) + bezCurve[0];
bezCurve[2] = (tHat2 * dist) + bezCurve[3];
return (bezCurve);
}
/* First and last control points of the Bezier curve are */
/* positioned exactly at the first and last data points */
/* Control points 1 and 2 are positioned an alpha distance out */
/* on the tangent vectors, left and right, respectively */
bezCurve[0] = d[first];
bezCurve[3] = d[last];
bezCurve[1] = (tHat1 * alpha_l) + bezCurve[0];
bezCurve[2] = (tHat2 * alpha_r) + bezCurve[3];
return (bezCurve);
}
/*
* Reparameterize:
* Given set of points and their parameterization, try to find
* a better parameterization.
*
*/
static double[] Reparameterize(Point[] d,int first,int last,double[] u,Point[] bezCurve)
{
int nPts = last-first+1;
int i;
double[] uPrime = new double[nPts]; /* New parameter values */
for (i = first; i <= last; i++) {
uPrime[i-first] = NewtonRaphsonRootFind(bezCurve, d[i], u[i-first]);
}
return uPrime;
}
/*
* NewtonRaphsonRootFind :
* Use Newton-Raphson iteration to find better root.
*/
static double NewtonRaphsonRootFind(Point[] Q,Point P,double u)
{
double numerator, denominator;
Point[] Q1 = new Point[3], Q2 = new Point[2]; /* Q' and Q'' */
Point Q_u, Q1_u, Q2_u; /*u evaluated at Q, Q', & Q'' */
double uPrime; /* Improved u */
int i;
/* Compute Q(u) */
Q_u = BezierII(3, Q, u);
/* Generate control vertices for Q' */
for (i = 0; i <= 2; i++) {
Q1[i].X = (Q[i+1].X - Q[i].X) * 3.0;
Q1[i].Y = (Q[i+1].Y - Q[i].Y) * 3.0;
}
/* Generate control vertices for Q'' */
for (i = 0; i <= 1; i++) {
Q2[i].X = (Q1[i+1].X - Q1[i].X) * 2.0;
Q2[i].Y = (Q1[i+1].Y - Q1[i].Y) * 2.0;
}
/* Compute Q'(u) and Q''(u) */
Q1_u = BezierII(2, Q1, u);
Q2_u = BezierII(1, Q2, u);
/* Compute f(u)/f'(u) */
numerator = (Q_u.X - P.X) * (Q1_u.X) + (Q_u.Y - P.Y) * (Q1_u.Y);
denominator = (Q1_u.X) * (Q1_u.X) + (Q1_u.Y) * (Q1_u.Y) +
(Q_u.X - P.X) * (Q2_u.X) + (Q_u.Y - P.Y) * (Q2_u.Y);
if (denominator == 0.0f) return u;
/* u = u - f(u)/f'(u) */
uPrime = u - (numerator/denominator);
return (uPrime);
}
/*
* Bezier :
* Evaluate a Bezier curve at a particular parameter value
*
*/
static Point BezierII(int degree,Point[] V,double t)
{
int i, j;
Point Q; /* Point on curve at parameter t */
Point[] Vtemp; /* Local copy of control points */
/* Copy array */
Vtemp = new Point[degree+1];
for (i = 0; i <= degree; i++) {
Vtemp[i] = V[i];
}
/* Triangle computation */
for (i = 1; i <= degree; i++) {
for (j = 0; j <= degree-i; j++) {
Vtemp[j].X = (1.0 - t) * Vtemp[j].X + t * Vtemp[j+1].X;
Vtemp[j].Y = (1.0 - t) * Vtemp[j].Y + t * Vtemp[j+1].Y;
}
}
Q = Vtemp[0];
return Q;
}
/*
* B0, B1, B2, B3 :
* Bezier multipliers
*/
static double B0(double u)
{
double tmp = 1.0 - u;
return (tmp * tmp * tmp);
}
static double B1(double u)
{
double tmp = 1.0 - u;
return (3 * u * (tmp * tmp));
}
static double B2(double u)
{
double tmp = 1.0 - u;
return (3 * u * u * tmp);
}
static double B3(double u)
{
return (u * u * u);
}
/*
* ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
*Approximate unit tangents at endpoints and "center" of digitized curve
*/
static Vector ComputeLeftTangent(Point[] d,int end)
{
Vector tHat1;
tHat1 = d[end+1]- d[end];
tHat1.Normalize();
return tHat1;
}
static Vector ComputeRightTangent(Point[] d,int end)
{
Vector tHat2;
tHat2 = d[end-1] - d[end];
tHat2.Normalize();
return tHat2;
}
static Vector ComputeCenterTangent(Point[] d,int center)
{
Vector V1, V2, tHatCenter = new Vector();
V1 = d[center-1] - d[center];
V2 = d[center] - d[center+1];
tHatCenter.X = (V1.X + V2.X)/2.0;
tHatCenter.Y = (V1.Y + V2.Y)/2.0;
tHatCenter.Normalize();
return tHatCenter;
}
/*
* ChordLengthParameterize :
* Assign parameter values to digitized points
* using relative distances between points.
*/
static double[] ChordLengthParameterize(Point[] d,int first,int last)
{
int i;
double[] u = new double[last-first+1]; /* Parameterization */
u[0] = 0.0;
for (i = first+1; i <= last; i++) {
u[i-first] = u[i-first-1] + (d[i-1] - d[i]).Length;
}
for (i = first + 1; i <= last; i++) {
u[i-first] = u[i-first] / u[last-first];
}
return u;
}
/*
* ComputeMaxError :
* Find the maximum squared distance of digitized points
* to fitted curve.
*/
static double ComputeMaxError(Point[] d,int first,int last,Point[] bezCurve,double[] u,out int splitPoint)
{
int i;
double maxDist; /* Maximum error */
double dist; /* Current error */
Point P; /* Point on curve */
Vector v; /* Vector from point to curve */
splitPoint = (last - first + 1)/2;
maxDist = 0.0;
for (i = first + 1; i < last; i++) {
P = BezierII(3, bezCurve, u[i-first]);
v = P - d[i];
dist = v.LengthSquared;
if (dist >= maxDist) {
maxDist = dist;
splitPoint = i;
}
}
return maxDist;
}
private static double V2Dot(Vector a,Vector b)
{
return((a.X*b.X)+(a.Y*b.Y));
}
}
Kris's answer is a very good port of the original to C#, but the performance isn't ideal and there are some places where floating point instability can cause some issues and return NaN values (this is true in the original code too). I created a library that contains my own port of it as well as Ramer-Douglas-Peuker, and should work not only with WPF points, but the new SIMD-enabled vector types and Unity 3D also:
https://github.com/burningmime/curves
Maybe this WPF+Bezier-based article is a good start: Draw a Smooth Curve through a Set of 2D Points with Bezier Primitives
Well, the job of Kris was very useful.
I realized that the problem he pointed out about the algorithm terminating earlier because of a miscalculated error ending on 0, is due to the fact that one point is repeated and the computed tangent is infinite.
I have done a translation to Java, based on the code of Kris, it works fine I believe:
EDIT:
I still working and trying to get a better behavior on the algorithm. I realized that on very spiky angles, the Bezier curves simply don't behave good. So I tried to combine Bezier curves with Lines and this is the result:
import java.awt.Point;
import java.awt.Shape;
import java.awt.geom.CubicCurve2D;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.util.LinkedList;
import java.util.List;
import java.util.concurrent.atomic.AtomicInteger;
import javax.vecmath.Point2d;
import javax.vecmath.Tuple2d;
import javax.vecmath.Vector2d;
/*
An Algorithm for Automatically Fitting Digitized Curves
by Philip J. Schneider
from "Graphics Gems", Academic Press, 1990
*/
public class FitCurves
{
/* Fit the Bezier curves */
private final static int MAXPOINTS = 10000;
private final static double epsilon = 1.0e-6;
/**
* Rubén:
* This is the sensitivity. When it is 1, it will create a line if it is at least as long as the
* distance from the previous control point.
* When it is greater, it will create less lines, and when it is lower, more lines.
* This is based on the previous control point since I believe it is a good indicator of the curvature
* where it is coming from, and we don't want long and second derived constant curves to be modeled with
* many lines.
*/
private static final double lineSensitivity=0.75;
public interface ResultCurve {
public Point2D getStart();
public Point2D getEnd();
public Shape getShape();
}
public static class BezierCurve implements ResultCurve {
public Point start;
public Point end;
public Point ctrl1;
public Point ctrl2;
public BezierCurve(Point2D start, Point2D ctrl1, Point2D ctrl2, Point2D end) {
this.start=new Point((int)Math.round(start.getX()), (int)Math.round(start.getY()));
this.end=new Point((int)Math.round(end.getX()), (int)Math.round(end.getY()));
this.ctrl1=new Point((int)Math.round(ctrl1.getX()), (int)Math.round(ctrl1.getY()));
this.ctrl2=new Point((int)Math.round(ctrl2.getX()), (int)Math.round(ctrl2.getY()));
if(this.ctrl1.x<=1 || this.ctrl1.y<=1) {
throw new IllegalStateException("ctrl1 invalid");
}
if(this.ctrl2.x<=1 || this.ctrl2.y<=1) {
throw new IllegalStateException("ctrl2 invalid");
}
}
public Shape getShape() {
return new CubicCurve2D.Float(start.x, start.y, ctrl1.x, ctrl1.y, ctrl2.x, ctrl2.y, end.x, end.y);
}
public Point getStart() {
return start;
}
public Point getEnd() {
return end;
}
}
public static class CurveSegment implements ResultCurve {
Point2D start;
Point2D end;
public CurveSegment(Point2D startP, Point2D endP) {
this.start=startP;
this.end=endP;
}
public Shape getShape() {
return new Line2D.Float(start, end);
}
public Point2D getStart() {
return start;
}
public Point2D getEnd() {
return end;
}
}
public static List<ResultCurve> FitCurve(double[][] points, double error) {
Point[] allPoints=new Point[points.length];
for(int i=0; i < points.length; i++) {
allPoints[i]=new Point((int) Math.round(points[i][0]), (int) Math.round(points[i][1]));
}
return FitCurve(allPoints, error);
}
public static List<ResultCurve> FitCurve(Point[] d, double error)
{
Vector2d tHat1, tHat2; /* Unit tangent vectors at endpoints */
int first=0;
int last=d.length-1;
tHat1 = ComputeLeftTangent(d, first);
tHat2 = ComputeRightTangent(d, last);
List<ResultCurve> result = new LinkedList<ResultCurve>();
FitCubic(d, first, last, tHat1, tHat2, error, result);
return result;
}
private static void FitCubic(Point[] d, int first, int last, Vector2d tHat1, Vector2d tHat2, double error, List<ResultCurve> result)
{
PointE[] bezCurve; /*Control points of fitted Bezier curve*/
double[] u; /* Parameter values for point */
double[] uPrime; /* Improved parameter values */
double maxError; /* Maximum fitting error */
int nPts; /* Number of points in subset */
double iterationError; /*Error below which you try iterating */
int maxIterations = 4; /* Max times to try iterating */
Vector2d tHatCenter; /* Unit tangent vector at splitPoint */
int i;
double errorOnLine=error;
iterationError = error * error;
nPts = last - first + 1;
AtomicInteger outputSplitPoint=new AtomicInteger();
/**
* Rubén: Here we try to fit the form with a line, and we mark the split point if we find any line with a minimum length
*/
/*
* the minimum distance for a length (so we don't create a very small line, when it could be slightly modeled with the previous Bezier,
* will be proportional to the distance of the previous control point of the last Bezier.
*/
BezierCurve res=null;
for(i=result.size()-1; i >0; i--) {
ResultCurve thisCurve=result.get(i);
if(thisCurve instanceof BezierCurve) {
res=(BezierCurve)thisCurve;
break;
}
}
Line seg=new Line(d[first], d[last]);
int nAcceptableTogether=0;
int startPoint=-1;
int splitPointTmp=-1;
if(Double.isInfinite(seg.getGradient())) {
for (i = first; i <= last; i++) {
double dist=Math.abs(d[i].x-d[first].x);
if(dist<errorOnLine) {
nAcceptableTogether++;
if(startPoint==-1) startPoint=i;
} else {
if(startPoint!=-1) {
double minLineLength=Double.POSITIVE_INFINITY;
if(res!=null) {
minLineLength=lineSensitivity * res.ctrl2.distance(d[startPoint]);
}
double thisFromStart=d[startPoint].distance(d[i]);
if(thisFromStart >= minLineLength) {
splitPointTmp=i;
startPoint=-1;
break;
}
}
nAcceptableTogether=0;
startPoint=-1;
}
}
} else {
//looking for the max squared error
for (i = first; i <= last; i++) {
Point thisPoint=d[i];
Point2D calculatedP=seg.getByX(thisPoint.getX());
double dist=thisPoint.distance(calculatedP);
if(dist<errorOnLine) {
nAcceptableTogether++;
if(startPoint==-1) startPoint=i;
} else {
if(startPoint!=-1) {
double thisFromStart=d[startPoint].distance(thisPoint);
double minLineLength=Double.POSITIVE_INFINITY;
if(res!=null) {
minLineLength=lineSensitivity * res.ctrl2.distance(d[startPoint]);
}
if(thisFromStart >= minLineLength) {
splitPointTmp=i;
startPoint=-1;
break;
}
}
nAcceptableTogether=0;
startPoint=-1;
}
}
}
if(startPoint!=-1) {
double minLineLength=Double.POSITIVE_INFINITY;
if(res!=null) {
minLineLength=lineSensitivity * res.ctrl2.distance(d[startPoint]);
}
if(d[startPoint].distance(d[last]) >= minLineLength) {
splitPointTmp=startPoint;
startPoint=-1;
} else {
nAcceptableTogether=0;
}
}
outputSplitPoint.set(splitPointTmp);
if(nAcceptableTogether==(last-first+1)) {
//This is a line!
System.out.println("line, length: " + d[first].distance(d[last]));
result.add(new CurveSegment(d[first], d[last]));
return;
}
/*********************** END of the Line approach, lets try the normal algorithm *******************************************/
if(splitPointTmp < 0) {
if(nPts == 2) {
double dist = d[first].distance(d[last]) / 3.0; //sqrt((last.x-first.x)^2 + (last.y-first.y)^2) / 3.0
bezCurve = new PointE[4];
bezCurve[0] = new PointE(d[first]);
bezCurve[3] = new PointE(d[last]);
bezCurve[1]=new PointE(tHat1).scaleAdd(dist, bezCurve[0]); //V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
bezCurve[2]=new PointE(tHat2).scaleAdd(dist, bezCurve[3]); //V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
result.add(new BezierCurve(bezCurve[0],bezCurve[1],bezCurve[2],bezCurve[3]));
return;
}
/* Parameterize points, and attempt to fit curve */
u = ChordLengthParameterize(d, first, last);
bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2);
/* Find max deviation of points to fitted curve */
maxError = ComputeMaxError(d, first, last, bezCurve, u, outputSplitPoint);
if(maxError < error) {
result.add(new BezierCurve(bezCurve[0],bezCurve[1],bezCurve[2],bezCurve[3]));
return;
}
/* If error not too large, try some reparameterization */
/* and iteration */
if(maxError < iterationError)
{
for(i = 0; i < maxIterations; i++) {
uPrime = Reparameterize(d, first, last, u, bezCurve);
bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
maxError = ComputeMaxError(d, first, last, bezCurve, uPrime, outputSplitPoint);
if(maxError < error) {
result.add(new BezierCurve(bezCurve[0],bezCurve[1],bezCurve[2],bezCurve[3]));
return;
}
u = uPrime;
}
}
}
/* Fitting failed -- split at max error point and fit recursively */
tHatCenter = ComputeCenterTangent(d, outputSplitPoint.get());
FitCubic(d, first, outputSplitPoint.get(), tHat1, tHatCenter, error,result);
tHatCenter.negate();
FitCubic(d, outputSplitPoint.get(), last, tHatCenter, tHat2, error,result);
}
//Checked!!
static PointE[] GenerateBezier(Point2D[] d, int first, int last, double[] uPrime, Vector2d tHat1, Vector2d tHat2)
{
int i;
Vector2d[][] A = new Vector2d[MAXPOINTS][2]; /* Precomputed rhs for eqn */
int nPts; /* Number of pts in sub-curve */
double[][] C = new double[2][2]; /* Matrix C */
double[] X = new double[2]; /* Matrix X */
double det_C0_C1, /* Determinants of matrices */
det_C0_X,
det_X_C1;
double alpha_l, /* Alpha values, left and right */
alpha_r;
PointE[] bezCurve = new PointE[4]; /* RETURN bezier curve ctl pts */
nPts = last - first + 1;
/* Compute the A's */
for (i = 0; i < nPts; i++) {
Vector2d v1=new Vector2d(tHat1);
Vector2d v2=new Vector2d(tHat2);
v1.scale(B1(uPrime[i]));
v2.scale(B2(uPrime[i]));
A[i][0] = v1;
A[i][1] = v2;
}
/* Create the C and X matrices */
C[0][0] = 0.0;
C[0][1] = 0.0;
C[1][0] = 0.0;
C[1][1] = 0.0;
X[0] = 0.0;
X[1] = 0.0;
for (i = 0; i < nPts; i++) {
C[0][0] += A[i][0].dot(A[i][0]); //C[0][0] += V2Dot(&A[i][0], &A[i][0]);
C[0][1] += A[i][0].dot(A[i][1]); //C[0][1] += V2Dot(&A[i][0], &A[i][1]);
/* C[1][0] += V2Dot(&A[i][0], &A[i][9]);*/
C[1][0] = C[0][1]; //C[1][0] = C[0][1]
C[1][1] += A[i][1].dot(A[i][1]); //C[1][1] += V2Dot(&A[i][1], &A[i][1]);
Tuple2d scaleLastB2=new Vector2d(PointE.getPoint2d(d[last])); scaleLastB2.scale(B2(uPrime[i])); // V2ScaleIII(d[last], B2(uPrime[i]))
Tuple2d scaleLastB3=new Vector2d(PointE.getPoint2d(d[last])); scaleLastB3.scale(B3(uPrime[i])); // V2ScaleIII(d[last], B3(uPrime[i]))
Tuple2d dLastB2B3Sum=new Vector2d(scaleLastB2); dLastB2B3Sum.add(scaleLastB3); //V2AddII(V2ScaleIII(d[last], B2(uPrime[i])), V2ScaleIII(d[last], B3(uPrime[i]))
Tuple2d scaleFirstB1=new Vector2d(PointE.getPoint2d(d[first])); scaleFirstB1.scale(B1(uPrime[i])); //V2ScaleIII(d[first], B1(uPrime[i]))
Tuple2d sumScaledFirstB1andB2B3=new Vector2d(scaleFirstB1); sumScaledFirstB1andB2B3.add(dLastB2B3Sum); //V2AddII(V2ScaleIII(d[first], B1(uPrime[i])), V2AddII(V2ScaleIII(d[last], B2(uPrime[i])), V2ScaleIII(d[last], B3(uPrime[i])))
Tuple2d scaleFirstB0=new Vector2d(PointE.getPoint2d(d[first])); scaleFirstB0.scale(B0(uPrime[i])); //V2ScaleIII(d[first], B0(uPrime[i])
Tuple2d sumB0Rest=new Vector2d(scaleFirstB0); sumB0Rest.add(sumScaledFirstB1andB2B3); //V2AddII(V2ScaleIII(d[first], B0(uPrime[i])), V2AddII( V2ScaleIII(d[first], B1(uPrime[i])), V2AddII(V2ScaleIII(d[last], B2(uPrime[i])), V2ScaleIII(d[last], B3(uPrime[i]))))));
Vector2d tmp=new Vector2d(PointE.getPoint2d(d[first + i]));
tmp.sub(sumB0Rest);
X[0] += A[i][0].dot(tmp);
X[1] += A[i][1].dot(tmp);
}
/* Compute the determinants of C and X */
det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
det_C0_X = C[0][0] * X[1] - C[1][0] * X[0];
det_X_C1 = X[0] * C[1][1] - X[1] * C[0][1];
/* Finally, derive alpha values */
alpha_l = (det_C0_C1 == 0) ? 0.0 : det_X_C1 / det_C0_C1;
alpha_r = (det_C0_C1 == 0) ? 0.0 : det_C0_X / det_C0_C1;
/* If alpha negative, use the Wu/Barsky heuristic (see text) */
/* (if alpha is 0, you get coincident control points that lead to
* divide by zero in any subsequent NewtonRaphsonRootFind() call. */
double segLength = d[first].distance(d[last]); //(d[first] - d[last]).Length;
double epsilonRel = epsilon * segLength;
if (alpha_l < epsilonRel || alpha_r < epsilonRel) {
/* fall back on standard (probably inaccurate) formula, and subdivide further if needed. */
double dist = segLength / 3.0;
bezCurve[0] = new PointE(d[first]);
bezCurve[3] = new PointE(d[last]);
Vector2d b1Tmp=new Vector2d(tHat1); b1Tmp.scaleAdd(dist, bezCurve[0].getPoint2d());
bezCurve[1] = new PointE(b1Tmp); //(tHat1 * dist) + bezCurve[0];
Vector2d b2Tmp=new Vector2d(tHat2); b2Tmp.scaleAdd(dist, bezCurve[3].getPoint2d());
bezCurve[2] = new PointE(b2Tmp); //(tHat2 * dist) + bezCurve[3];
return (bezCurve);
}
/* First and last control points of the Bezier curve are */
/* positioned exactly at the first and last data points */
/* Control points 1 and 2 are positioned an alpha distance out */
/* on the tangent vectors, left and right, respectively */
bezCurve[0] = new PointE(d[first]);
bezCurve[3] = new PointE(d[last]);
Vector2d alphaLTmp=new Vector2d(tHat1); alphaLTmp.scaleAdd(alpha_l, bezCurve[0].getPoint2d());
bezCurve[1] = new PointE(alphaLTmp); //(tHat1 * alpha_l) + bezCurve[0]
Vector2d alphaRTmp=new Vector2d(tHat2); alphaRTmp.scaleAdd(alpha_r, bezCurve[3].getPoint2d());
bezCurve[2] = new PointE(alphaRTmp); //(tHat2 * alpha_r) + bezCurve[3];
return (bezCurve);
}
/*
* Reparameterize:
* Given set of points and their parameterization, try to find
* a better parameterization.
*
*/
static double[] Reparameterize(Point2D[] d,int first,int last,double[] u, Point2D[] bezCurve)
{
int nPts = last-first+1;
int i;
double[] uPrime = new double[nPts]; /* New parameter values */
for (i = first; i <= last; i++) {
uPrime[i-first] = NewtonRaphsonRootFind(bezCurve, d[i], u[i-first]);
}
return uPrime;
}
/*
* NewtonRaphsonRootFind :
* Use Newton-Raphson iteration to find better root.
*/
static double NewtonRaphsonRootFind(Point2D[] Q, Point2D P, double u)
{
double numerator, denominator;
Point2D[] Q1 = new Point2D[3]; //Q'
Point2D[] Q2 = new Point2D[2]; //Q''
Point2D Q_u, Q1_u, Q2_u; /*u evaluated at Q, Q', & Q'' */
double uPrime; /* Improved u */
int i;
/* Compute Q(u) */
Q_u = BezierII(3, Q, u);
/* Generate control vertices for Q' */
for (i = 0; i <= 2; i++) {
double qXTmp=(Q[i+1].getX() - Q[i].getX()) * 3.0; //Q1[i].x = (Q[i+1].x - Q[i].x) * 3.0;
double qYTmp=(Q[i+1].getY() - Q[i].getY()) * 3.0; //Q1[i].y = (Q[i+1].y - Q[i].y) * 3.0;
Q1[i]=new Point2D.Double(qXTmp, qYTmp);
}
/* Generate control vertices for Q'' */
for (i = 0; i <= 1; i++) {
double qXTmp=(Q1[i+1].getX() - Q1[i].getX()) * 2.0; //Q2[i].x = (Q1[i+1].x - Q1[i].x) * 2.0;
double qYTmp=(Q1[i+1].getY() - Q1[i].getY()) * 2.0; //Q2[i].y = (Q1[i+1].y - Q1[i].y) * 2.0;
Q2[i]=new Point2D.Double(qXTmp, qYTmp);
}
/* Compute Q'(u) and Q''(u) */
Q1_u = BezierII(2, Q1, u);
Q2_u = BezierII(1, Q2, u);
/* Compute f(u)/f'(u) */
numerator = (Q_u.getX() - P.getX()) * (Q1_u.getX()) + (Q_u.getY() - P.getY()) * (Q1_u.getY());
denominator = (Q1_u.getX()) * (Q1_u.getX()) + (Q1_u.getY()) * (Q1_u.getY()) + (Q_u.getX() - P.getX()) * (Q2_u.getX()) + (Q_u.getY() - P.getY()) * (Q2_u.getY());
if (denominator == 0.0f) return u;
/* u = u - f(u)/f'(u) */
uPrime = u - (numerator/denominator);
return (uPrime);
}
/*
* Bezier :
* Evaluate a Bezier curve at a particular parameter value
*
*/
static Point2D BezierII(int degree, Point2D[] V, double t)
{
int i, j;
Point2D Q; /* Point on curve at parameter t */
Point2D[] Vtemp; /* Local copy of control points */
/* Copy array */
Vtemp = new Point2D[degree+1];
for (i = 0; i <= degree; i++) {
Vtemp[i] = new Point2D.Double(V[i].getX(), V[i].getY());
}
/* Triangle computation */
for (i = 1; i <= degree; i++) {
for (j = 0; j <= degree-i; j++) {
double tmpX, tmpY;
tmpX = (1.0 - t) * Vtemp[j].getX() + t * Vtemp[j+1].getX();
tmpY = (1.0 - t) * Vtemp[j].getY() + t * Vtemp[j+1].getY();
Vtemp[j].setLocation(tmpX, tmpY);
}
}
Q = Vtemp[0];
return Q;
}
/*
* B0, B1, B2, B3 :
* Bezier multipliers
*/
static double B0(double u)
{
double tmp = 1.0 - u;
return (tmp * tmp * tmp);
}
static double B1(double u)
{
double tmp = 1.0 - u;
return (3 * u * (tmp * tmp));
}
static double B2(double u)
{
double tmp = 1.0 - u;
return (3 * u * u * tmp);
}
static double B3(double u)
{
return (u * u * u);
}
/*
* ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
*Approximate unit tangents at endpoints and "center" of digitized curve
*/
static Vector2d ComputeLeftTangent(Point[] d, int end)
{
Vector2d tHat1=new Vector2d(PointE.getPoint2d(d[end+1]));
tHat1.sub(PointE.getPoint2d(d[end]));
tHat1.normalize();
return tHat1;
}
static Vector2d ComputeRightTangent(Point[] d, int end)
{
//tHat2 = V2SubII(d[end-1], d[end]); tHat2 = *V2Normalize(&tHat2);
Vector2d tHat2=new Vector2d(PointE.getPoint2d(d[end-1]));
tHat2.sub(PointE.getPoint2d(d[end]));
tHat2.normalize();
return tHat2;
}
static Vector2d ComputeCenterTangent(Point[] d ,int center)
{
//V1 = V2SubII(d[center-1], d[center]);
Vector2d V1=new Vector2d(PointE.getPoint2d(d[center-1]));
V1.sub(new PointE(d[center]).getPoint2d());
//V2 = V2SubII(d[center], d[center+1]);
Vector2d V2=new Vector2d(PointE.getPoint2d(d[center]));
V2.sub(PointE.getPoint2d(d[center+1]));
//tHatCenter.x = (V1.x + V2.x)/2.0;
//tHatCenter.y = (V1.y + V2.y)/2.0;
//tHatCenter = *V2Normalize(&tHatCenter);
Vector2d tHatCenter=new Vector2d((V1.x + V2.x)/2.0, (V1.y + V2.y)/2.0);
tHatCenter.normalize();
return tHatCenter;
}
/*
* ChordLengthParameterize :
* Assign parameter values to digitized points
* using relative distances between points.
*/
static double[] ChordLengthParameterize(Point[] d,int first,int last)
{
int i;
double[] u = new double[last-first+1]; /* Parameterization */
u[0] = 0.0;
for (i = first+1; i <= last; i++) {
u[i-first] = u[i-first-1] + d[i-1].distance(d[i]);
}
for (i = first + 1; i <= last; i++) {
u[i-first] = u[i-first] / u[last-first];
}
return u;
}
/*
* ComputeMaxError :
* Find the maximum squared distance of digitized points
* to fitted curve.
*/
static double ComputeMaxError(Point2D[] d, int first, int last, Point2D[] bezCurve, double[] u, AtomicInteger splitPoint)
{
int i;
double maxDist; /* Maximum error */
double dist; /* Current error */
Point2D P; /* Point on curve */
Vector2d v; /* Vector from point to curve */
int tmpSplitPoint=(last - first + 1)/2;
maxDist = 0.0;
for (i = first + 1; i < last; i++) {
P = BezierII(3, bezCurve, u[i-first]);
v = new Vector2d(P.getX() - d[i].getX(), P.getY() - d[i].getY()); //P - d[i];
dist = v.lengthSquared();
if (dist >= maxDist) {
maxDist = dist;
tmpSplitPoint=i;
}
}
splitPoint.set(tmpSplitPoint);
return maxDist;
}
/**
* This is kind of a bridge between javax.vecmath and java.util.Point2D
* #author Ruben
* #since 1.24
*/
public static class PointE extends Point2D.Double {
private static final long serialVersionUID = -1482403817370130793L;
public PointE(Tuple2d tup) {
super(tup.x, tup.y);
}
public PointE(Point2D p) {
super(p.getX(), p.getY());
}
public PointE(double x, double y) {
super(x, y);
}
public PointE scale(double dist) {
return new PointE(getX()*dist, getY()*dist);
}
public PointE scaleAdd(double dist, Point2D sum) {
return new PointE(getX()*dist + sum.getX(), getY()*dist + sum.getY());
}
public PointE substract(Point2D p) {
return new PointE(getX() - p.getX(), getY() - p.getY());
}
public Point2d getPoint2d() {
return getPoint2d(this);
}
public static Point2d getPoint2d(Point2D p) {
return new Point2d(p.getX(), p.getY());
}
}
Here an image of the latter working, white are lines, and red are Bezier:
Using this approach we use less control points and more accurate.
The sensitivity for the lines creation can be adjusted through the lineSensitivity attribute. If you don't want lines to be used at all just set it to infinite.
I'm sure this can be improved. Feel free to contribute :)
The algorithm is not doing any reduction, and because of the first explained in my post we have to run one. Here is a DouglasPeuckerReduction implementation, which for me works in some cases even more efficiently (less points to store and faster to render) than an additional FitCurves
import java.awt.Point;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;
public class DouglasPeuckerReduction {
public static List<Point> reduce(Point[] points, double tolerance)
{
if (points == null || points.length < 3) return Arrays.asList(points);
int firstPoint = 0;
int lastPoint = points.length - 1;
SortedList<Integer> pointIndexsToKeep;
try {
pointIndexsToKeep = new SortedList<Integer>(LinkedList.class);
} catch (Throwable t) {
t.printStackTrace(System.out);
ErrorReport.process(t);
return null;
}
//Add the first and last index to the keepers
pointIndexsToKeep.add(firstPoint);
pointIndexsToKeep.add(lastPoint);
//The first and the last point cannot be the same
while (points[firstPoint].equals(points[lastPoint])) {
lastPoint--;
}
reduce(points, firstPoint, lastPoint, tolerance, pointIndexsToKeep);
List<Point> returnPoints = new ArrayList<Point>(pointIndexsToKeep.size());
for (int pIndex : pointIndexsToKeep) {
returnPoints.add(points[pIndex]);
}
return returnPoints;
}
private static void reduce(Point[] points, int firstPoint, int lastPoint, double tolerance, List<Integer> pointIndexsToKeep) {
double maxDistance = 0;
int indexFarthest = 0;
Line tmpLine=new Line(points[firstPoint], points[lastPoint]);
for (int index = firstPoint; index < lastPoint; index++) {
double distance = tmpLine.getDistanceFrom(points[index]);
if (distance > maxDistance) {
maxDistance = distance;
indexFarthest = index;
}
}
if (maxDistance > tolerance && indexFarthest != 0) {
//Add the largest point that exceeds the tolerance
pointIndexsToKeep.add(indexFarthest);
reduce(points, firstPoint, indexFarthest, tolerance, pointIndexsToKeep);
reduce(points, indexFarthest, lastPoint, tolerance, pointIndexsToKeep);
}
}
}
I am using here my own implementation of a SortedList, and of a Line. You will have to make it yourself, sorry.
I haven't tested it, but one approach that comes to mind would be sampling values in some interval and creating a spline to connect the dots.
For example, say the x value of your curve starts at 0 and ends at 10. So you sample the y values at x=1,2,3,4,5,6,7,8,9,10 and create a spline from the points (0, y(0)), (1,y(1)), ... (10, y(10))
It would probably have problems such as accidental spikes drawn by the user, but it may be worth a shot
For Silverlight users of Kris' answer, Point is hobbled and Vector doesn't exist. This is a minimal Vector class that supports the code:
public class Vector
{
public double X { get; set; }
public double Y { get; set; }
public Vector(double x=0, double y=0)
{
X = x;
Y = y;
}
public static implicit operator Vector(Point b)
{
return new Vector(b.X, b.Y);
}
public static Point operator *(Vector left, double right)
{
return new Point(left.X * right, left.Y * right);
}
public static Vector operator -(Vector left, Point right)
{
return new Vector(left.X - right.X, left.Y - right.Y);
}
internal void Negate()
{
X = -X;
Y = -Y;
}
internal void Normalize()
{
double factor = 1.0 / Math.Sqrt(LengthSquared);
X *= factor;
Y *= factor;
}
public double LengthSquared { get { return X * X + Y * Y; } }
}
Also had to address use of Length and +,- operators. I chose to just add functions to the FitCurves class, and rewrite their usages where the compiler complained.
public static double Length(Point a, Point b)
{
double x = a.X-b.X;
double y = a.Y-b.Y;
return Math.Sqrt(x*x+y*y);
}
public static Point Add(Point a, Point b)
{
return new Point(a.X + b.X, a.Y + b.Y);
}
public static Point Subtract(Point a, Point b)
{
return new Point(a.X - b.X, a.Y - b.Y);
}
Related
Project: intelligence scissors.
The first part of the project is to load an image (in the form of RGBPixel 2d array) then to construct a graph of weights to use it later to determine the shortest path between 2 points on the image (the 2 points will be determined by an anchor and a free point..In short i will have the source and the destination points).
I have a function that open the image and return a RGBPixel 2d array already made.Now image is loaded i want to construct the graph to do the i should use a function called CalculatePixelEnergies here is the code
public static Vector2D CalculatePixelEnergies(int x, int y, RGBPixel[,] ImageMatrix)
{
if (ImageMatrix == null) throw new Exception("image is not set!");
Vector2D gradient = CalculateGradientAtPixel(x, y, ImageMatrix);
double gradientMagnitude = Math.Sqrt(gradient.X * gradient.X + gradient.Y * gradient.Y);
double edgeAngle = Math.Atan2(gradient.Y, gradient.X);
double rotatedEdgeAngle = edgeAngle + Math.PI / 2.0;
Vector2D energy = new Vector2D();
energy.X = Math.Abs(gradientMagnitude * Math.Cos(rotatedEdgeAngle));
energy.Y = Math.Abs(gradientMagnitude * Math.Sin(rotatedEdgeAngle));
return energy;
}
This function use CalculateGradientAtPixel, Here is the code in case you want it.
private static Vector2D CalculateGradientAtPixel(int x, int y, RGBPixel[,] ImageMatrix)
{
Vector2D gradient = new Vector2D();
RGBPixel mainPixel = ImageMatrix[y, x];
double pixelGrayVal = 0.21 * mainPixel.red + 0.72 * mainPixel.green + 0.07 * mainPixel.blue;
if (y == GetHeight(ImageMatrix) - 1)
{
//boundary pixel.
for (int i = 0; i < 3; i++)
{
gradient.Y = 0;
}
}
else
{
RGBPixel downPixel = ImageMatrix[y + 1, x];
double downPixelGrayVal = 0.21 * downPixel.red + 0.72 * downPixel.green + 0.07 * downPixel.blue;
gradient.Y = pixelGrayVal - downPixelGrayVal;
}
if (x == GetWidth(ImageMatrix) - 1)
{
//boundary pixel.
gradient.X = 0;
}
else
{
RGBPixel rightPixel = ImageMatrix[y, x + 1];
double rightPixelGrayVal = 0.21 * rightPixel.red + 0.72 * rightPixel.green + 0.07 * rightPixel.blue;
gradient.X = pixelGrayVal - rightPixelGrayVal;
}
return gradient;
}
In my code of graph construction i decided to make a 2d double array to hold the weights, here what i do but it seems to be a wrong construction
public static double [,] calculateWeights(RGBPixel[,] ImageMatrix)
{
double[,] weights = new double[1000, 1000];
int height = ImageOperations.GetHeight(ImageMatrix);
int width = ImageOperations.GetWidth(ImageMatrix);
for (int y = 0; y < height - 1; y++)
{
for (int x = 0; x < width - 1; x++)
{
Vector2D e;
e = ImageOperations.CalculatePixelEnergies(x, y, ImageMatrix);
weights[y + 1, x] = 1 / e.X;
weights[y, x + 1] = 1 / e.Y;
}
}
return weights;
}
an example for an image
an other example for an image
Help me, because I'm rly tired of this...
I need to count angles between current point and the closest 3 points (look at an image below) - I need to sort the angles in descending order (to get the point with the largest angle - if it doesn't fit expectations, I have to get another one).
I tried to do something, but it doesn't work...
private static Vertex[] SortByAngle(IEnumerable<Vertex> vs, Vertex current, Vertex previous)
{
if (current.CompareTo(previous) == 0)
{
previous.X = previous.X - 1.0; // this is a trick to handle the first point
}
var vertices = new Dictionary<Vertex, double>();
foreach (var v in vs)
{
double priorAngle = Angle(previous, current);
double nextAngle = Angle(current, v);
double angleInBetween = 180.0 - (priorAngle + nextAngle);
vertices.Add((Vertex) v.Clone(), angleInBetween);
}
// here the angles are incorrect, because I want to sort them in desc order, but it's a real mess when I do OrderByDescending - something is wrong with my code:S
vertices = vertices.OrderBy(v => v.Value).ToDictionary(k => k.Key, v => v.Value);
return vertices.Select(v => new Vertex(v.Key.X, v.Key.Y)).ToArray();
}
private static double Angle(Vertex v1, Vertex v2, double offsetInDegrees = 0.0)
{
return (RadianToDegree(Math.Atan2(-v2.Y + v1.Y, -v2.X + v1.X)) + offsetInDegrees);
}
public static double RadianToDegree(double radian)
{
var degree = radian * (180.0 / Math.PI);
if (degree < 0)
degree = 360 + degree;
return degree;
}
vs is my set of 3 nearest points
current and previous are obvious:)
i didn't tested it, but i restyled a little, avoiding dictionaries. I think your mistake is in: double angleInBetween = 180.0 - (priorAngle + nextAngle); should be: double angleInBetween = (180.0 - priorAngle) + nextAngle;
public struct Vertex
{
public double X { get; set; }
public double Y { get; set; }
}
private static double CalcDistance(Vertex v1, Vertex v2)
{
double dX = (v2.X - v1.X);
double dY = (v2.Y - v1.Y);
return Math.Sqrt((dX * dX) + (dY * dY));
}
private static Vertex[] SortByAngle(IEnumerable<Vertex> vs, Vertex current, Vertex previous)
{
var verticesOnDistance = from vertex in vs
where !vertex.Equals(current)
let distance = CalcDistance(current, vertex)
orderby distance
select vertex;
double priorAngle = Angle(previous, current);
var verticeAngles = from vertex in verticesOnDistance.Take(3)
let nextAngle = Angle(current, vertex)
let angleInBetween = (180.0 - priorAngle) + nextAngle
orderby angleInBetween descending
select vertex;
return verticeAngles.ToArray();
}
private static double Angle(Vertex v1, Vertex v2, double offsetInDegrees = 0.0)
{
return (RadianToDegree(Math.Atan2(-v2.Y + v1.Y, -v2.X + v1.X)) + offsetInDegrees);
}
public static double RadianToDegree(double radian)
{
var degree = radian * (180.0 / Math.PI);
if (degree < 0)
degree = 360 + degree;
return degree;
}
I'm running a little out of time here. I'll be on later... I'm not sure this is correct, but maybe shine another light on it...
Good luck
I wanted a class that can convert one system to another.
I've found a source code in python and tried to port it into C#.
This is the python source. From here
import math
class GlobalMercator(object):
def __init__(self, tileSize=256):
"Initialize the TMS Global Mercator pyramid"
self.tileSize = tileSize
self.initialResolution = 2 * math.pi * 6378137 / self.tileSize
# 156543.03392804062 for tileSize 256 pixels
self.originShift = 2 * math.pi * 6378137 / 2.0
# 20037508.342789244
def LatLonToMeters(self, lat, lon ):
"Converts given lat/lon in WGS84 Datum to XY in Spherical Mercator EPSG:900913"
mx = lon * self.originShift / 180.0
my = math.log( math.tan((90 + lat) * math.pi / 360.0 )) / (math.pi / 180.0)
my = my * self.originShift / 180.0
return mx, my
def MetersToLatLon(self, mx, my ):
"Converts XY point from Spherical Mercator EPSG:900913 to lat/lon in WGS84 Datum"
lon = (mx / self.originShift) * 180.0
lat = (my / self.originShift) * 180.0
lat = 180 / math.pi * (2 * math.atan( math.exp( lat * math.pi / 180.0)) - math.pi / 2.0)
return lat, lon
def PixelsToMeters(self, px, py, zoom):
"Converts pixel coordinates in given zoom level of pyramid to EPSG:900913"
res = self.Resolution( zoom )
mx = px * res - self.originShift
my = py * res - self.originShift
return mx, my
def MetersToPixels(self, mx, my, zoom):
"Converts EPSG:900913 to pyramid pixel coordinates in given zoom level"
res = self.Resolution( zoom )
px = (mx + self.originShift) / res
py = (my + self.originShift) / res
return px, py
def PixelsToTile(self, px, py):
"Returns a tile covering region in given pixel coordinates"
tx = int( math.ceil( px / float(self.tileSize) ) - 1 )
ty = int( math.ceil( py / float(self.tileSize) ) - 1 )
return tx, ty
def PixelsToRaster(self, px, py, zoom):
"Move the origin of pixel coordinates to top-left corner"
mapSize = self.tileSize << zoom
return px, mapSize - py
def MetersToTile(self, mx, my, zoom):
"Returns tile for given mercator coordinates"
px, py = self.MetersToPixels( mx, my, zoom)
return self.PixelsToTile( px, py)
def TileBounds(self, tx, ty, zoom):
"Returns bounds of the given tile in EPSG:900913 coordinates"
minx, miny = self.PixelsToMeters( tx*self.tileSize, ty*self.tileSize, zoom )
maxx, maxy = self.PixelsToMeters( (tx+1)*self.tileSize, (ty+1)*self.tileSize, zoom )
return ( minx, miny, maxx, maxy )
def TileLatLonBounds(self, tx, ty, zoom ):
"Returns bounds of the given tile in latutude/longitude using WGS84 datum"
bounds = self.TileBounds( tx, ty, zoom)
minLat, minLon = self.MetersToLatLon(bounds[0], bounds[1])
maxLat, maxLon = self.MetersToLatLon(bounds[2], bounds[3])
return ( minLat, minLon, maxLat, maxLon )
def Resolution(self, zoom ):
"Resolution (meters/pixel) for given zoom level (measured at Equator)"
# return (2 * math.pi * 6378137) / (self.tileSize * 2**zoom)
return self.initialResolution / (2**zoom)
def ZoomForPixelSize(self, pixelSize ):
"Maximal scaledown zoom of the pyramid closest to the pixelSize."
for i in range(30):
if pixelSize > self.Resolution(i):
return i-1 if i!=0 else 0 # We don't want to scale up
def GoogleTile(self, tx, ty, zoom):
"Converts TMS tile coordinates to Google Tile coordinates"
# coordinate origin is moved from bottom-left to top-left corner of the extent
return tx, (2**zoom - 1) - ty
def QuadTree(self, tx, ty, zoom ):
"Converts TMS tile coordinates to Microsoft QuadTree"
quadKey = ""
ty = (2**zoom - 1) - ty
for i in range(zoom, 0, -1):
digit = 0
mask = 1 << (i-1)
if (tx & mask) != 0:
digit += 1
if (ty & mask) != 0:
digit += 2
quadKey += str(digit)
return quadKey
Here is my C# port.
public class GlobalMercator {
private Int32 TileSize;
private Double InitialResolution;
private Double OriginShift;
private const Int32 EarthRadius = 6378137;
public GlobalMercator() {
TileSize = 256;
InitialResolution = 2 * Math.PI * EarthRadius / TileSize;
OriginShift = 2 * Math.PI * EarthRadius / 2;
}
public DPoint LatLonToMeters(Double lat, Double lon) {
var p = new DPoint();
p.X = lon * OriginShift / 180;
p.Y = Math.Log(Math.Tan((90 + lat) * Math.PI / 360)) / (Math.PI / 180);
p.Y = p.Y * OriginShift / 180;
return p;
}
public GeoPoint MetersToLatLon(DPoint m) {
var ll = new GeoPoint();
ll.Longitude = (m.X / OriginShift) * 180;
ll.Latitude = (m.Y / OriginShift) * 180;
ll.Latitude = 180 / Math.PI * (2 * Math.Atan(Math.Exp(ll.Latitude * Math.PI / 180)) - Math.PI / 2);
return ll;
}
public DPoint PixelsToMeters(DPoint p, Int32 zoom) {
var res = Resolution(zoom);
var met = new DPoint();
met.X = p.X * res - OriginShift;
met.Y = p.Y * res - OriginShift;
return met;
}
public DPoint MetersToPixels(DPoint m, Int32 zoom) {
var res = Resolution(zoom);
var pix = new DPoint();
pix.X = (m.X + OriginShift) / res;
pix.Y = (m.Y + OriginShift) / res;
return pix;
}
public Point PixelsToTile(DPoint p) {
var t = new Point();
t.X = (Int32)Math.Ceiling(p.X / (Double)TileSize) - 1;
t.Y = (Int32)Math.Ceiling(p.Y / (Double)TileSize) - 1;
return t;
}
public Point PixelsToRaster(Point p, Int32 zoom) {
var mapSize = TileSize << zoom;
return new Point(p.X, mapSize - p.Y);
}
public Point MetersToTile(Point m, Int32 zoom) {
var p = MetersToPixels(m, zoom);
return PixelsToTile(p);
}
public Pair<DPoint> TileBounds(Point t, Int32 zoom) {
var min = PixelsToMeters(new DPoint(t.X * TileSize, t.Y * TileSize), zoom);
var max = PixelsToMeters(new DPoint((t.X + 1) * TileSize, (t.Y + 1) * TileSize), zoom);
return new Pair<DPoint>(min, max);
}
public Pair<GeoPoint> TileLatLonBounds(Point t, Int32 zoom) {
var bound = TileBounds(t, zoom);
var min = MetersToLatLon(bound.Min);
var max = MetersToLatLon(bound.Max);
return new Pair<GeoPoint>(min, max);
}
public Double Resolution(Int32 zoom) {
return InitialResolution / (2 ^ zoom);
}
public Double ZoomForPixelSize(Double pixelSize) {
for (var i = 0; i < 30; i++)
if (pixelSize > Resolution(i))
return i != 0 ? i - 1 : 0;
throw new InvalidOperationException();
}
public Point ToGoogleTile(Point t, Int32 zoom) {
return new Point(t.X, ((Int32)Math.Pow(2, zoom) - 1) - t.Y);
}
public Point ToTmsTile(Point t, Int32 zoom) {
return new Point(t.X, ((Int32)Math.Pow(2, zoom) - 1) - t.Y);
}
}
public struct Point {
public Point(Int32 x, Int32 y)
: this() {
X = x;
Y = y;
}
public Int32 X { get; set; }
public Int32 Y { get; set; }
}
public struct DPoint {
public DPoint(Double x, Double y)
: this() {
this.X = x;
this.Y = y;
}
public Double X { get; set; }
public Double Y { get; set; }
public static implicit operator DPoint(Point p) {
return new DPoint(p.X, p.Y);
}
}
public class GeoPoint {
public Double Latitude { get; set; }
public Double Longitude { get; set; }
}
public class Pair<T> {
public Pair() {}
public Pair(T min, T max) {
Min = min;
Max = max;
}
public T Min { get; set; }
public T Max { get; set; }
}
I have two questions.
Did I port the code correctly? (I intentionally omitted one method as I don't use it and added one my own)
Here I have coordinates
WGS84 datum (longitude/latitude):
-123.75 36.59788913307022
-118.125 40.97989806962013
Spherical Mercator (meters):
-13775786.985667605 4383204.9499851465
-13149614.849955441 5009377.085697312
Pixels
2560 6144 2816 6400
Google
x:10, y:24, z:6
TMS
x:10, y:39, z:6
QuadTree
023010
How should I chain the methods so that I get from Google's tile coordinates (10, 24, 6) the spherical mercator meters?
Update
Finding answer for my second question is more important for me.
There's at least one bug in your class:
public Double Resolution(Int32 zoom) {
return InitialResolution / (2 ^ zoom); // BAD CODE, USE Math.Pow, not ^
}
Where you've mistaken the binary XOR operator for the exponent operator.
I've rewritten the code, made most functions static, and added a few more relevant functions:
/// <summary>
/// Conversion routines for Google, TMS, and Microsoft Quadtree tile representations, derived from
/// http://www.maptiler.org/google-maps-coordinates-tile-bounds-projection/
/// </summary>
public class WebMercator
{
private const int TileSize = 256;
private const int EarthRadius = 6378137;
private const double InitialResolution = 2 * Math.PI * EarthRadius / TileSize;
private const double OriginShift = 2 * Math.PI * EarthRadius / 2;
//Converts given lat/lon in WGS84 Datum to XY in Spherical Mercator EPSG:900913
public static Point LatLonToMeters(double lat, double lon)
{
var p = new Point();
p.X = lon * OriginShift / 180;
p.Y = Math.Log(Math.Tan((90 + lat) * Math.PI / 360)) / (Math.PI / 180);
p.Y = p.Y * OriginShift / 180;
return p;
}
//Converts XY point from (Spherical) Web Mercator EPSG:3785 (unofficially EPSG:900913) to lat/lon in WGS84 Datum
public static Point MetersToLatLon(Point m)
{
var ll = new Point();
ll.X = (m.X / OriginShift) * 180;
ll.Y = (m.Y / OriginShift) * 180;
ll.Y = 180 / Math.PI * (2 * Math.Atan(Math.Exp(ll.Y * Math.PI / 180)) - Math.PI / 2);
return ll;
}
//Converts pixel coordinates in given zoom level of pyramid to EPSG:900913
public static Point PixelsToMeters(Point p, int zoom)
{
var res = Resolution(zoom);
var met = new Point();
met.X = p.X * res - OriginShift;
met.Y = p.Y * res - OriginShift;
return met;
}
//Converts EPSG:900913 to pyramid pixel coordinates in given zoom level
public static Point MetersToPixels(Point m, int zoom)
{
var res = Resolution(zoom);
var pix = new Point();
pix.X = (m.X + OriginShift) / res;
pix.Y = (m.Y + OriginShift) / res;
return pix;
}
//Returns a TMS (NOT Google!) tile covering region in given pixel coordinates
public static Tile PixelsToTile(Point p)
{
var t = new Tile();
t.X = (int)Math.Ceiling(p.X / (double)TileSize) - 1;
t.Y = (int)Math.Ceiling(p.Y / (double)TileSize) - 1;
return t;
}
public static Point PixelsToRaster(Point p, int zoom)
{
var mapSize = TileSize << zoom;
return new Point(p.X, mapSize - p.Y);
}
//Returns tile for given mercator coordinates
public static Tile MetersToTile(Point m, int zoom)
{
var p = MetersToPixels(m, zoom);
return PixelsToTile(p);
}
//Returns bounds of the given tile in EPSG:900913 coordinates
public static Rect TileBounds(Tile t, int zoom)
{
var min = PixelsToMeters(new Point(t.X * TileSize, t.Y * TileSize), zoom);
var max = PixelsToMeters(new Point((t.X + 1) * TileSize, (t.Y + 1) * TileSize), zoom);
return new Rect(min, max);
}
//Returns bounds of the given tile in latutude/longitude using WGS84 datum
public static Rect TileLatLonBounds(Tile t, int zoom)
{
var bound = TileBounds(t, zoom);
var min = MetersToLatLon(new Point(bound.Left, bound.Top));
var max = MetersToLatLon(new Point(bound.Right, bound.Bottom));
return new Rect(min, max);
}
//Resolution (meters/pixel) for given zoom level (measured at Equator)
public static double Resolution(int zoom)
{
return InitialResolution / (Math.Pow(2, zoom));
}
public static double ZoomForPixelSize(double pixelSize)
{
for (var i = 0; i < 30; i++)
if (pixelSize > Resolution(i))
return i != 0 ? i - 1 : 0;
throw new InvalidOperationException();
}
// Switch to Google Tile representation from TMS
public static Tile ToGoogleTile(Tile t, int zoom)
{
return new Tile(t.X, ((int)Math.Pow(2, zoom) - 1) - t.Y);
}
// Switch to TMS Tile representation from Google
public static Tile ToTmsTile(Tile t, int zoom)
{
return new Tile(t.X, ((int)Math.Pow(2, zoom) - 1) - t.Y);
}
//Converts TMS tile coordinates to Microsoft QuadTree
public static string QuadTree(int tx, int ty, int zoom)
{
var quadtree = "";
ty = ((1 << zoom) - 1) - ty;
for (var i = zoom; i >= 1; i--)
{
var digit = 0;
var mask = 1 << (i - 1);
if ((tx & mask) != 0)
digit += 1;
if ((ty & mask) != 0)
digit += 2;
quadtree += digit;
}
return quadtree;
}
//Converts a quadtree to tile coordinates
public static Tile QuadTreeToTile(string quadtree, int zoom)
{
int tx= 0, ty = 0;
for (var i = zoom; i >= 1; i--)
{
var ch = quadtree[zoom - i];
var mask = 1 << (i - 1);
var digit = ch - '0';
if ((digit & 1) != 0)
tx += mask;
if ((digit & 2) != 0)
ty += mask;
}
ty = ((1 << zoom) - 1) - ty;
return new Tile(tx, ty);
}
//Converts a latitude and longitude to quadtree at the specified zoom level
public static string LatLonToQuadTree(Point latLon, int zoom)
{
Point m = LatLonToMeters(latLon.Y, latLon.X);
Tile t = MetersToTile(m, zoom);
return QuadTree(t.X, t.Y, zoom);
}
//Converts a quadtree location into a latitude/longitude bounding rectangle
public static Rect QuadTreeToLatLon(string quadtree)
{
int zoom = quadtree.Length;
Tile t = QuadTreeToTile(quadtree, zoom);
return TileLatLonBounds(t, zoom);
}
//Returns a list of all of the quadtree locations at a given zoom level within a latitude/longude box
public static List<string> GetQuadTreeList(int zoom, Point latLonMin, Point latLonMax)
{
if (latLonMax.Y< latLonMin.Y|| latLonMax.X< latLonMin.X)
return null;
Point mMin = LatLonToMeters(latLonMin.Y, latLonMin.X);
Tile tmin = MetersToTile(mMin, zoom);
Point mMax = LatLonToMeters(latLonMax.Y, latLonMax.X);
Tile tmax = MetersToTile(mMax, zoom);
var arr = new List<string>();
for (var ty = tmin.Y; ty <= tmax.Y; ty++)
{
for (var tx = tmin.X; tx <= tmax.X; tx++)
{
var quadtree = QuadTree(tx, ty, zoom);
arr.Add(quadtree);
}
}
return arr;
}
}
/// <summary>
/// Reference to a Tile X, Y index
/// </summary>
public class Tile
{
public Tile() { }
public Tile(int x, int y)
{
X = x;
Y = y;
}
public int X { get; set; }
public int Y { get; set; }
}
To solve your second question, use the following sequence:
int zoom = 6;
Tile googleTile = new Tile(10,24);
Tile tmsTile = GlobalMercator.ToTmsTile(googleTile, zoom);
Rect r3 = GlobalMercator.TileLatLonBounds(tmsTile, zoom);
var tl = GlobalMercator.LatLonToMeters(r3.Top, r3.Left);
var br = GlobalMercator.LatLonToMeters(r3.Bottom, r3.Right);
Debug.WriteLine("{0:0.000} {1:0.000}", tl.X, tl.Y);
Debug.WriteLine("{0:0.000} {1:0.000}", br.X, br.Y);
// -13775787.000 4383205.000
// -13149615.000 5009376.500
The best opensource solution for converting coordinates from one projection to another is Proj4 originally written in c but ported to numerous programming languages. The port to c# that I have tried and used is DotSpatial Projections found on CodePlex. It is easy to find out how to use it based on the examples. The only thing you need to know are conversion parameters for your case.
CoordinateSharp is available on NuGet. It's light weight and makes coordinate conversions really. It's not designed for mapping, but strait up conversions (and location based celestial information) if that is a factor.
Example
Coordinate c = new Coordinate(myLat,myLong);
c.UTM; //Outputs UTM string
c.UTM.Easting //UTM easting property
A couple of pointers for anyone reading that wants to use Oybek excellent code:
You need to add using System.Windows but also Add a Reference to the WindowsBase assembly, otherwise VS wont find Point and Rect.
Note that just must not use System.Drawing
And here's a new function that will convert Zoom lat/lng to a Google Tile:
public static Tile LatLonToGoogleTile(Point latLon, int zoom)
{
Point m = LatLonToMeters(latLon.Y, latLon.X);
Tile t = MetersToTile(m, zoom);
return ToGoogleTile(t, zoom);
}
How can I calculate the square root of a Float in C#, similar to Core.Sqrt in XNA?
Since .net core 2.0 you can use MathF.Sqrt.
In older versions, you can calculate it for double and then cast back to float. May be a bit slow, but should work.
(float)Math.Sqrt(inputFloat)
Hate to say this, but 0x5f3759df seems to take 3x as long as Math.Sqrt. I just did some testing with timers.
Math.Sqrt in a for-loop accessing pre-calculated arrays resulted in approx 80ms.
0x5f3759df under the same circumstances resulted in 180+ms
The test was conducted several times using the Release mode optimizations.
Source below:
/*
================
SquareRootFloat
================
*/
unsafe static void SquareRootFloat(ref float number, out float result)
{
long i;
float x, y;
const float f = 1.5F;
x = number * 0.5F;
y = number;
i = *(long*)&y;
i = 0x5f3759df - (i >> 1);
y = *(float*)&i;
y = y * (f - (x * y * y));
y = y * (f - (x * y * y));
result = number * y;
}
/*
================
SquareRootFloat
================
*/
unsafe static float SquareRootFloat(float number)
{
long i;
float x, y;
const float f = 1.5F;
x = number * 0.5F;
y = number;
i = *(long*)&y;
i = 0x5f3759df - (i >> 1);
y = *(float*)&i;
y = y * (f - (x * y * y));
y = y * (f - (x * y * y));
return number * y;
}
/// <summary>
/// The main entry point for the application.
/// </summary>
[STAThread]
static void Main()
{
int Cycles = 10000000;
Random rnd = new Random();
float[] Values = new float[Cycles];
for (int i = 0; i < Cycles; i++)
Values[i] = (float)(rnd.NextDouble() * 10000.0);
TimeSpan SqrtTime;
float[] Results = new float[Cycles];
DateTime Start = DateTime.Now;
for (int i = 0; i < Cycles; i++)
{
SquareRootFloat(ref Values[i], out Results[i]);
//Results[i] = (float)Math.Sqrt((float)Values[i]);
//Results[i] = SquareRootFloat(Values[i]);
}
DateTime End = DateTime.Now;
SqrtTime = End - Start;
Console.WriteLine("Sqrt was " + SqrtTime.TotalMilliseconds.ToString() + " long");
Console.ReadKey();
}
}
var result = Math.Sqrt((double)value);
private double operand1;
private void squareRoot_Click(object sender, EventArgs e)
{
operand1 = Math.Sqrt(operand1);
this.textBox1.Text = operand1.ToString();
}
I'm looking for an algorithm that determines the near and far intersection points between a line segment and an axis-aligned box.
Here is my method definition:
public static Point3D[] IntersectionOfLineSegmentWithAxisAlignedBox(
Point3D rayBegin, Point3D rayEnd, Point3D boxCenter, Size3D boxSize)
If the line segment doesn't intersect the box, the method should return an empty Point3D array.
From my research so far, I've come across some research papers with highly optimized algorithms, but they all seem to be written in C++ and would require multiple long class files to be converted to C#. For my purposes, something that is reasonably efficient, easy to understand by someone who gets dot products and cross products, and simple/short would be preferred.
Here's what I ended up using:
public static List<Point3D> IntersectionOfLineSegmentWithAxisAlignedBox(
Point3D segmentBegin, Point3D segmentEnd, Point3D boxCenter, Size3D boxSize)
{
var beginToEnd = segmentEnd - segmentBegin;
var minToMax = new Vector3D(boxSize.X, boxSize.Y, boxSize.Z);
var min = boxCenter - minToMax / 2;
var max = boxCenter + minToMax / 2;
var beginToMin = min - segmentBegin;
var beginToMax = max - segmentBegin;
var tNear = double.MinValue;
var tFar = double.MaxValue;
var intersections = new List<Point3D>();
foreach (Axis axis in Enum.GetValues(typeof(Axis)))
{
if (beginToEnd.GetCoordinate(axis) == 0) // parallel
{
if (beginToMin.GetCoordinate(axis) > 0 || beginToMax.GetCoordinate(axis) < 0)
return intersections; // segment is not between planes
}
else
{
var t1 = beginToMin.GetCoordinate(axis) / beginToEnd.GetCoordinate(axis);
var t2 = beginToMax.GetCoordinate(axis) / beginToEnd.GetCoordinate(axis);
var tMin = Math.Min(t1, t2);
var tMax = Math.Max(t1, t2);
if (tMin > tNear) tNear = tMin;
if (tMax < tFar) tFar = tMax;
if (tNear > tFar || tFar < 0) return intersections;
}
}
if (tNear >= 0 && tNear <= 1) intersections.Add(segmentBegin + beginToEnd * tNear);
if (tFar >= 0 && tFar <= 1) intersections.Add(segmentBegin + beginToEnd * tFar);
return intersections;
}
public enum Axis
{
X,
Y,
Z
}
public static double GetCoordinate(this Point3D point, Axis axis)
{
switch (axis)
{
case Axis.X:
return point.X;
case Axis.Y:
return point.Y;
case Axis.Z:
return point.Z;
default:
throw new ArgumentException();
}
}
public static double GetCoordinate(this Vector3D vector, Axis axis)
{
switch (axis)
{
case Axis.X:
return vector.X;
case Axis.Y:
return vector.Y;
case Axis.Z:
return vector.Z;
default:
throw new ArgumentException();
}
}
Well, for an axis-aligned box it's pretty simple: you have to find intersection of your ray with 6 planes (defined by the box faces) and then check the points you found against the box vertices coordinates limits.
Optimized version of the answer. There is no reason to do allocations or lookups.
public struct Ray3
{
public Vec3 origin, direction;
public bool IntersectRayBox(Box3 box, out Vec3 point1, out Vec3 point2)
{
var min = (box.center - (box.size / 2)) - origin;
var max = (box.center + (box.size / 2)) - origin;
float near = float.MinValue;
float far = float.MaxValue;
// X
float t1 = min.x / direction.x;
float t2 = max.x / direction.x;
float tMin = Math.Min(t1, t2);
float tMax = Math.Max(t1, t2);
if (tMin > near) near = tMin;
if (tMax < far) far = tMax;
if (near > far || far < 0)
{
point1 = Vec3.zero;
point2 = Vec3.zero;
return false;
}
// Y
t1 = min.y / direction.y;
t2 = max.y / direction.y;
tMin = Math.Min(t1, t2);
tMax = Math.Max(t1, t2);
if (tMin > near) near = tMin;
if (tMax < far) far = tMax;
if (near > far || far < 0)
{
point1 = Vec3.zero;
point2 = Vec3.zero;
return false;
}
// Z
t1 = min.z / direction.z;
t2 = max.z / direction.z;
tMin = Math.Min(t1, t2);
tMax = Math.Max(t1, t2);
if (tMin > near) near = tMin;
if (tMax < far) far = tMax;
if (near > far || far < 0)
{
point1 = Vec3.zero;
point2 = Vec3.zero;
return false;
}
point1 = origin + direction * near;
point2 = origin + direction * far;
return true;
}
}