First Commit

.classpath

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.6"/>
+	<classpathentry kind="src" path="src"/>
+	<classpathentry kind="output" path="bin"/>
+</classpath>

.project

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>MyPathfinder</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+	</natures>
+</projectDescription>

.settings/org.eclipse.jdt.core.prefs

@@ -0,0 +1,11 @@
+eclipse.preferences.version=1
+org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled
+org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.6
+org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve
+org.eclipse.jdt.core.compiler.compliance=1.6
+org.eclipse.jdt.core.compiler.debug.lineNumber=generate
+org.eclipse.jdt.core.compiler.debug.localVariable=generate
+org.eclipse.jdt.core.compiler.debug.sourceFile=generate
+org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
+org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
+org.eclipse.jdt.core.compiler.source=1.6

src/Main/CalculatePath.java

@@ -0,0 +1,110 @@
+package Main;
+
+import PartTypes.Acceleration;
+import PartTypes.Jerk;
+
+public class CalculatePath {
+
+	private static Waypoint[] points;
+	private static Trajectory.Segment[] segments;
+	private static Trajectory.Config config;
+	private static int part, waypoint;
+	private static double xChange, yChange, slope, distance, acceleration, velocity;
+	private static double[] Jerk1, Accel, Jerk2, Constant, Jerk3, Decel, Jerk4;
+	private static int seg = 1;
+	private static double time;
+	private static double[] segTimes;
+	private static int portion;
+
+	public static Trajectory generateTrajectory(Waypoint[] waypoints, Trajectory.Config configuration) {
+		part = 1;
+		points = waypoints;
+		config = configuration;
+		int numOfPoints = waypoints.length;
+		for(int i = 1; i <= numOfPoints; i++) {
+			calculateBetweenWaypoints(i);
+		}
+		return new Trajectory(segments);
+	}
+
+	private static void calculateBetweenWaypoints(int point) {
+		waypoint = point;
+
+		yChange = points[waypoint+1].y - points[waypoint].y;
+		xChange = points[waypoint+1].x - points[waypoint].x;
+		slope = yChange/xChange; //for every x toward the next point, y changes how much, might not be needed in the end
+
+		if(points[waypoint].angle == points[waypoint + 1].angle 
+				&& (Math.tan(slope) == points[waypoint].angle || points[waypoint].x == points[waypoint + 1].x)) {
+			calculatePart(true);
+			part++;
+		}else {
+			calculatePart(true);
+			part++;
+			calculatePart(false);
+			part++;
+			calculatePart(true);
+			part++;
+		}
+	}
+
+	private static void calculatePart(boolean straight) {
+		if(straight) {
+			double distance = Math.pow(Math.pow(xChange, 2) + Math.pow(yChange, 2), .5);
+
+			int i = 0;
+			while(segments[i] != null) {
+				i++;
+			}
+
+			double remainingDistance = distance;
+			Jerk accelJerkUp = new Jerk(segments[i].acceleration, config.max_acceleration, segments[i].velocity, (Double) null, remainingDistance, config);
+			remainingDistance = distance - accelJerkUp.getJerkValues()[1];
+			Jerk accelJerkDown;
+			accelJerkDown = new Jerk(config.max_acceleration, 0, (Double) null, config.max_velocity, remainingDistance, config);
+			remainingDistance = remainingDistance - accelJerkDown.getJerkValues()[1];
+			Acceleration acceleration = new Acceleration(accelJerkUp.getJerkValues()[4], accelJerkDown.getJerkValues()[3],config);
+			double totalAccelDist = accelJerkUp.getJerkValues()[1] + acceleration.getAccelerationValues()[1] + accelJerkDown.getJerkValues()[1];
+			if(totalAccelDist > distance) {
+				if(accelJerkUp.getJerkValues()[2] == config.max_acceleration) {
+					acceleration = new Acceleration(totalAccelDist, totalAccelDist, config);
+				}else {
+
+				}
+			}else {
+
+
+
+			}
+
+
+			if(points[waypoint + 2] == null) {
+				if(distance > remainingDistance) {
+					AccelSeconds = (segments[i].velocity - difVelocity) / config.max_acceleration;
+					AccelDist = remainingDistance;
+				}else if(distance == remainingDistance){
+					AccelSeconds = 0;
+					AccelDist = 0;
+				}else {
+					JerkSeconds = 0;
+					JerkDist = 0;
+					AccelSeconds = (segments[i].velocity - 0) / config.max_acceleration;
+					AccelDist = distance;
+				}
+				//segTime[portion] = 
+			}else {
+				if() {
+
+				}else if(){
+
+				}else {
+
+				}
+			}
+
+
+		}else { //curve which is part of circle and two clothoids
+
+		}
+	}
+}

