Merge branch 'main' of https://github.com/JH159753/GameAILab1
This commit is contained in:
JH159753
commit
97517f736f
151
Boid.pde
151
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<waypoints.size(); i++)
|
||||
{
|
||||
text(i, waypoints.get(i).x + 10, waypoints.get(i).y + 10);
|
||||
}
|
||||
}
|
||||
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);
|
||||
|
||||
=======
|
||||
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<PVector> 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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<PVector> waypoints)
|
||||
{
|
||||
// TODO: change to follow *all* waypoints
|
||||
this.target = waypoints.get(0);
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -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<PVector> 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<PVector> waypoints)
|
||||
{
|
||||
this.target = waypoints.get(0);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -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<PVector> waypoints)
|
||||
{
|
||||
// TODO: change to follow *all* waypoints
|
||||
this.target = waypoints.get(0);
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -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<PVector> 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<PVector> 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);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
70
Map.pde
70
Map.pde
|
|
@ -114,25 +114,36 @@ class Obstacle
|
|||
// visible screen (or not too far outside)
|
||||
boolean isPointInPolygon(PVector point, ArrayList<Wall> 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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
465
NavMesh.pde
465
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<Wall> polygon;
|
||||
PVector center;
|
||||
ArrayList<Node> neighbors;
|
||||
ArrayList<Wall> connections;
|
||||
String id;
|
||||
ArrayList<Wall> polygon;
|
||||
ArrayList<Integer> indices = new ArrayList<Integer>();
|
||||
PVector center;
|
||||
ArrayList<Node> neighbours = new ArrayList<Node>();
|
||||
|
||||
|
||||
Node(String id, ArrayList<Wall> 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<Node> nodes = new ArrayList<Node>();
|
||||
int recursionDepth = 0;
|
||||
int maxDepth = 1000;
|
||||
int pointAmount = 0;
|
||||
|
||||
HashMap<PVector, Integer> vert_lookup_map = new HashMap<PVector, Integer>();
|
||||
ArrayList<PVector> mapVectors = new ArrayList<PVector>();
|
||||
|
||||
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<Wall> polygon_1 = new ArrayList<Wall>();
|
||||
ArrayList<Wall> polygon_2 = new ArrayList<Wall>();
|
||||
|
||||
//get the vertex positions from your original node
|
||||
ArrayList<PVector> node_verts = new ArrayList<PVector>();
|
||||
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<Wall> polygon)
|
||||
{
|
||||
|
||||
for (int i = 0; i<polygon.size() - 1; i++)
|
||||
{
|
||||
// finding the reflex angle by finding where it turns right
|
||||
if (polygon.get(i).normal.dot(polygon.get(i + 1).direction) >= 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<Wall> polygon, int convex_index)
|
||||
{
|
||||
//you need the PVectors for this one
|
||||
ArrayList<PVector> vertices = new ArrayList<PVector>();
|
||||
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<PVector> findPath(PVector start, PVector destination)
|
||||
{
|
||||
/// implement A* to find a path
|
||||
ArrayList<PVector> 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<PVector> findPath(PVector start, PVector dest)
|
||||
{
|
||||
println("dest vec " + dest);
|
||||
ArrayList<SearchFrontier> frontier = new ArrayList<SearchFrontier>();
|
||||
ArrayList<Node> visited_nodes = new ArrayList<Node>();
|
||||
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<PVector> findDestPath(PVector dest, Node node_start, ArrayList<SearchFrontier> genPath)
|
||||
{
|
||||
//we're going to build this list up from the end and then reverse it.
|
||||
|
||||
ArrayList<PVector> res = new ArrayList<PVector>();
|
||||
//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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
37
lab1.pde
37
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<PVector> 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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue