From 580b49ae5528549b57f990fdb8538b2bc497f0b4 Mon Sep 17 00:00:00 2001 From: JH159753 Date: Sat, 17 Sep 2022 02:45:09 -0700 Subject: [PATCH] Provided framework, no changes --- Boid.pde | 97 ++++++++++++++ CustomMaps.pde | 58 +++++++++ Flocking.pde | 10 ++ KinematicMovement.pde | 67 ++++++++++ Map.pde | 286 ++++++++++++++++++++++++++++++++++++++++++ NavMesh.pde | 43 +++++++ lab1.pde | 182 +++++++++++++++++++++++++++ settings.pde | 37 ++++++ util.pde | 15 +++ 9 files changed, 795 insertions(+) create mode 100644 Boid.pde create mode 100644 CustomMaps.pde create mode 100644 Flocking.pde create mode 100644 KinematicMovement.pde create mode 100644 Map.pde create mode 100644 NavMesh.pde create mode 100644 lab1.pde create mode 100644 settings.pde create mode 100644 util.pde diff --git a/Boid.pde b/Boid.pde new file mode 100644 index 0000000..f911be7 --- /dev/null +++ b/Boid.pde @@ -0,0 +1,97 @@ +/// In this file, you will have to implement seek and waypoint-following +/// The relevant locations are marked with "TODO" + +class Crumb +{ + PVector position; + Crumb(PVector position) + { + this.position = position; + } + void draw() + { + fill(255); + noStroke(); + circle(this.position.x, this.position.y, CRUMB_SIZE); + } +} + +class Boid +{ + Crumb[] crumbs = {}; + int last_crumb; + float acceleration; + float rotational_acceleration; + KinematicMovement kinematic; + PVector target; + + Boid(PVector position, float heading, float max_speed, float max_rotational_speed, float acceleration, float rotational_acceleration) + { + this.kinematic = new KinematicMovement(position, heading, max_speed, max_rotational_speed); + this.last_crumb = millis(); + this.acceleration = acceleration; + this.rotational_acceleration = rotational_acceleration; + } + + void update(float dt) + { + if (target != null) + { + // TODO: Implement seek here + } + + // place crumbs, do not change + if (LEAVE_CRUMBS && (millis() - this.last_crumb > CRUMB_INTERVAL)) + { + this.last_crumb = millis(); + this.crumbs = (Crumb[])append(this.crumbs, new Crumb(this.kinematic.position)); + if (this.crumbs.length > MAX_CRUMBS) + this.crumbs = (Crumb[])subset(this.crumbs, 1); + } + + // do not change + this.kinematic.update(dt); + + draw(); + } + + void draw() + { + for (Crumb c : this.crumbs) + { + c.draw(); + } + + fill(255); + noStroke(); + float x = kinematic.position.x; + float y = kinematic.position.y; + float r = kinematic.heading; + circle(x, y, BOID_SIZE); + // front + float xp = x + BOID_SIZE*cos(r); + float yp = y + BOID_SIZE*sin(r); + + // left + float x1p = x - (BOID_SIZE/2)*sin(r); + float y1p = y + (BOID_SIZE/2)*cos(r); + + // right + float x2p = x + (BOID_SIZE/2)*sin(r); + float y2p = y - (BOID_SIZE/2)*cos(r); + triangle(xp, yp, x1p, y1p, x2p, y2p); + } + + void seek(PVector target) + { + this.target = target; + + } + + void follow(ArrayList waypoints) + { + // TODO: change to follow *all* waypoints + this.target = waypoints.get(0); + + } +} diff --git a/CustomMaps.pde b/CustomMaps.pde new file mode 100644 index 0000000..31a8979 --- /dev/null +++ b/CustomMaps.pde @@ -0,0 +1,58 @@ +// These are custom maps (for part b), defined as lists of coordinates for the outline +// You can add additional maps as you see fit +PVector[] customMap(int nr) +{ + if (nr == 1) + { + return new PVector[] {new PVector(0, 1), new PVector(0.8, 1), new PVector(0.55, 0.7), new PVector(0.75, 0.1), + new PVector(1, 0.9), new PVector(1, 0), new PVector(0.25, 0), new PVector(0.25, 0.15), new PVector(0, 0.15)}; + } + else if (nr == 2) + { + return new PVector[] {new PVector(0, 1), new PVector(0.25, 1), new PVector(0.4, 0.75), new PVector(0.45, 0.5), + new PVector(0.1, 0.5), new PVector(0.12, 0.35), new PVector(0.25, 0.35), new PVector(0.5, 0.4), + new PVector(0.5, 0.65), new PVector(0.6, 0.65), new PVector(0.6, 0.8), new PVector(0.5, 0.82), + new PVector(0.3, 1), new PVector(1,1), new PVector(1,0), new PVector(0.57, 0), new PVector(0.6, 0.35), + new PVector(0.7, 0.35), new PVector(0.67, 0.1), new PVector(0.85, 0.1), new PVector(0.82,0.15), + new PVector(0.75, 0.2), new PVector(0.75, 0.75), new PVector(0.85, 0.92), new PVector(0.62, 0.9), + new PVector(0.7, 0.8), new PVector(0.7, 0.5), new PVector(0.58, 0.45), new PVector(0.45,0), + new PVector(0.25, 0), new PVector(0.25, 0.15), new PVector(0, 0.15)}; + } + else if (nr == 3) + { + return new PVector[] {new PVector(0, 1), new PVector(0.45, 1), new PVector(0.45, 0.6), new PVector(0.1, 0.6), + new PVector(0.2, 0.23), new PVector(0.35, 0.25), new PVector(0.35, 0.35), new PVector(0.25, 0.35), + new PVector(0.25, 0.3), new PVector(0.2, 0.3), new PVector(0.2, 0.55), new PVector(0.3, 0.55), + new PVector(0.3, 0.46), new PVector(0.4, 0.45), new PVector(0.4, 0.55), new PVector(0.6, 0.62), + new PVector(0.6, 0.85), new PVector(0.5, 0.85), new PVector(0.5, 1), new PVector(1,1), + new PVector(1,0.9), new PVector(0.7, 0.9), new PVector(0.7, 0.62), + new PVector(0.8, 0.75), new PVector(0.8, 0.85), new PVector(1, 0.85), + new PVector(1,0), new PVector(0.9, 0), new PVector(0.93, 0.13), new PVector(0.87, 0.1), + new PVector(0.85, 0.05), new PVector(0.75, 0.05), new PVector(0.75, 0.15), new PVector(0.8, 0.15), + new PVector(0.8, 0.25), new PVector(0.95,0.25), new PVector(0.92, 0.55), new PVector(0.75, 0.55), + new PVector(0.75, 0.45), new PVector(0.85, 0.40), new PVector(0.8, 0.5), new PVector(0.9, 0.5), + new PVector(0.9, 0.35), new PVector(0.67, 0.35), new PVector(0.67, 0.2), new PVector(0.5, 0.2), + new PVector(0.5, 0.35), new PVector(0.65, 0.45), new PVector(0.45, 0.47), + new PVector(0.45, 0.1), new PVector(0.7, 0.15), new PVector(0.7, 0), new PVector(0.25, 0), + new PVector(0.25, 0.15), new PVector(0, 0.15) + }; + } + else if (nr == 4) + { + return new PVector[] {new PVector(0, 1), new PVector(0.85, 1), new PVector(0.87, 0.15), new PVector(0.45, 0.15), new PVector(0.35, 0.3), + new PVector(0.15, 0.3), new PVector(0.15, 0.55), new PVector(0.5, 0.55), new PVector(0.57, 0.42), new PVector(0.45, 0.75), new PVector(0.6, 0.8), + new PVector(0.6, 0.6), new PVector(0.65, 0.35), new PVector(0.55, 0.35), new PVector(0.47, 0.45), new PVector(0.3, 0.45), + new PVector(0.32, 0.37), new PVector(0.45, 0.4), new PVector(0.5, 0.27), new PVector(0.7, 0.3), new PVector(0.7, 0.85), + new PVector(0.4, 0.8), new PVector(0.4, 0.65), new PVector(0.13, 0.65), new PVector(0.1, 0.27), new PVector(0.35, 0.2), + new PVector(0.45, 0.12), new PVector(0.9, 0.1), new PVector(0.9, 1), new PVector(1,1), new PVector(1,0), new PVector(0.25, 0), + new PVector(0.25, 0.15), new PVector(0, 0.15) + }; + + } + else /// you can use nr==5, nr==6, ... nr==9 to add your own custom maps + { + return new PVector[] {new PVector(0, 1), new PVector(0.8, 1), new PVector(0.55, 0.7), new PVector(0.75, 0.1), + new PVector(1, 0.9), new PVector(1, 0), new PVector(0.25, 0), new PVector(0.25, 0.15), new PVector(0, 0.15)}; + } + +} diff --git a/Flocking.pde b/Flocking.pde new file mode 100644 index 0000000..244f6e5 --- /dev/null +++ b/Flocking.pde @@ -0,0 +1,10 @@ + +/// called when "f" is pressed; should instantiate additional boids and start flocking +void flock() +{ +} + +/// called when "f" is pressed again; should remove the flock +void unflock() +{ +} diff --git a/KinematicMovement.pde b/KinematicMovement.pde new file mode 100644 index 0000000..2058878 --- /dev/null +++ b/KinematicMovement.pde @@ -0,0 +1,67 @@ +/// Do not change this file! +/// The only methods you can call are increaseSpeed, getPosition, getHeading, getSpeed and getRotationalVelocity + +class KinematicMovement +{ + // position + private PVector position; + private float heading; + + private float speed; + private float rotational_velocity; + + float max_speed; + float max_rotational_speed; + KinematicMovement(PVector position, float heading, float max_speed, float max_rotational_speed) + { + this.position = position; + this.heading = heading; + this.speed = 0; + this.rotational_velocity = 0; + this.max_speed = max_speed; + this.max_rotational_speed = max_rotational_speed; + } + void update(float dt) + { + PVector velocity = new PVector(cos(this.heading), sin(this.heading)).mult(speed); + + PVector destination = PVector.add(this.position, PVector.mult(velocity, dt)); + // check for map collisions; only move if no collisions + if (!map.collides(this.position, destination)) + this.position = destination; + this.heading += this.rotational_velocity*dt; + this.heading = normalize_angle(this.heading); + } + private void setSpeed(float s, float rs) + { + this.speed = constrain(s, -max_speed, max_speed); + this.rotational_velocity = constrain(rs, -this.max_rotational_speed, this.max_rotational_speed); + } + + // These are the public methods + void increaseSpeed(float ds, float drs) + { + setSpeed(this.speed + ds, this.rotational_velocity + drs); + } + + PVector getPosition() + { + return position; + } + + float getHeading() + { + return heading; + } + + float getSpeed() + { + return speed; + } + + float getRotationalVelocity() + { + return rotational_velocity; + } + // End public methods +} diff --git a/Map.pde b/Map.pde new file mode 100644 index 0000000..000fed4 --- /dev/null +++ b/Map.pde @@ -0,0 +1,286 @@ +/// You do not have to change this file, but you can, if you want to add a more sophisticated generator. +class Wall +{ + PVector start; + PVector end; + PVector normal; + PVector direction; + float len; + + Wall(PVector start, PVector end) + { + this.start = start; + this.end = end; + direction = PVector.sub(this.end, this.start); + len = direction.mag(); + direction.normalize(); + normal = new PVector(-direction.y, direction.x); + } + + + boolean crosses(PVector from, PVector to) + { + // Vector pointing from `this.start` to `from` + PVector d1 = PVector.sub(from, this.start); + // Vector pointing from `this.start` to `to` + PVector d2 = PVector.sub(to, this.start); + // If both vectors are on the same side of the wall + // their dot products with the normal will have the same sign + // If they are both positive, or both negative, their product will + // be positive. + float dist1 = normal.dot(d1); + float dist2 = normal.dot(d2); + if (dist1 * dist2 > 0) return false; + + // if the start and end are on different sides, we need to determine + // how far the intersection point is along the wall + // first we determine how far the projections of from and to are + // along the wall + float ldist1 = direction.dot(d1); + float ldist2 = direction.dot(d2); + + // the distance of the intersection point from the start + // is proportional to the normal distance of `from` in + // along the total movement + float t = dist1/(dist1 - dist2); + + // calculate the intersection as this proportion + float intersection = ldist1 + t*(ldist2 - ldist1); + if (intersection < 0 || intersection > len) return false; + return true; + } + + // Return the mid-point of this wall + PVector center() + { + return PVector.mult(PVector.add(start, end), 0.5); + } + + void draw() + { + strokeWeight(3); + line(start.x, start.y, end.x, end.y); + if (SHOW_WALL_DIRECTION) + { + PVector marker = PVector.add(PVector.mult(start, 0.2), PVector.mult(end, 0.8)); + circle(marker.x, marker.y, 5); + } + } +} + +void AddPolygon(ArrayList walls, PVector[] nodes) +{ + for (int i = 0; i < nodes.length; ++ i) + { + int next = (i+1)%nodes.length; + walls.add(new Wall(nodes[i], nodes[next])); + } +} + +void AddPolygonScaled(ArrayList walls, PVector[] nodes) +{ + for (int i = 0; i < nodes.length; ++ i) + { + int next = (i+1)%nodes.length; + walls.add(new Wall(new PVector(nodes[i].x*width, nodes[i].y*height), new PVector(nodes[next].x*width, nodes[next].y*height))); + } +} + +class Obstacle +{ + ArrayList walls; + Obstacle() + { + walls = new ArrayList(); + PVector origin = new PVector(width*0.1 + random(width*0.65), height*0.12 + random(height*0.65)); + if (origin.x < 100 && origin.y > 500) origin.add(new PVector(150,0)); + PVector[] nodes = new PVector[] {}; + float angle = random(100)/100.0; + for (int i = 0; i < 3 + random(2) && angle < TAU; ++i) + { + float distance = height*0.05 + random(height*0.15); + nodes = (PVector[])append(nodes, PVector.add(origin, new PVector(cos(-angle)*distance, sin(-angle)*distance))); + angle += 1 + random(25)/50; + } + AddPolygon(walls, nodes); + } + + +} + +// Given a (closed!) polygon surrounded by walls, tests if the +// given point is inside that polygon. +// Note that this only works for polygons that are inside the +// visible screen (or not too far outside) +boolean isPointInPolygon(PVector point, ArrayList walls) +{ + // we create a test point "far away" horizontally + PVector testpoint = PVector.add(point, new PVector(width*2, 0)); + + // Then we count how often the line from the given point + // to our test point intersects the polygon outline + int count = 0; + for (Wall w: walls) + { + if (w.crosses(point, testpoint)) + count += 1; + } + + // If we cross an odd number of times, we started inside + // otherwise we started outside the polygon + // Intersections alternate between enter and exit, + // so if we "know" that the testpoint is outside + // and odd number means we exited one more time + // than we entered. + return (count%2) == 1; +} + +class Map +{ + ArrayList walls; + ArrayList obstacles; + ArrayList outline; + + ArrayList pts; + + Map() + { + walls = new ArrayList(); + outline = new ArrayList(); + obstacles = new ArrayList(); + } + + boolean collides(PVector from, PVector to) + { + for (Wall w : walls) + { + if (w.crosses(from, to)) return true; + } + return false; + } + + void doSplit(boolean xdir, float from, float to, float other, float otherend, ArrayList points, int level) + { + float range = abs(to-from); + float sign = -1; + if (to > from) sign = 1; + if (range < 70) return; + if (level > 1 && random(0,1) < 0.05*level) return; + float split = from + sign*random(range*0.35, range*0.45); + float splitend = split + sign*random(20, range*0.35-10); + if (xdir) + points.add(new PVector(split, other)); + else + points.add(new PVector(other, split)); + float othersign = 1; + if (otherend < other) othersign = -1; + float otherrange = abs(other-otherend); + float spikeend = other + othersign*random(otherrange*0.4, otherrange*(0.9 - 0.1*level)); + doSplit(!xdir, other, spikeend, split, from, points, level+1); + if (xdir) + { + points.add(new PVector(split, spikeend)); + points.add(new PVector(splitend, spikeend)); + } + else + { + points.add(new PVector(spikeend, split)); + points.add(new PVector(spikeend, splitend)); + } + doSplit(!xdir, spikeend, other, splitend, to, points, level + 1); + + if (xdir) + points.add(new PVector(splitend, other)); + else + points.add(new PVector(other, splitend)); + } + + void randomMap() + { + ArrayList points = new ArrayList(); + + points.add(new PVector(0, height)); + doSplit(true, 50, width, height, 0, points, 0); + points.add(new PVector(width, height)); + points.add(new PVector(width, 0)); + points.add(new PVector(200, 0)); + points.add(new PVector(200, 80)); + points.add(new PVector(0, 80)); + + //pts = points; + AddPolygon(outline, points.toArray(new PVector[]{})); + } + + + void generate(int which) + { + outline.clear(); + obstacles.clear(); + walls.clear(); + if (which < 0) + { + randomMap(); + } + else if (which == 0) + { + AddPolygon(outline, new PVector[] {new PVector(-100, height+100), new PVector(width+100, height+100), new PVector(width+100, -100), new PVector(-100,-100)}); + } + else + { + AddPolygonScaled(outline, customMap(which)); + } + walls.addAll(outline); + for (int i = 0; i < random(MAX_OBSTACLES); ++i) + { + Obstacle obst = new Obstacle(); + boolean ok = true; + // only obstacle if it doesn't intersect with any existing one (or the exterior) + for (Wall w : obst.walls) + { + if (collides(w.start, w.end)) ok = false; + if (!isReachable(w.start)) ok = false; + } + if (ok) + { + obstacles.add(obst); + walls.addAll(obst.walls); + } + } + } + + void update(float dt) + { + draw(); + } + + void draw() + { + stroke(255); + strokeWeight(3); + for (Wall w : walls) + { + w.draw(); + } + if (pts != null) + { + PVector current = new PVector(width/2, height/2); + for (PVector p : pts) + { + fill(255,0,0); + circle(p.x, p.y, 4); + line(current.x, current.y, p.x, p.y); + current = p; + } + } + } + + boolean isReachable(PVector point) + { + if (!isPointInPolygon(point, outline)) return false; + for (Obstacle o: obstacles) + { + if (isPointInPolygon(point, o.walls)) return false; + } + return true; + } +} diff --git a/NavMesh.pde b/NavMesh.pde new file mode 100644 index 0000000..d153a5d --- /dev/null +++ b/NavMesh.pde @@ -0,0 +1,43 @@ +// Useful to sort lists by a custom key +import java.util.Comparator; + + +/// In this file you will implement your navmesh and pathfinding. + +/// This node representation is just a suggestion +class Node +{ + int id; + ArrayList polygon; + PVector center; + ArrayList neighbors; + ArrayList connections; +} + + + +class NavMesh +{ + void bake(Map map) + { + /// generate the graph you need for pathfinding + } + + ArrayList findPath(PVector start, PVector destination) + { + /// implement A* to find a path + ArrayList result = null; + return result; + } + + + void update(float dt) + { + draw(); + } + + void draw() + { + /// use this to draw the nav mesh graph + } +} diff --git a/lab1.pde b/lab1.pde new file mode 100644 index 0000000..d0e5f76 --- /dev/null +++ b/lab1.pde @@ -0,0 +1,182 @@ +/// You do not need to change anything in this file, but you can +/// For example, if you want to add additional options controllable by keys +/// keyPressed would be the place for that. + +ArrayList waypoints = new ArrayList(); +Boid billy; +int lastt; + +int mapnr = 0; + +Map map = new Map(); +NavMesh nm = new NavMesh(); + +boolean entering_path = false; + +boolean show_nav_mesh = false; + +boolean show_waypoints = false; + +boolean show_help = false; + +boolean flocking_enabled = false; + +void setup() { + size(800, 600); + + billy = new Boid(BILLY_START, BILLY_START_HEADING, BILLY_MAX_SPEED, BILLY_MAX_ROTATIONAL_SPEED, BILLY_MAX_ACCELERATION, BILLY_MAX_ROTATIONAL_ACCELERATION); + randomSeed(0); + map.generate(mapnr); + nm.bake(map); +} + +void mousePressed() { + if (show_help) return; + PVector target = new PVector(mouseX, mouseY); + if (!map.isReachable(target)) return; + if (mouseButton == LEFT) + { + + if (waypoints.size() == 0) + { + billy.seek(target); + } + else + { + waypoints.add(target); + entering_path = false; + billy.follow(waypoints); + } + } + else if (mouseButton == RIGHT) + { + if (!entering_path) + waypoints = new ArrayList(); + waypoints.add(target); + entering_path = true; + } +} + +void keyPressed() +{ + if (show_help) + { + show_help = false; + return; + } + if (key == 'h') + { + show_help = true; + } + + if (show_help) return; + if (key == 'g') + { + map.generate(-1); + mapnr = -1; + nm.bake(map); + } + else if (key == 'n') + { + show_nav_mesh = !show_nav_mesh; + } + else if (key == 'w') + { + show_waypoints = !show_waypoints; + } + else if ((key >= '1' && key <= '9')) + { + mapnr = key-'1' + 1; + map.generate(mapnr); + + nm.bake(map); + } + else if (key == '0') + { + mapnr = 0; + map.generate(0); + nm.bake(map); + } + else if (key == 'f') + { + flocking_enabled = !flocking_enabled; + if (flocking_enabled) + { + flock(); + } + else + { + unflock(); + } + } +} + +void show_status(boolean active, String show, int x) +{ + fill(255,255,255); + if (active) + fill(255,0,0); + text(show, x, 40); +} + +void draw() { + background(0); + + if (entering_path || show_waypoints) + { + stroke(255,0,0); + strokeWeight(1); + PVector current = billy.kinematic.position; + if (show_waypoints && billy.target != null) + { + line(current.x, current.y, billy.target.x, billy.target.y); + current = billy.target; + } + for (PVector wp : waypoints) + { + line(current.x, current.y, wp.x, wp.y); + current = wp; + } + if (entering_path) + line(current.x, current.y, mouseX, mouseY); + } + + + float dt = (millis() - lastt)/1000.0; + lastt = millis(); + billy.update(dt); + map.update(dt); + if (show_nav_mesh) + nm.update(dt); + textSize(12); + show_status(show_nav_mesh, "N", 30); + show_status(show_waypoints, "W", 50); + show_status(show_help, "H", 70); + show_status(flocking_enabled, "F", 90); + if (mapnr < 0) + show_status(false, "R", 110); + else + show_status(false, String.format("%d", mapnr), 110); + + if (show_help) + { + fill(255); + stroke(0,0,255); + rect(width*0.25, height*0.25, width*0.5, height*0.5); + fill(0); + textSize(32); + text("HELP", width*0.5-30, height*0.25 + 40); + textSize(18); + text("0,1,2,3,4 - Show custom map 0,1,2,3,4", width*0.25+40, height*0.25 + 70); + text("G - Generate random map", width*0.25+40, height*0.25 + 90); + text("N - Show NavMesh", width*0.25+40, height*0.25 + 110); + text("W - Show waypoints while moving", width*0.25+40, height*0.25 + 130); + text("F - Enable/disable flocking", width*0.25+40, height*0.25 + 150); + text("H - This screen", width*0.25+40, height*0.25 + 170); + + text("Press any key to close", width*0.5 - 80, height*0.75 - 80); + textSize(12); + } + + +} diff --git a/settings.pde b/settings.pde new file mode 100644 index 0000000..4b3163b --- /dev/null +++ b/settings.pde @@ -0,0 +1,37 @@ +// Currently not used, but you may want to use it to enable/disable +// draw-calls or debug output as needed +boolean DEBUG = false; + +// The radius of the circle representing the boid body +int BOID_SIZE = 20; + +// Where does billy start? +PVector BILLY_START = new PVector(50,500); +float BILLY_START_HEADING = 0; + +// How fast can billy go and turn? +float BILLY_MAX_SPEED = 80; +float BILLY_MAX_ROTATIONAL_SPEED = 3; + +float BILLY_MAX_ACCELERATION = 1; +float BILLY_MAX_ROTATIONAL_ACCELERATION = 1; + +// Should boids leave breadcrumbs behind? +boolean LEAVE_CRUMBS = true; + +// How many crumbs? +int MAX_CRUMBS = 1000; + +// Time between crumbs +int CRUMB_INTERVAL = 200; + +// How big are the crumbs? +int CRUMB_SIZE = 2; + +// use for debugging, if you want to see where walls start/end (a circle is drawn closer to the end) +boolean SHOW_WALL_DIRECTION = false; + +// How many obstacles should be generated *at most* +// Note that maps 2-4 are pretty dense and obstacles +// are only placed if they won't intersect with the map +int MAX_OBSTACLES = 0; diff --git a/util.pde b/util.pde new file mode 100644 index 0000000..125a445 --- /dev/null +++ b/util.pde @@ -0,0 +1,15 @@ +// Normalize an angle to be between 0 and TAU (= 2 PI) +float normalize_angle(float angle) +{ + while (angle < 0) angle += TAU; + while (angle > TAU) angle -= TAU; + return angle; +} + +// Normalize an angle to be between -PI and PI +float normalize_angle_left_right(float angle) +{ + while (angle < -PI) angle += TAU; + while (angle > PI) angle -= TAU; + return angle; +}