src/Main/Pathfinder.java

@@ -0,0 +1,7 @@
+package Main;
+
+public class Pathfinder {
+	public static Trajectory generate(Waypoint[] waypoints, Trajectory.Config config) {
+		return CalculatePath.generateTrajectory(waypoints, config);
+	}
+}

src/Main/Trajectory.java

@@ -0,0 +1,56 @@
+package Main;
+
+public class Trajectory {
+
+	public static class Config {
+
+        public double dt, max_velocity, max_acceleration, max_jerk;
+
+        /**
+         * Create a Trajectory Configuration
+         * @param dt                    The time delta between points (in seconds)
+         * @param max_velocity          The maximum velocity the body is capable of traveling at (in meters per second)
+         * @param max_acceleration      The maximum acceleration to use (in meters per second per second)
+         * @param max_jerk              The maximum jerk (acceleration per second) to use
+         */
+        public Config(double dt, double max_velocity, double max_acceleration, double max_jerk) {
+            this.dt = dt;
+            this.max_velocity = max_velocity;
+            this.max_acceleration = max_acceleration;
+            this.max_jerk = max_jerk;
+        }
+    }
+
+	public Segment[] segments;
+
+    public Trajectory(Segment[] segments) {
+        this.segments = segments;
+    }
+
+    public Trajectory(int length) {
+        this.segments = new Segment[length];
+    }
+
+    public Segment get(int index) {
+    		return segments[index];
+    }
+
+    public int length() {
+        return segments.length;
+    }
+
+	public static class Segment {
+		public double dt, x, y, position, velocity, acceleration, jerk, heading;
+
+        public Segment(double dt, double x, double y, double position, double velocity, double acceleration, double jerk, double heading) {
+            this.dt = dt;
+            this.x = x;
+            this.y = y;
+            this.position = position;
+            this.velocity = velocity;
+            this.acceleration = acceleration;
+            this.jerk = jerk;
+            this.heading = heading;
+        }
+	}
+}

src/Main/Waypoint.java

@@ -0,0 +1,12 @@
+package Main;
+
+public class Waypoint {
+
+	double x, y, angle;
+
+	public Waypoint(double x, double y, double angle) {
+		this.x = x;
+		this.y = y;
+		this.angle = angle;
+	}
+}

src/PartTypes/Acceleration.java

@@ -0,0 +1,48 @@
+package PartTypes;
+
+import Main.Trajectory;
+
+public class Acceleration {
+
+	double accelSeconds, accelDistance, final_velocity;
+
+	/**
+     * Create Acceleration
+     * @param start_velocity	The velocity before the constant acceleration (in used length units per second)
+     * @param end_velocity	The velocity before the constant acceleration (in used length units per second)		leave "null" if unknown
+     * @param distance_to_be_covered		The distance covered by the acceleration (in used length units)		leave "null" if unknown
+     * @param config			The configuration for the trajectory/path
+     */
+	public Acceleration(double start_velocity, double end_velocity, double distance_to_be_covered, Trajectory.Config config) {
+		double max_acceleration;
+		if(end_velocity > start_velocity) {
+			max_acceleration = config.max_acceleration;
+		}else {
+			max_acceleration = -config.max_acceleration;
+		}
+
+		if(end_velocity == (Double)null) {
+			accelDistance = distance_to_be_covered;
+			final_velocity = (((accelDistance - (1/2) * config.max_acceleration) / start_velocity) * max_acceleration) + start_velocity;
+			accelSeconds = (final_velocity - start_velocity) / max_acceleration;
+		}
+
+		if(distance_to_be_covered == (Double)null) {
+			accelSeconds = (end_velocity - start_velocity) / max_acceleration;
+			accelDistance = start_velocity * accelSeconds + (1/2) * config.max_acceleration;
+			final_velocity = end_velocity;
+		}
+	}
+	/**
+	 * 
+	 * @return<p>accelSeconds [0] The time (in seconds) needed to accelerate</>
+	 * <p>accelDistance [1] The distance covered (in used length units) during this constant acceleration</>
+	 */
+	public double[] getAccelerationValues() {
+		return new double[] {
+				accelSeconds,
+				accelDistance,
+				final_velocity
+		};
+	}
+}

