376 lines
13 KiB
Plaintext
376 lines
13 KiB
Plaintext
// General constants
|
|
float AGENT_RADIUS = 2;
|
|
float SLOW_RADIUS = 6;
|
|
float TARGET_RADIUS = 2;
|
|
float MAX_SPEED = 12;
|
|
float MAX_ACCELERATION = 1;
|
|
|
|
// Rotation
|
|
float SLOW_ROTATION_RADIUS = PI/10;
|
|
float TARGET_ROTATION_RADIUS = PI/20;
|
|
float MAX_ANGULAR_SPEED = PI/90;
|
|
|
|
// Repel from leader
|
|
float THRESH = 6;
|
|
float CONSTANT = 5;
|
|
|
|
float FOLLOWER_PATHING_DISTANCE = 15;
|
|
|
|
class Agent {
|
|
boolean selected;
|
|
boolean reachedDest;
|
|
PVector destination;
|
|
private GridPath path;
|
|
|
|
private Body body;
|
|
private Fixture fixture;
|
|
float orientation;
|
|
|
|
boolean isAvoiding = false;
|
|
boolean avoidanceDir = false;
|
|
|
|
Agent(PVector startPos) {
|
|
// Define a polygon
|
|
PolygonShape sd = new PolygonShape();
|
|
// Circles in box2d processing gave assertion errors, so this is close enough
|
|
sd.setAsBox(box2d.scalarPixelsToWorld(AGENT_RADIUS*2), box2d.scalarPixelsToWorld(AGENT_RADIUS*2));
|
|
|
|
// Define a fixture
|
|
FixtureDef fd = new FixtureDef();
|
|
fd.shape = sd;
|
|
// Parameters that affect physics
|
|
fd.density = 20.0;
|
|
fd.friction = 0.0;
|
|
fd.restitution = 0.0;
|
|
|
|
// Define the body and make it from the shape
|
|
BodyDef bd = new BodyDef();
|
|
bd.type = BodyType.DYNAMIC;
|
|
bd.position.set(box2d.coordPixelsToWorld(startPos));
|
|
|
|
body = box2d.createBody(bd);
|
|
this.body.setUserData(this);
|
|
this.fixture = this.body.createFixture(fd);
|
|
|
|
this.orientation = 0;
|
|
|
|
this.selected = false;
|
|
this.reachedDest = true;
|
|
this.destination = startPos;
|
|
}
|
|
|
|
void SetDest(PVector dest, GridPath path) {
|
|
this.reachedDest = false;
|
|
this.destination = dest;
|
|
this.path = path;
|
|
}
|
|
|
|
PVector GetPos() {
|
|
return box2d.getBodyPixelCoordPVector(this.body);
|
|
}
|
|
|
|
boolean IsIn(PVector a, PVector b) {
|
|
PVector pos = GetPos();
|
|
if (a.x < b.x && a.y < b.y) {
|
|
return pos.x >= a.x && pos.x <= b.x && pos.y >= a.y && pos.y <= b.y;
|
|
} else if (a.x > b.x && a.y < b.y) {
|
|
return pos.x <= a.x && pos.x >= b.x && pos.y >= a.y && pos.y <= b.y;
|
|
} else if (a.x < b.x && a.y > b.y) {
|
|
return pos.x >= a.x && pos.x <= b.x && pos.y <= a.y && pos.y >= b.y;
|
|
} else if (a.x > b.x && a.y > b.y) {
|
|
return pos.x <= a.x && pos.x >= b.x && pos.y <= a.y && pos.y >= b.y;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void GridPathing() {
|
|
PVector pos = GetPos();
|
|
Point floor = grid.CalcFromScreenCoords(pos);
|
|
|
|
if (grid.IsValidSpace(floor)) {
|
|
if (floor.equals(grid.CalcFromScreenCoords(this.destination))) {
|
|
// We are at same grid, so steer directly towards destination
|
|
Steering(new PVector(this.destination.x - pos.x, this.destination.y - pos.y), pos);
|
|
return;
|
|
}
|
|
|
|
Point sector = grid.GetSector(floor);
|
|
Point sectorFloor = grid.GridToSector(floor);
|
|
if (path == null || path.sectorPaths[sector.x][sector.y] == null) {
|
|
// Re-calculate path if we go off track
|
|
this.SetDest(this.destination,
|
|
grid.Pathing(floor, grid.CalcFromScreenCoords(this.destination)));
|
|
// Still no grid path, so blind steer
|
|
if (path == null || path.sectorPaths[sector.x][sector.y] == null) {
|
|
Steering(new PVector(this.destination.x - pos.x, this.destination.y - pos.y), pos);
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (!path.sectorPaths[sector.x][sector.y].sectorDest.equals(sectorFloor) &&
|
|
path.sectorPaths[sector.x][sector.y].losField[sectorFloor.x][sectorFloor.y]) {
|
|
// Navigate straight towards destination in sector if we have LOS
|
|
PVector sDest = grid.CalcScreenCoordsFromPoint(grid.SectorToGrid(grid.sectors[sector.x][sector.y].start,
|
|
path.sectorPaths[sector.x][sector.y].sectorDest));
|
|
sDest.add(grid.spaceSizeX / 2, grid.spaceSizeY / 2);
|
|
Steering(new PVector(sDest.x - pos.x, sDest.y - pos.y), pos);
|
|
return;
|
|
}
|
|
|
|
// Get direction of movement from flow field
|
|
PVector dir = new PVector(path.sectorPaths[sector.x][sector.y].flowField[sectorFloor.x][sectorFloor.y].x,
|
|
path.sectorPaths[sector.x][sector.y].flowField[sectorFloor.x][sectorFloor.y].y);
|
|
Steering(dir, pos);
|
|
return;
|
|
}
|
|
|
|
// Blind steer towards grid if we go off screen
|
|
Steering(new PVector(this.destination.x - pos.x, this.destination.y - pos.y), pos);
|
|
}
|
|
|
|
void FollowerPathing(PVector leaderPos) {
|
|
PVector pos = GetPos();
|
|
if (dist(this.destination.x, this.destination.y, pos.x, pos.y) > FOLLOWER_PATHING_DISTANCE) {
|
|
// Too far away, so we should use real pathfinding to get back to formation
|
|
GridPathing();
|
|
return;
|
|
}
|
|
PVector dir = new PVector(this.destination.x - pos.x, this.destination.y - pos.y);
|
|
|
|
// Repel from leader so as not to run them over
|
|
PVector leaderDir = new PVector(pos.x - leaderPos.x, pos.y - leaderPos.y);
|
|
float distanceFromRepel = leaderDir.mag();
|
|
if (distanceFromRepel < THRESH) {
|
|
float strength = CONSTANT / distanceFromRepel * distanceFromRepel;
|
|
leaderDir.normalize();
|
|
leaderDir.mult(strength);
|
|
dir.add(leaderDir);
|
|
}
|
|
|
|
Steering(dir, pos);
|
|
}
|
|
|
|
private void Steering(PVector dir, PVector pos) {
|
|
if (dir.x == 0 && dir.y == 0) {
|
|
return;
|
|
}
|
|
dir.normalize();
|
|
|
|
// Rough orientation setting is just for formation setting purposes
|
|
float targetOrientation = atan2(dir.x, dir.y);
|
|
float rotation = targetOrientation - orientation;
|
|
float rotationSize = abs(rotation);
|
|
// Calculate target rotation
|
|
float targetRotation = 0;
|
|
if (rotationSize > SLOW_ROTATION_RADIUS) {
|
|
targetRotation = MAX_ANGULAR_SPEED;
|
|
} else if (rotationSize > TARGET_ROTATION_RADIUS) {
|
|
targetRotation = MAX_ANGULAR_SPEED * rotationSize / SLOW_ROTATION_RADIUS;
|
|
}
|
|
// Set direction
|
|
targetRotation *= rotation / rotationSize;
|
|
// Update orientation
|
|
orientation += targetRotation;
|
|
|
|
// Determine target speed
|
|
float targetSpeed = 0;
|
|
if (!reachedDest) {
|
|
float distance = dist(this.destination.x, this.destination.y, pos.x, pos.y);
|
|
if (distance > SLOW_RADIUS) {
|
|
targetSpeed = MAX_SPEED;
|
|
} else if (distance > TARGET_RADIUS) {
|
|
targetSpeed = MAX_SPEED * distance / (float) SLOW_RADIUS;
|
|
} else {
|
|
this.reachedDest = true;
|
|
}
|
|
}
|
|
|
|
// Slow down the agent based on grid cost
|
|
Point floor = grid.CalcFromScreenCoords(pos);
|
|
if (grid.IsValidSpace(floor)) {
|
|
targetSpeed -= ((float)grid.costField[floor.x][floor.y] / (float)Byte.MAX_VALUE) * targetSpeed;
|
|
}
|
|
|
|
// Dir is in screen coords, we need box2d coords
|
|
dir.y *= -1;
|
|
// Determine target velocity
|
|
dir.mult(targetSpeed);
|
|
|
|
// Determine acceleration
|
|
PVector agentAcceleration = new PVector(dir.x, dir.y);
|
|
agentAcceleration.sub(this.body.getLinearVelocity().x, this.body.getLinearVelocity().y);
|
|
Vec2 avoidanceForce = CollisionAvoidance();
|
|
if (avoidanceForce != null) {
|
|
agentAcceleration.add(avoidanceForce.x, avoidanceForce.y);
|
|
}
|
|
|
|
// Clip acceleration
|
|
if (agentAcceleration.mag() > MAX_ACCELERATION) {
|
|
agentAcceleration.normalize();
|
|
agentAcceleration.mult(MAX_ACCELERATION);
|
|
}
|
|
|
|
// Update velocity
|
|
this.body.setLinearVelocity(new Vec2(this.body.getLinearVelocity().x + agentAcceleration.x,
|
|
this.body.getLinearVelocity().y + agentAcceleration.y));
|
|
}
|
|
|
|
// Processing does not support lambdas, so this workaround is required
|
|
class AgentRayCastCallback implements RayCastCallback {
|
|
Agent agent;
|
|
float minFraction;
|
|
Fixture closestFixture;
|
|
|
|
AgentRayCastCallback(Agent agent, float minFraction) {
|
|
this.agent = agent;
|
|
this.minFraction = minFraction;
|
|
this.closestFixture = null;
|
|
}
|
|
|
|
public float reportFixture(Fixture fixture, Vec2 point, Vec2 normal, float fraction) {
|
|
//Ignore ourself
|
|
if (fixture == agent.fixture) {
|
|
return fraction;
|
|
}
|
|
if (fraction < minFraction &&
|
|
(fixture.getBody().getType() == BodyType.DYNAMIC || fixture.getBody().getType() == BodyType.STATIC)) {
|
|
minFraction = fraction;
|
|
closestFixture = fixture;
|
|
}
|
|
return 0;
|
|
}
|
|
}
|
|
// Collision avoidance function inspired by howtorts.github.io/2014/01/14/avoidance-behaviours.html
|
|
private Vec2 CollisionAvoidance() {
|
|
if (this.body.getLinearVelocity().lengthSquared() <= AGENT_RADIUS) {
|
|
this.isAvoiding = false;
|
|
return null;
|
|
}
|
|
|
|
AgentRayCastCallback callback = new AgentRayCastCallback(this, 2);
|
|
box2d.world.raycast(callback, this.body.getPosition(), this.body.getPosition().add(this.body.getLinearVelocity()));
|
|
if (callback.closestFixture == null) {
|
|
this.isAvoiding = false;
|
|
return null;
|
|
}
|
|
|
|
if (callback.closestFixture.getBody().getType() == BodyType.STATIC) {
|
|
this.isAvoiding = true;
|
|
return this.body.getPosition().sub(callback.closestFixture.getBody().getPosition()).mulLocal(1/callback.minFraction);
|
|
}
|
|
|
|
Vec2 resultVector = null;
|
|
Body collisionBody = callback.closestFixture.getBody();
|
|
float ourVelocityLengthSquared = this.body.getLinearVelocity().lengthSquared();
|
|
Vec2 combinedVelocity = this.body.getLinearVelocity().add(collisionBody.getLinearVelocity());
|
|
float combinedVelocityLengthSquared = combinedVelocity.lengthSquared();
|
|
|
|
// We are going in the same direction and they aren't avoiding
|
|
if (combinedVelocityLengthSquared > ourVelocityLengthSquared &&
|
|
((Agent)callback.closestFixture.getBody().getUserData()).isAvoiding == false) {
|
|
this.isAvoiding = false;
|
|
return null;
|
|
}
|
|
|
|
Vec2 vectorInOtherDirection = callback.closestFixture.getBody().getPosition().sub(this.body.getPosition());
|
|
|
|
boolean isLeft;
|
|
if (((Agent)callback.closestFixture.getBody().getUserData()).isAvoiding == true) {
|
|
isLeft = ((Agent)callback.closestFixture.getBody().getUserData()).avoidanceDir;
|
|
} else {
|
|
float dot = this.body.getLinearVelocity().x * -vectorInOtherDirection.y + this.body.getLinearVelocity().y * vectorInOtherDirection.x;
|
|
isLeft = dot > 0;
|
|
}
|
|
this.isAvoiding = true;
|
|
this.avoidanceDir = isLeft;
|
|
|
|
resultVector = isLeft ?
|
|
new Vec2(-vectorInOtherDirection.y, vectorInOtherDirection.x) :
|
|
new Vec2(vectorInOtherDirection.y, -vectorInOtherDirection.x);
|
|
resultVector.normalize();
|
|
|
|
resultVector.mulLocal(AGENT_RADIUS * 2).mulLocal(1/callback.minFraction);
|
|
return resultVector;
|
|
}
|
|
|
|
// For drawing
|
|
private void SetTransform(PVector translate, float scale, float rot) {
|
|
pushMatrix();
|
|
translate(translate.x, translate.y);
|
|
rotate(radians(rot));
|
|
scale(scale);
|
|
}
|
|
void DrawAgent() {
|
|
if (DEBUG_ON && selected) {
|
|
if (path != null) {
|
|
for (int row = 0; row < grid.rows; ++row) {
|
|
for (int col= 0; col < grid.cols; ++col) {
|
|
stroke(0);
|
|
fill(0);
|
|
Point sector = grid.GetSector(new Point(row, col));
|
|
Point sectorPoint = grid.GridToSector(new Point(row, col));
|
|
if (path.sectorPaths[sector.x][sector.y] != null) {
|
|
if (path.sectorPaths[sector.x][sector.y].losField[sectorPoint.x][sectorPoint.y]) {
|
|
stroke(0, 0, 255);
|
|
fill(128);
|
|
PVector a = grid.CalcScreenCoordsFromPoint(grid.SectorToGrid(grid.sectors[sector.x][sector.y].start, sectorPoint));
|
|
rect(a.x, a.y, grid.spaceSizeX, grid.spaceSizeY);
|
|
}
|
|
stroke(0);
|
|
fill(0);
|
|
text(Float.toString(path.sectorPaths[sector.x][sector.y].integrationField[sectorPoint.x][sectorPoint.y]),
|
|
row*grid.spaceSizeX + grid.spaceSizeX/2, col*grid.spaceSizeY + grid.spaceSizeY/2);
|
|
line(row*grid.spaceSizeX + grid.spaceSizeX/2, col*grid.spaceSizeY + grid.spaceSizeY/2,
|
|
row*grid.spaceSizeX + grid.spaceSizeX/2 + (path.sectorPaths[sector.x][sector.y].flowField[sectorPoint.x][sectorPoint.y].x * 25),
|
|
col*grid.spaceSizeY + grid.spaceSizeY/2 + (path.sectorPaths[sector.x][sector.y].flowField[sectorPoint.x][sectorPoint.y].y * 25));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
stroke(255, 255, 255);
|
|
fill(255, 255, 255);
|
|
ellipse(destination.x, destination.y, 2, 2);
|
|
}
|
|
|
|
this.SetTransform(box2d.getBodyPixelCoordPVector(this.body), AGENT_RADIUS * 2, 0);
|
|
if (selected) {
|
|
stroke(0, 0, 255);
|
|
} else {
|
|
stroke(255, 0, 0);
|
|
}
|
|
ellipse(0, 0, 1, 1);
|
|
popMatrix();
|
|
}
|
|
}
|
|
|
|
// Function to check contacts and tell agents they have reached
|
|
// their destination if they hit another agent with the same
|
|
// destination as them that has already reached the destination
|
|
void CheckContacts() {
|
|
for (Contact cp = box2d.world.getContactList(); cp != null; cp = cp.getNext()) {
|
|
// Get both shapes
|
|
Fixture f1 = cp.getFixtureA();
|
|
Fixture f2 = cp.getFixtureB();
|
|
// Get both bodies
|
|
Body b1 = f1.getBody();
|
|
Body b2 = f2.getBody();
|
|
|
|
// Get our objects that reference these bodies
|
|
Object o1 = b1.getUserData();
|
|
Object o2 = b2.getUserData();
|
|
if (o1 == null || o2 == null) {
|
|
return;
|
|
}
|
|
|
|
if (o1.getClass() == Agent.class && o2.getClass() == Agent.class) {
|
|
Agent p1 = (Agent) o1;
|
|
Agent p2 = (Agent) o2;
|
|
|
|
if (p1.destination.equals(p2.destination) && (p1.reachedDest || p2.reachedDest)) {
|
|
p1.reachedDest = true;
|
|
p2.reachedDest = true;
|
|
}
|
|
}
|
|
}
|
|
}
|