diff --git a/Boid.pde b/Boid.pde index aaba96e..e7eec17 100644 --- a/Boid.pde +++ b/Boid.pde @@ -1,6 +1,6 @@ /// In this file, you will have to implement seek and waypoint-following /// The relevant locations are marked with "TODO" - +import java.util.*; class Crumb { PVector position; @@ -18,6 +18,61 @@ class Crumb class Boid { +<<<<<<< HEAD + 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 (waypoints != null) { + for (int i = 0; i= .1) { + //println("positive angle"); + kinematic.increaseSpeed(0.0, 2); + +======= Crumb[] crumbs = {}; int last_crumb; float acceleration; @@ -36,6 +91,7 @@ class Boid this.acceleration = acceleration; this.rotational_acceleration = rotational_acceleration; } +>>>>>>> 3d16b646a807cb7a2384072f4c267c5888644f96 void update(float dt) { @@ -86,6 +142,39 @@ class Boid kinematic.increaseSpeed(0.0, kinematic.getRotationalVelocity()); } } +<<<<<<< HEAD + } + + + + //Sometimes our Boid just goes and does weird things and I don't know why + + //if the target is outside its arrival threshold, accelerate. + //if the target is inside its arrival threshold, accelerate backwards until the speed is 0. + if (direction.mag() > arrivalThreshold) { + //println("main if"); + kinematic.increaseSpeed(.5, 0); + } else if (direction.mag() < arrivalThreshold) { + //Need more specific code here to handle arrivals correctly + + if (kinematic.getSpeed() < 40 && direction.mag() > 30) { + //println("if 1"); + kinematic.increaseSpeed(1, 0); + } else if (kinematic.getSpeed() < 20 && direction.mag() > 15) { + //println("if .75"); + kinematic.increaseSpeed(.75, 0); + } else if (kinematic.getSpeed() < 10 && direction.mag() > 5) { + //println("if .5"); + kinematic.increaseSpeed(.5, 0); + } else if (kinematic.getSpeed() < 5 && direction.mag() < 5) { + //println("if -kin"); + //This should ensure that the boid's speed can be dropped to exactly 0 so we don't have stuttering + + kinematic.increaseSpeed(-kinematic.getSpeed(), 0); + } else { + //println("else"); + kinematic.increaseSpeed(-1, 0); +======= @@ -207,6 +296,7 @@ class Boid } +>>>>>>> 3d16b646a807cb7a2384072f4c267c5888644f96 } @@ -251,6 +341,64 @@ class Boid } +<<<<<<< HEAD + // //println("func count " + count); + // if(count > waypoints.size() - 1){ + // this.target = waypoints.get(0); + // return; + // } + // else { + // // TODO: change to follow *all* waypoints + // println("count " + count); + // this.target = waypoints.get(count); + // PVector temp = waypoints.remove(count); + // count++; + // //count--; + + // follow(waypoints); + // } + + //} + void follow(ArrayList waypoints) + { + if(waypoints.size() == 0) return; + println("vector " + waypoints); + println("reverse vector " + waypoints); + int count = 0; + PVector stop = waypoints.get(0); + this.seek(stop); + + + + + PVector temp = waypoints.remove(0); + println("temp vector " + waypoints); + //follow(waypoints); + + //this.target = waypoints.get(0); + //do{ + + + // println("in while " + count); + ////this.target = waypoints.get(count); + //this.target = waypoints.get(count); + //if(PVector.sub(this.target,this.kinematic.position).mag() < 40){ + // count++; + //} + + + //}while(count < waypoints.size()); + //count++; + //for(int i = 1; i < waypoints.size(); i++){ + // println("dist " + PVector.sub(this.target,this.kinematic.position).mag()); + // if(PVector.sub(this.target,this.kinematic.position).mag() < 40){ + // this.seek(waypoints.get(i)); + // this.target = waypoints.get(i); + // } + + } + +======= // place crumbs, do not change if (LEAVE_CRUMBS && (millis() - this.last_crumb > CRUMB_INTERVAL)) { @@ -313,4 +461,5 @@ class Boid } +>>>>>>> 3d16b646a807cb7a2384072f4c267c5888644f96 } diff --git a/Boid10308162985695495767.autosave b/Boid10308162985695495767.autosave new file mode 100644 index 0000000..f3b2a3b --- /dev/null +++ b/Boid10308162985695495767.autosave @@ -0,0 +1,99 @@ +/// 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 + print(kinematic.getHeading()); + + } + + // 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/Boid1037558311854801587.autosave b/Boid1037558311854801587.autosave new file mode 100644 index 0000000..2fe67df --- /dev/null +++ b/Boid1037558311854801587.autosave @@ -0,0 +1,189 @@ +/// 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 + + + //This makes a vector with the direction our boid needs to go to + PVector direction = PVector.sub(target, kinematic.position); + + //atan2(direction.y, direction.x) will return the direction we need to go in radians + + //print direction we need to go and the direction we are facing right now + //println(atan2(direction.y, direction.x) + " " + normalize_angle_left_right(kinematic.getHeading())); + + float directionalThreshold = .1; + float angleToTarget = normalize_angle_left_right(atan2(direction.y, direction.x) - normalize_angle_left_right(kinematic.getHeading())); + float arrivalThreshold = 60.0; + + //This just draws a circle for visual debugging purposes + circle(target.x, target.y, 3); + + //prints the angle to the target + //println(angleToTarget); + + //if the angle is larger than the threshold in the positive direction, rotate counterclockwise + if (angleToTarget >= .1) { + println("positive angle"); + kinematic.increaseSpeed(0.0, 2); + + //if the angle is smaller than the threshold in the negative direction, rotate clockwise + } else if (angleToTarget < -.1) { + kinematic.increaseSpeed(0.0, -1); + + //if the angle is within our threshold, stop our rotational velocity by rotating opposite + } else if (directionalThreshold > angleToTarget) { + + if (kinematic.getRotationalVelocity() > 0) { + kinematic.increaseSpeed(0.0, -1); + } else if (kinematic.getRotationalVelocity() < 0) { + kinematic.increaseSpeed(0.0, 1); + } + + + + + //Sometimes our Boid just goes and does weird things and I don't know why + + //if the target is outside its arrival threshold, accelerate. + //if the target is inside its arrival threshold, accelerate backwards until the speed is 0. + if (direction.mag() > arrivalThreshold) { + kinematic.increaseSpeed(1,0); + } else if (direction.mag() < arrivalThreshold) { + //Need more specific code here to handle arrivals correctly + + if (kinematic.getSpeed() < 40 && direction.mag() > 30) { + kinematic.increaseSpeed(1,0); + } else if (kinematic.getSpeed() < 20 && direction.mag() > 15) { + kinematic.increaseSpeed(.75,0); + } else if (kinematic.getSpeed() < 10 && direction.mag() > 5) { + kinematic.increaseSpeed(.5,0); + } else if (kinematic.getSpeed() < 5 && direction.mag() < 5) { + //This should ensure that the boid's speed can be dropped to exactly 0 so we don't have stuttering + kinematic.increaseSpeed(-kinematic.getSpeed(),0); + } else { + kinematic.increaseSpeed(-1,0); + } + + + + } + } + + + + //drawing a line for testing purposes + //line(kinematic.position.x, kinematic.position.y, kinematic.position.x + direction.x, kinematic.position.y + direction.y); + } + + // 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; + } +int count = 0; + + //void follow(ArrayList waypoints) + //{ + + // //println("func count " + count); + // if(count > waypoints.size() - 1){ + // this.target = waypoints.get(0); + // return; + // } + // else { + // // TODO: change to follow *all* waypoints + // println("count " + count); + // this.target = waypoints.get(count); + // PVector temp = waypoints.remove(count); + // count++; + // //count--; + + // follow(waypoints); + // } + + //} + void follow(ArrayList waypoints) + { + this.target = waypoints.get(0); + + + } +} diff --git a/Boid10419224425232634497.autosave b/Boid10419224425232634497.autosave new file mode 100644 index 0000000..73e7fb9 --- /dev/null +++ b/Boid10419224425232634497.autosave @@ -0,0 +1,175 @@ +/// 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 + + + //This makes a vector with the direction our boid needs to go to + PVector direction = PVector.sub(target, kinematic.position); + + //atan2(direction.y, direction.x) will return the direction we need to go in radians + + //print direction we need to go and the direction we are facing right now + //println(atan2(direction.y, direction.x) + " " + normalize_angle_left_right(kinematic.getHeading())); + + float directionalThreshold = .1; + float angleToTarget = normalize_angle(atan2(direction.y, direction.x)) - kinematic.getHeading(); + float arrivalThreshold = 60.0; + + //This just draws a circle for visual debugging purposes + //circle(target.x, target.y, arrivalThreshold); + + //prints the angle to the target + //println(angleToTarget); + + //if the angle is larger than the threshold in the positive direction, rotate counterclockwise + if (angleToTarget > directionalThreshold) { + kinematic.increaseSpeed(0.0, 1); + + //if the angle is smaller than the threshold in the negative direction, rotate clockwise + } else if (angleToTarget < -directionalThreshold) { + kinematic.increaseSpeed(0.0, -1); + + //if the angle is within our threshold, stop our rotational velocity by rotating opposite + } else if (directionalThreshold > angleToTarget) { + + if (kinematic.getRotationalVelocity() > 0) { + kinematic.increaseSpeed(0.0, -1); + } + else if (kinematic.getRotationalVelocity() < 0) { + kinematic.increaseSpeed(0.0, 1); + } + } + + + + //Slight flaw: since the arrival threshold is so big, the boid just won't move if its target is that close. + + //if the target is outside its arrival threshold, accelerate. + //if the target is inside its arrival threshold, accelerate backwards until the speed is 0. + if (direction.mag() > arrivalThreshold) { + kinematic.increaseSpeed(1,0); + } else if (direction.mag() < arrivalThreshold) { + //Need more specific code here to handle arrivals correctly + + if (kinematic.getSpeed() < 40 && direction.mag() > 30) { + kinematic.increaseSpeed(1,0); + } else if (kinematic.getSpeed() < 20 && direction.mag() > 15) { + kinematic.increaseSpeed(.75,0); + } else if (kinematic.getSpeed() < 10 && direction.mag() > 5) { + kinematic.increaseSpeed(.5,0); + } else if (kinematic.getSpeed() < 5 && direction.mag() < 3) { + kinematic.increaseSpeed(.25,0); + } else { + kinematic.increaseSpeed(-1,0); + } + + + } + + + + //drawing a line for testing purposes + //line(kinematic.position.x, kinematic.position.y, kinematic.position.x + direction.x, kinematic.position.y + direction.y); + + + + + + + + + + } + + // 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/Boid17200078021981803956.autosave b/Boid17200078021981803956.autosave new file mode 100644 index 0000000..15350a8 --- /dev/null +++ b/Boid17200078021981803956.autosave @@ -0,0 +1,198 @@ +/// 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 + + + //This makes a vector with the direction our boid needs to go to + PVector direction = PVector.sub(target, kinematic.position); + + //atan2(direction.y, direction.x) will return the direction we need to go in radians + + //print direction we need to go and the direction we are facing right now + //println(atan2(direction.y, direction.x) + " " + normalize_angle_left_right(kinematic.getHeading())); + + float directionalThreshold = .1; + float angleToTarget = normalize_angle_left_right(atan2(direction.y, direction.x) - normalize_angle_left_right(kinematic.getHeading())); + float arrivalThreshold = 60.0; + + //This just draws a circle for visual debugging purposes + circle(target.x, target.y, 3); + + //prints the angle to the target + //println(angleToTarget); + + //if the angle is larger than the threshold in the positive direction, rotate counterclockwise + if (angleToTarget >= .1) { + println("positive angle"); + kinematic.increaseSpeed(0.0, 2); + + //if the angle is smaller than the threshold in the negative direction, rotate clockwise + } else if (angleToTarget < -.1) { + kinematic.increaseSpeed(0.0, -1); + + //if the angle is within our threshold, stop our rotational velocity by rotating opposite + } else if (directionalThreshold > angleToTarget) { + + if (kinematic.getRotationalVelocity() > 0) { + kinematic.increaseSpeed(0.0, -1); + } else if (kinematic.getRotationalVelocity() < 0) { + kinematic.increaseSpeed(0.0, 1); + } + } + + + + //Sometimes our Boid just goes and does weird things and I don't know why + + //if the target is outside its arrival threshold, accelerate. + //if the target is inside its arrival threshold, accelerate backwards until the speed is 0. + if (direction.mag() > arrivalThreshold) { + //println("main if"); + kinematic.increaseSpeed(.5, 0); + } else if (direction.mag() < arrivalThreshold) { + //Need more specific code here to handle arrivals correctly + + if (kinematic.getSpeed() < 40 && direction.mag() > 30) { + //println("if 1"); + kinematic.increaseSpeed(1, 0); + } else if (kinematic.getSpeed() < 20 && direction.mag() > 15) { + //println("if .75"); + kinematic.increaseSpeed(.75, 0); + } else if (kinematic.getSpeed() < 10 && direction.mag() > 5) { + //println("if .5"); + kinematic.increaseSpeed(.5, 0); + } else if (kinematic.getSpeed() < 5 && direction.mag() < 5) { + //println("if -kin"); + //This should ensure that the boid's speed can be dropped to exactly 0 so we don't have stuttering + + kinematic.increaseSpeed(-kinematic.getSpeed(), 0); + } else { + println("else"); + kinematic.increaseSpeed(-1, 0); + } + } + + + + //drawing a line for testing purposes + //line(kinematic.position.x, kinematic.position.y, kinematic.position.x + direction.x, kinematic.position.y + direction.y); + } + + // 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; + } +int count = 0; + + //void follow(ArrayList waypoints) + //{ + + // //println("func count " + count); + // if(count > waypoints.size() - 1){ + // this.target = waypoints.get(0); + // return; + // } + // else { + // // TODO: change to follow *all* waypoints + // println("count " + count); + // this.target = waypoints.get(count); + // PVector temp = waypoints.remove(count); + // count++; + // //count--; + + // follow(waypoints); + // } + + //} + void follow(ArrayList waypoints) + { + this.target = waypoints.get(0); + // println("distance " + PVector.sub(this.target,this.kinematic.position).mag()); + for (int i = 1; i < waypoints.size(); i++){ + println("distance " + PVector.sub(this.target,this.kinematic.position).mag()); + if(PVector.sub(this.target,this.kinematic.position).mag() < 4) + this.target = waypoints.get(i); + + } + } + +} diff --git a/Map.pde b/Map.pde index 000fed4..c1fd95b 100644 --- a/Map.pde +++ b/Map.pde @@ -114,25 +114,36 @@ class Obstacle // 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) + int inside = 0; + int outside = 0; + for (int x = 0; x < 5; ++x) { - if (w.crosses(point, testpoint)) - count += 1; + for (int y = 0; y < 5; ++y) + { + if (x + y == 0) continue; + // we create a test point "far away" horizontally + PVector testpoint = PVector.add(point, new PVector(2*width*(x-3), 2*width*(y-2))); + + // 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. + if (count%2 == 1) inside++; + else outside++; + } } - - // 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; + return inside > outside; } class Map @@ -283,4 +294,29 @@ class Map } return true; } + PVector percentFromPoint(PVector from, PVector to, float percent) + { + //p1 + ((p2 - p1) * percent) + return PVector.add(from, PVector.mult(PVector.sub(to, from),percent)); + } + + boolean intersectsWall(PVector from, PVector to) + { + //5% of the way from the start + PVector start = percentFromPoint(from, to, 0.01); + + //95% of the way from the start + PVector end = percentFromPoint(from, to, 0.99); + + if (!isReachable(start)) return true; + + //println("Start: " + start); + //println("End: " + end); + + for (Wall w : walls) + { + if (w.crosses(start, end)) return true; + } + return false; + } } diff --git a/NavMesh.pde b/NavMesh.pde index d153a5d..e6a659b 100644 --- a/NavMesh.pde +++ b/NavMesh.pde @@ -1,43 +1,446 @@ -// Useful to sort lists by a custom key -import java.util.Comparator; +import java.util.*; - -/// 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; + String id; + ArrayList polygon; + ArrayList indices = new ArrayList(); + PVector center; + ArrayList neighbours = new ArrayList(); + + + Node(String id, ArrayList polygon) + { + this.id = id; + this.polygon = polygon; + center = findCenter(); + } + + PVector findCenter() + { + int x_avg = 0; + int y_avg = 0; + + for(Wall w: polygon) { + x_avg += w.start.x; + y_avg += w.start.y; + } + x_avg /=polygon.size(); + y_avg /=polygon.size(); + return new PVector(x_avg, y_avg); + } + + boolean isNeighbours(Node n) + { + int prev = indices.get(indices.size()-1); + for(Integer i: indices) + { + if (n.indices.contains(prev) && n.indices.contains(i)) return true; + prev = i; + } + + return false; + } + } +class SearchFrontier{ + Node node; + SearchFrontier prev_frontier; + float distanceToEnd; + float distanceToLast = 0; + + SearchFrontier(Node n, SearchFrontier from, PVector end) + { + this.node = n; + this.distanceToEnd = PVector.dist(n.center, end); + if (from != null) + { + this.prev_frontier = from; + this.distanceToLast = PVector.dist(n.center, from.node.center) + from.distanceToLast; + } + + } + + float heuristicSum() + { + return distanceToEnd + distanceToLast; + } +} + class NavMesh -{ - void bake(Map map) - { - /// generate the graph you need for pathfinding - } +{ + ArrayList nodes = new ArrayList(); + int recursionDepth = 0; + int maxDepth = 1000; + int pointAmount = 0; + + HashMap vert_lookup_map = new HashMap(); + ArrayList mapVectors = new ArrayList(); + + PVector midpoint(Node a, Node b) + { + int start = 0; + int end = 0; + + int prev_index = a.indices.get(a.indices.size()-1); + for(Integer i: a.indices) + { + if (b.indices.contains(prev_index) && b.indices.contains(i)) { + start = prev_index; + end = i; + break; + } + prev_index = i; + } + println(a.id + " and " + b.id + " share indices " + start + " and " + end); + + + PVector start_vect, end_vect; + start_vect = mapVectors.get(start); + end_vect = mapVectors.get(end); + + return new PVector(start_vect.x + (end_vect.x - start_vect.x)/2, + start_vect.y + (end_vect.y - start_vect.y)/2); + } + + + void calculateAdjacencies() + { + for (Node n: nodes) + { + n.neighbours.clear(); + } + + //for(int i = 0; i < nodes.size(); i++){ + + //if(i + 1 >= nodes.size()) continue; + //Node a = nodes.get(i); + //Node b = nodes.get(i + 1); + + //if(a.isneighbours(b)) a.neighbours.add(b); + + + //} + for (Node a: nodes) + { + //this is terrible for efficiency i'm so sorry + for (Node b: nodes) + { + if (b.equals(a)) continue; + if (a.isNeighbours(b)) a.neighbours.add(b); + } + } + } + void setIndices(Node node) + { + for(Wall w: node.polygon) + { + node.indices.add(vert_lookup_map.get(w.start)); + } + } + + //assume index_1 < index_2 + void splitMap(Node node, int index_1, int index_2) + { + + ArrayList polygon_1 = new ArrayList(); + ArrayList polygon_2 = new ArrayList(); + + //get the vertex positions from your original node + ArrayList node_verts = new ArrayList(); + for(Wall w: node.polygon) + { + node_verts.add(w.start); + } + + //for polygon_1, just make a polygon from index A to B + for(int i = index_1; i<=index_2; i++) + { + //finishes the polygon + if (i == index_2) { + polygon_1.add( new Wall(node_verts.get(index_2), node_verts.get(index_1)) ); + break; + } + + int next_index = i+1; + if (next_index > node_verts.size()-1) next_index = 0; + polygon_1.add( new Wall(node_verts.get(i), node_verts.get(next_index)) ); + } + + //for polygon_2 + //a little bit tricker, since poly b has a disjunction between vertex indices + //the loop is thus different for constructing b + //start from index_2 and go further until you hit index A. You are guaranteed to finish the polygon once you connect A and B. + int i = index_2; + boolean completedpolygon_2 = false; + while (!completedpolygon_2) { + if (i == index_1) { + polygon_2.add( new Wall(node_verts.get(index_1), node_verts.get(index_2)) ); + completedpolygon_2 = true; + break; + } + + int next_index = i+1; + if (next_index > node_verts.size()-1) next_index = 0; + polygon_2.add( new Wall(node_verts.get(i), node_verts.get(next_index)) ); + + i = next_index; + } + + + //we'll create a node to store poly a + Node nodeA = new Node(recursionDepth+"A", polygon_1); + setIndices(nodeA); + nodes.add(nodeA); + + //the same goes for b + Node nodeB = new Node(recursionDepth+"B", polygon_2); + setIndices(nodeB); + nodes.add(nodeB); + + + + //this portion is not at all necessary for the program to function but it helps when debugging + recursionDepth++; + if (recursionDepth == maxDepth) return; + + //polygons are added to the node list, in order of A and B + //0.[NODE 0A] + //1.[NODE 0B] + + //findReflexVertex will return -1 if the shape is all good + //remove the bad nodes from the list and add in two new ones + //order in the node list has no effect on neighboursing + //the node list functions identically to a bag in that regard + if (findReflexVertex(polygon_1) != -1) { + nodes.remove(nodeA); + convexDecomposition(nodeA); + } + if (findReflexVertex(polygon_2) != -1) { + nodes.remove(nodeB); + convexDecomposition(nodeB); + } + } + + + int findReflexVertex(ArrayList polygon) + { + + for (int i = 0; i= 0) { + return i + 1; + } + } + + return -1; + } + + //given a reflexive index, find a vertex that you can go to without intersection another wall + int joiningVertex(ArrayList polygon, int convex_index) + { + //you need the PVectors for this one + ArrayList vertices = new ArrayList(); + for(Wall w: polygon) + { + vertices.add(w.start); + } + + //our "bad" point + PVector pointAtIndex = vertices.get(convex_index); + + //we don't need to consider the vertex's neighbours since they obviously can't be connected to + int next_index = convex_index + 1; + if (next_index >= vertices.size()) next_index = 0; + + int lastIndex = convex_index - 1; + if (lastIndex < 0) lastIndex = vertices.size() - 1; + + for (int potentialConnecting = vertices.size()-1; potentialConnecting>=0; potentialConnecting--) + { + //skip neighbours and the bad point + if (potentialConnecting == next_index || potentialConnecting == convex_index || potentialConnecting == lastIndex) continue; + + PVector potentialConnectingPoint = vertices.get(potentialConnecting); + + if (!map.intersectsWall(pointAtIndex, potentialConnectingPoint)) + { + return potentialConnecting; + } + } + + return -1; + } + + + void convexDecomposition(Node node) + { + int convex_index = findReflexVertex(node.polygon); + if (convex_index == -1) return; + + int joining_index = joiningVertex(node.polygon, convex_index); + if (joining_index == -1) return; + + // split polygons from small index to the max index + splitMap(node, min(convex_index, joining_index), max(convex_index, joining_index)); + } + + //creates a hashmap with key PVector and value Integer + //creating a hashmap for this removes the risk of directly comparing PVectors since it should look by reference instead of value + void setVertexMap(Map map) + { + //clear all lookups and map vectors + mapVectors.clear(); + vert_lookup_map.clear(); + + for (int i = 0; i < map.walls.size(); i++) + { + vert_lookup_map.put(map.walls.get(i).start, i); + mapVectors.add(map.walls.get(i).start); + + } + } + + void bake(Map map) + { + //reset recursions and other values + recursionDepth = 0; + nodes.clear(); + pointAmount = map.walls.size(); + + vert_lookup_map.clear(); + mapVectors.clear(); + + //make hashmap of vertices + setVertexMap(map); + + //create a node with the whole map walls + Node m = new Node("Map", map.outline); + setIndices(m); + - ArrayList findPath(PVector start, PVector destination) - { - /// implement A* to find a path - ArrayList result = null; - return result; - } + convexDecomposition(m); + + calculateAdjacencies(); + + + } + + Node nodeFromPoint(PVector p) + { + for (Node n: nodes) + { + if (isPointInPolygon(p,n.polygon)) + return n; + } + + return null; + } + //Uses A* to find a path from start to dest + ArrayList findPath(PVector start, PVector dest) + { + println("dest vec " + dest); + ArrayList frontier = new ArrayList(); + ArrayList visited_nodes = new ArrayList(); + Node node_start = nodeFromPoint(start); + Node node_dest = nodeFromPoint(dest); + println("dest node " + node_dest); + //for (Node n: nodes) + //{ + // if (isPointInPolygon(start,n.polygon)) node_start = n; + // else if (isPointInPolygon(dest,n.polygon)) node_dest = n; + // //else println("node_dest is null"); + + //} - void update(float dt) - { - draw(); - } - - void draw() - { - /// use this to draw the nav mesh graph - } + + SearchFrontier s = new SearchFrontier(node_start, null, node_dest.findCenter()); + frontier.add(s); + visited_nodes.add(frontier.get(0).node); + + println("frontier " + frontier); + //till the end of of frontier + while (frontier.get(0).node != node_dest) + { + + SearchFrontier first_frontier = frontier.get(0); + // add all the neighbours of first + + for (Node neighbours: first_frontier.node.neighbours) + { + println("loop"); + if (!visited_nodes.contains(neighbours)) + { + frontier.add(new SearchFrontier(neighbours, first_frontier, node_dest.findCenter())); + } + } + //first in frontier no longer required + frontier.remove(0); + //sort via lambda function + //shorter paths have priority + frontier.sort((a,b) -> { + if (a.heuristicSum() > b.heuristicSum()) return 1; + else if (a.heuristicSum() < b.heuristicSum()) return -1; + else return 0; + }); + //add the removed node to visited list + visited_nodes.add(first_frontier.node); + } + + return findDestPath(dest, node_start, frontier); + } + + + + //given a list of frontiers, create a PVector path from the start to dest + ArrayList findDestPath(PVector dest, Node node_start, ArrayList genPath) + { + //we're going to build this list up from the end and then reverse it. + + ArrayList res = new ArrayList(); + //add the end + res.add(dest); + SearchFrontier front = genPath.get(0); + while (front.node != node_start) { + PVector midPoint = midpoint(front.node, front.prev_frontier.node); + res.add(midPoint); + + //assign previous frontier to start + front = front.prev_frontier; + } + + + Collections.reverse(res); + println("result " + res); + return res; + } + + + void update(float dt) + { + draw(); + } + + void draw() + { + + strokeWeight(3); + + + for (Node n: nodes) + { + for (Wall w: n.polygon) + { + stroke(0,255,255); + strokeWeight(3); + line(w.start.x, w.start.y, w.end.x, w.end.y); + //w.draw(); + } + } + } } diff --git a/lab1.pde b/lab1.pde index d0e5f76..3f47d8d 100644 --- a/lab1.pde +++ b/lab1.pde @@ -13,7 +13,7 @@ NavMesh nm = new NavMesh(); boolean entering_path = false; -boolean show_nav_mesh = false; +boolean show_nav_mesh = true; boolean show_waypoints = false; @@ -37,14 +37,41 @@ void mousePressed() { if (mouseButton == LEFT) { - if (waypoints.size() == 0) + if (!entering_path) //if you haven't made a path yet { - billy.seek(target); + //println("node size " + nm.nodes.size()); + if (nm.nodes.size() > 0) //if you're on a map + { + println("Pathfinding to single target"); + waypoints = nm.findPath(billy.kinematic.position, target); + println("waypoints " + waypoints); + //Collections.reverse(waypoints); + billy.follow(waypoints); + } + else { //if you're not on a map + println("Simply seeking target"); + billy.seek(target); + } } - else + else //if you have a path { - waypoints.add(target); + //finish the path + if (nm.nodes.size() > 0) //if you're on a map + { + PVector start_vectoint = waypoints.get(waypoints.size() -1); + ArrayList finalRoute = nm.findPath(start_vectoint, target); + for (PVector p: finalRoute) + { + waypoints.add(p); + } + } + else //if you're not on a map + { + waypoints.add(target); + } + println("Finishing Path"); entering_path = false; + println(waypoints); billy.follow(waypoints); } }