src/PartTypes/Constant.java

@@ -0,0 +1,5 @@
+package PartTypes;
+
+public class Constant {
+
+}

src/PartTypes/Jerk.java

@@ -0,0 +1,67 @@
+package PartTypes;
+
+import java.math.MathContext;
+
+import Main.Trajectory;
+
+public class Jerk {
+
+	double actual_final_acceleration, distance_covered, time_needed, actual_initial_velocity, actual_final_velocity;
+
+	/**
+     * Create Jerk
+     * @param known_initial_acceleration		The initial acceleration of the jerk (in used length units per second squared)
+     * @param expected_final_acceleration		The wanted acceleration after the jerk (in used length units per second squared)
+     * @param known_initial_velocity     		The initial velocity of the jerk (in used length units per second)				leave "null" if unknown
+     * @param known_final_velocity			The final velocity of the jerk (in used length units per second)					leave "null" if unknown
+     * @param part_remaining_distance			The distance left of the part at the beginning of the jerk (in used length units)
+     * @param config							The configuration for the trajectory/path
+     */
+	public Jerk(double known_initial_acceleration, double expected_final_acceleration, double known_initial_velocity, double known_final_velocity, double part_remaining_distance, Trajectory.Config config) {
+		double max_jerk;
+		if(expected_final_acceleration - known_initial_acceleration < 0) {
+			max_jerk = config.max_jerk;
+		}else {
+			max_jerk = -config.max_jerk;
+		}
+
+		double test_jerk_time = (expected_final_acceleration - known_initial_acceleration) / max_jerk;
+		double test_jerk_distance = 	known_initial_velocity * test_jerk_time + (1/2) * known_initial_acceleration * Math.pow(test_jerk_time, 2) + (1/6) * max_jerk * Math.pow(test_jerk_time, 3);
+
+		if(test_jerk_distance > part_remaining_distance) {
+			//time_needed = ;
+		}else {
+			if(known_final_velocity == (Double)null) {
+				time_needed = test_jerk_time;
+				distance_covered = test_jerk_distance;
+				actual_final_acceleration = expected_final_acceleration;
+				actual_initial_velocity = known_initial_velocity;
+				actual_final_velocity = known_initial_velocity + known_initial_acceleration * time_needed + (1/2) * config.max_jerk * Math.pow(time_needed, 2);
+			}else {
+				time_needed = test_jerk_time;
+				distance_covered = test_jerk_distance;
+				actual_final_acceleration = expected_final_acceleration;
+				actual_initial_velocity = known_final_velocity -(known_initial_acceleration * time_needed + (1/2) * config.max_jerk * Math.pow(time_needed, 2));
+				actual_final_velocity = known_final_velocity;
+			}
+		}
+	}
+
+	/**
+	 * 
+	 * @return <p>time_needed	[0] The time required for the jerk portion</>
+	 * <p>distance_covered	[1] The length of the path traversed during the jerk portion</>
+	 * <p>acutal_final_acceleration	[2] The acceleration after the jerk portion</>
+	 * <p>actual_initial_velocity	[3] The velocity before the jerk portion</>
+	 * <p>actual_final_velocity	[4] The velocity after the jerk portion</>
+	 */
+	public double[] getJerkValues() {
+		return new double[] {
+			time_needed,
+			distance_covered,
+			actual_final_acceleration,
+			actual_initial_velocity,
+			actual_final_velocity
+		};
+	}
+}