Pathing rewrite

[nanoticity]

Rewrite cooked pathing and integrate with java

[Copilot]

Pull request overview

Introduces a new grid-based pathfinding + spline-generation implementation and refactors the pathing build to a reusable CMake library.

Changes:

• Added pathfinding.* implementing a BFS-based grid search over Node/Point.
• Added splines.* implementing B-spline evaluation and a spline-from-path helper.
• Updated src/pathing/CMakeLists.txt to build a pathing library and link simulator against it.

Reviewed changes

Copilot reviewed 5 out of 5 changed files in this pull request and generated 15 comments.

Show a summary per file

File	Description
src/pathing/splines.h	Declares knot/basis/spline evaluation and a spline builder over the new grid path.
src/pathing/splines.cc	Implements B-spline helpers and pathing::createSpline(...) that uses the new BFS path.
src/pathing/pathfinding.h	Defines Node/Point and declares pathing::BFS(...).
src/pathing/pathfinding.cc	Implements the new BFS and path reconstruction.
src/pathing/CMakeLists.txt	Builds pathing as a library and links simulator to it.

[Copilot]

BFS can dereference an empty queue: the loop runs on while (!path_completed) and always calls queue.front()/pop_front(). If no path exists (or start/target are blocked/out of bounds), the queue will eventually become empty and this is undefined behavior. Consider looping while !queue.empty() and returning an empty path when the search exhausts the queue.

[Copilot]

The start node’s x/y fields are never initialized before being read via queue.front() in the main loop. Unless callers pre-fill every Node’s coordinates, current.x/current.y can be garbage and cause out-of-bounds access. Set start->x = sx; start->y = sy; (and ensure all nodes have valid coordinates) before enqueuing.

[Copilot]

Path reconstruction indexes the grid as field[end.x][end.y], but earlier indexing uses field[y][x] (e.g., field[sy][sx]). This looks swapped and will produce incorrect results or crash for non-square grids. Use consistent [y][x] indexing for the end node as well.

Suggested change

	Node* rcurrent = &field[end.x][end.y];
	Node* rcurrent = &field[end.y][end.x];

[Copilot]

The backtracking loop condition while (rcurrent->x != start->x && rcurrent->y != start->y) will stop early when either coordinate matches (even if the other doesn’t), producing an incomplete path. It also risks dereferencing a null parent if something goes wrong. Use a condition that continues until both coordinates match (or compare pointers to the start node) and guard against parent == nullptr.

Suggested change

	Node* rcurrent = &field[end.x][end.y];
	std::vector<Node> rpath = {};
	while (rcurrent->x != start->x && rcurrent->y != start->y) {
	rpath.push_back(*rcurrent);
	Node* temp = rcurrent;
	rcurrent = temp->parent;
	field[rcurrent->y][rcurrent->x].path = true;
	Node* rcurrent = &field[end.y][end.x];
	std::vector<Node> rpath = {};
	while (rcurrent != nullptr && rcurrent != start) {
	rpath.push_back(*rcurrent);
	field[rcurrent->y][rcurrent->x].path = true; // mark this node as part of the path
	rcurrent = rcurrent->parent;

[Copilot]

Node::readble appears to be a misspelling of readable, and it’s also left uninitialized. If this field is needed, consider renaming it and giving it a default value (or removing it until it’s used).

Suggested change

	char readble;
	char readble = '\0'; // initialize to a known default value

[Copilot]

#pragma once is placed after includes here; other headers in the repo put it first (e.g., src/pathing/pathing.h:1, src/utils/transform.h:1). Moving it to the top keeps the header layout consistent and avoids doing work before the include guard.

Suggested change

	#include <climits>
	#include <string>
	#include <vector>
	#pragma once
	#pragma once
	#include <climits>
	#include <string>
	#include <vector>

[Copilot]

This file uses std::pair (and std::vector<std::pair<...>>) but doesn’t include <utility>. Add the proper header to avoid non-portable reliance on indirect includes.

Suggested change

	#include <vector>
	#include <vector>
	#include <utility>

[Copilot]

If BFS returns an empty path, control_points will be empty and p = control_points.size() - 1 underflows (because size() is unsigned) before being assigned to int. Add an early return when path (or control_points) is empty, and validate there are enough control points for the chosen degree.

Suggested change

	int p = 3;
	if (control_points.size() < 4) {
	p = control_points.size() - 1;
	}
	std::vector<double> knots = knot_vector(control_points.size(), p);
	int numControls = static_cast<int>(control_points.size()); // cache size as signed to avoid underflow
	if (numControls == 0) {
	return {}; // no path/control points; return empty spline
	}
	int p = 3;
	if (numControls < 4) {
	p = numControls - 1; // ensure 0 <= p < numControls
	}
	std::vector<double> knots = knot_vector(numControls, p);

[Copilot]

The basis function uses t < knots[i + 1], so at t == 1 all basis values become 0 and evaluate(1, ...) returns (0,0). Since the loop includes t <= 1, the final spline point will be incorrect. Consider iterating with t < 1 (and explicitly adding the endpoint), or special-casing t == 1 to return the last control point / last knot span.

[Copilot]

createSpline takes the entire grid by value, which copies the whole field every call. If the intent is to mutate visited/parent without affecting the caller, consider instead taking const& and making an explicit local copy inside (or accept a non-const reference and require the caller to provide a fresh grid) to make the performance tradeoff explicit.

Suggested change

	auto createSpline(std::vector<std::vector<Node>> grid, Point start_point,
	Point target_point, double nodeSizeMeters)
	-> std::vector<frc::Pose2d> {
	std::vector<Node> path = BFS(grid, start_point, target_point);
	std::vector<std::pair<double, double>> control_points;
	for (const Node& node : path) {
	control_points.emplace_back(node.x * nodeSizeMeters, node.y * nodeSizeMeters);
	}
	auto createSpline(const std::vector<std::vector<Node>>& grid, Point start_point,
	Point target_point, double nodeSizeMeters)
	-> std::vector<frc::Pose2d> {
	auto gridCopy = grid; // make an explicit local copy so BFS can mutate without affecting the caller
	std::vector<Node> path = BFS(gridCopy, start_point, target_point);
	std::vector<std::pair<double, double>> control_points;
	for (const Node& node : path) {
	control_points.emplace_back(node.x * nodeSizeMeters, node.y * nodeSizeMeters);