package se.oru.coordination.coordination_oru.util.splines; import java.util.ArrayList; import org.metacsp.multi.spatioTemporal.paths.Pose; import org.metacsp.multi.spatioTemporal.paths.PoseSteering; import com.vividsolutions.jts.geom.Coordinate; public class BezierSplineFactory { /** * A cubic bezier method to calculate the point at t along the Bezier Curve give * the parameter points. * @param p1 * @param p2 * @param p3 * @param p4 * @param t A value between 0 and 1, inclusive. * @return */ public static Coordinate cubicBezier(Coordinate p1, Coordinate p2, Coordinate p3, Coordinate p4, double t){ return new Coordinate( cubicBezierPoint(p1.x, p2.x, p3.x, p4.x, t), cubicBezierPoint(p1.y, p2.y, p3.y, p4.y, t), cubicBezierPoint(p1.z, p2.z, p3.z, p4.z, t)); } /** * A quadratic Bezier method to calculate the point at t along the Bezier Curve give * the parameter points. * @param p1 * @param p2 * @param p3 * @param t A value between 0 and 1, inclusive. * @return */ public static Coordinate quadBezier(Coordinate p1, Coordinate p2, Coordinate p3, double t){ return new Coordinate( quadBezierPoint(p1.x, p2.x, p3.x, t), quadBezierPoint(p1.y, p2.y, p3.y, t), quadBezierPoint(p1.z, p2.z, p3.z, t)); } /** * The cubic Bezier equation. * @param a0 * @param a1 * @param a2 * @param a3 * @param t * @return */ private static double cubicBezierPoint(double a0, double a1, double a2, double a3, double t){ return Math.pow(1-t, 3) * a0 + 3* Math.pow(1-t, 2) * t * a1 + 3*(1-t) * Math.pow(t, 2) * a2 + Math.pow(t, 3) * a3; } /** * The quadratic Bezier equation, * @param a0 * @param a1 * @param a2 * @param t * @return */ private static double quadBezierPoint(double a0, double a1, double a2, double t){ return Math.pow(1-t, 2) * a0 + 2* (1-t) * t * a1 + Math.pow(t, 2) * a2; } public static Pose[] asPoses(Coordinate[] spline) { Pose[] poses = new Pose[spline.length]; poses[0] = new Pose(spline[0].x, spline[0].y, Math.atan2(spline[1].y-spline[0].y, spline[1].x-spline[0].x)); poses[spline.length-1] = new Pose(spline[spline.length-1].x, spline[spline.length-1].y, Math.atan2(spline[spline.length-1].y-spline[spline.length-2].y, spline[spline.length-1].x-spline[spline.length-2].x)); for (int i = 1; i < spline.length-1; i++) { poses[i] = new Pose(spline[i].x,spline[i].y,Math.atan2(spline[i+1].y-spline[i].y, spline[i+1].x-spline[i].x)); } return poses; } public static PoseSteering[] asPoseSteering(Coordinate[] spline) { Pose[] poses = asPoses(spline); PoseSteering[] ret = new PoseSteering[poses.length]; for (int i = 0; i < ret.length; i++) ret[i] = new PoseSteering(poses[i].getX(),poses[i].getY(),poses[i].getTheta(),0.0); return ret; } public static Coordinate[] createBezierSpline(Coordinate p1, Coordinate p2, Coordinate p3, Coordinate p4, double dist) { ArrayList<Coordinate> ret = new ArrayList<Coordinate>(); double t = 0.0; Coordinate prevCoord = p1; ret.add(prevCoord); do { Coordinate coord = cubicBezier(p1, p2, p3, p4, t); if (prevCoord.distance(coord) >= dist) { ret.add(coord); prevCoord = coord; } t += 0.01; } while (t < 1.0); if (ret.get(ret.size()-1).distance(p4) > dist/4.0) ret.add(p4); return ret.toArray(new Coordinate[ret.size()]); } public static void main(String[] args) { Coordinate[] coords = new Coordinate[] { new Coordinate(0.0,0.0), new Coordinate(1.0,1.0), new Coordinate(1.0,2.0), new Coordinate(3.0,0.0) }; double t = 0.0; do { Coordinate coord = cubicBezier(coords[0], coords[1], coords[2], coords[3], t); System.out.println(coord); t += 0.1; } while (t < 1.0); } }