p5.js Web Editor | Stateful Agent class

xxxxxxxxxx
/*
Class exists of :
 - constructor
 - attributes
 - (class) methods
Class defines how an object behaves
*/
​
class RRTTree {
  constructor(start, goal) {
    this.start = start.copy();
    this.goal = goal.copy();
    // Each node is stored as an object with a position and a parent pointer.
    this.nodes = [{ pos: start.copy(), parent: null }];
    this.edges = []; // For debugging: store each edge as { from: p5.Vector, to: p5.Vector }
    this.goalThreshold = 10; // Distance within which we consider the goal reached.
  }
  
  expand() {
  // With a 1% chance, sample the goal to bias the tree toward it.
  let randomPoint = (random() < 0.01) ? this.goal.copy() : createVector(random(width), random(height));
​
  // Find the nearest node in the tree.
  let nearest = this.getNearest(randomPoint);
​
  // Define a fixed step size.
  const stepSize = 20;
​
  // Compute the direction from the nearest node toward the random point.
  let direction = p5.Vector.sub(randomPoint, nearest.pos);
  
  // Limit the step to the fixed step size.
  if (direction.mag() > stepSize) {
    direction.setMag(stepSize);
  }
  
  // Compute the new position by moving from the nearest node along the direction.
  let newPos = p5.Vector.add(nearest.pos, direction);
​
  // Create the new node and add it to the tree.
  let newNode = { pos: newPos.copy(), parent: nearest };
  this.nodes.push(newNode);
​
  // Store the edge for debugging purposes.
  this.edges.push({ from: nearest.pos.copy(), to: newPos.copy() });
}
​
  
  getNearest(point) {
    let nearest = this.nodes[0];
    let minDist = p5.Vector.dist(nearest.pos, point);
    for (let node of this.nodes) {
      let d = p5.Vector.dist(node.pos, point);
      if (d < minDist) {
        nearest = node;
        minDist = d;
      }
    }
    return nearest;
  }
  
  findPath() {
    let goalNode = null;
    // Look for any node close enough to the goal.
    for (let node of this.nodes) {
      if (p5.Vector.dist(node.pos, this.goal) < this.goalThreshold) {
        goalNode = node;
        break;
      }
    }
    if (goalNode === null) {
      return null;
    }
    
    // Reconstruct the path from the found goalNode back to the start.
    let path = [];
    let current = goalNode;
    while (current !== null) {
      path.push(current.pos.copy());
      current = current.parent;
    }
    path.reverse();
    // Optionally append the exact goal position.
    path.push(this.goal.copy());
    return path;
  }
  
  // --- Debug Visualization ---
  // Pushes drawing commands for edges and nodes into the provided drawQueue.
  debugDraw(drawQueue) {
    // Debug layer: RRT edges.
    for (let edge of this.edges) {
      drawQueue.push({
        type: 'line',
        x1: edge.from.x,
        y1: edge.from.y,
        x2: edge.to.x,
        y2: edge.to.y,
        color: [150, 150, 150], // Gray edges.
        strokeWeight: 1,
        layer: 'rrtEdges'
      });
    }
    // Debug layer: RRT nodes.
    for (let node of this.nodes) {
      drawQueue.push({
        type: 'circle',
        x: node.pos.x,
        y: node.pos.y,
        diameter: 5,
        color: [0, 0, 255], // Blue nodes.
        strokeWeight: 1,
        layer: 'rrtNodes'
      });
    }
  }
}
​
​
// The NavigatingState continuously expands the RRT tree and, when a valid
// path is found, moves the agent along that path.
class RRTNavigatingState {
  constructor(agent) {
    this.agent = agent;
    this.name = 'navigating';
    // Initialize the RRT tree with the current position and target.
    this.rrtTree = new RRTTree(this.agent.pos, this.agent.target);
    this.path = []; // Current best path (an array of waypoints)
  }
  
tick() {
  // Expand the RRT tree slowly (simulate limited compute).
  this.rrtTree.expand();
​
  // Try to get a complete solution path.
  let candidatePath = this.rrtTree.findPath();
​
  // We'll use nextWaypoint to store the next node the agent should head to.
  let nextWaypoint = null;
​
  if (candidatePath && candidatePath.length > 1) {
    // If a complete path exists, always move toward the second node.
    nextWaypoint = candidatePath[1];
  } else {
    // Otherwise, try to find a tree node whose parent is the agent’s current position.
    // (We assume the agent’s current position is one of the nodes—i.e. the start of the branch.)
    for (let node of this.rrtTree.nodes) {
      if (node.parent && p5.Vector.dist(node.parent.pos, this.agent.pos) < 1) {
        nextWaypoint = node.pos;
        break;
      }
    }
  }
​
  // If a next node exists, move toward it.
  if (nextWaypoint) {
    let diff = p5.Vector.sub(nextWaypoint, this.agent.pos);
​
    // If close enough to the next waypoint, snap to it.
    if (diff.mag() < this.agent.speed) {
      this.agent.pos = nextWaypoint.copy();
    } else {
      diff.normalize().mult(this.agent.speed);
      this.agent.pos.add(diff);
    }
​
    // Optionally: transition state if we've reached the overall target.
    if (p5.Vector.dist(this.agent.pos, this.agent.target) < 1) {
      this.agent.setState(GoalReachedState);
    }
  }
  // (If no nextWaypoint exists, the agent remains in place until more tree nodes appear.)
}
​
​
​
}
​
// Once the target is reached, the agent idles for a while before selecting
// a new target and restarting the navigation process.
class GoalReachedState {
  constructor(agent) {
    this.agent = agent;
    this.name = 'goal reached';
    this.ticksRemaining = int(random(300, 360));
  }
  
  tick() {
    this.ticksRemaining--;
    if (this.ticksRemaining <= 0) {
      // Pick a new target and restart with a fresh RRT.
      this.agent.target = createVector(random(width), random(height));
      this.agent.setState(RRTNavigatingState);
    }
  }
}
class RobustAgent {
  constructor(x, y) {
    this.pos = createVector(x, y);
    this.target = createVector(random(width), random(height));
    this.drawQueue = [];
    this.speed = 5; // Movement speed
    this.state = new RRTNavigatingState(this); // For RRT-based navigation.
    
    // Toggle which debug layers are visible.
    this.debugLayers = {
      rrtEdges: true,
      rrtNodes: true,
      rrtPath: true,
      generic: true  // For any non-layered debug items.
    };
  }
  
  // Transition to a new state.
  setState(StateClass) {
    if (this.state && this.state.exit) {
      this.state.exit();
    }
    this.state = new StateClass(this);
    if (this.state.enter) {
      this.state.enter();
    }
  }
  
  tick() {
    this.state.tick();
  }
  
  // Render the agent and any queued debug drawings.
  draw() {
    push();
    // Process each debug item in the draw queue.
    for (let item of this.drawQueue) {
      // If the item has a 'layer', only draw it if that layer is enabled.
      if (item.layer && this.debugLayers && !this.debugLayers[item.layer]) {
        continue;
      }
      // Set color and stroke weight.
      if (item.color) {
        stroke(item.color[0], item.color[1], item.color[2]);
      } else {
        stroke(255, 0, 0);
      }
      strokeWeight(item.strokeWeight || 2);
      
      // Draw based on item type.
      if (item.type === 'line') {
        line(item.x1, item.y1, item.x2, item.y2);
      } else if (item.type === 'circle') {
        noFill();
        ellipse(item.x, item.y, item.diameter, item.diameter);
      }
    }
    pop();
    
    // Draw the agent.
    push();
    
    fill('red')
    circle(this.target.x, this.target.y, 15)
    
    fill('blue');
    strokeWeight(1);
    circle(this.pos.x, this.pos.y, 15);
    
    pop();
    
   
    
    
    text(this.state.name, this.pos.x, this.pos.y - 10);
    
    // Clear the draw queue.
    this.drawQueue = [];
    
  
  }
}
​
// --- State Classes ---
​
// Searching: Waits for 10-20 ticks before picking a target and switching to Moving.
class SearchingState {
  constructor(agent) {
    this.agent = agent;
    this.name = 'searching'
    this.ticksRemaining = int(random(10, 20));
  }
  
  tick() {
    this.ticksRemaining--;
    if (this.ticksRemaining <= 0) {
      // Once searching is done, pick a new target and transition to Moving.
      this.agent.target = createVector(random(width), random(height));
      this.agent.setState(MovingState);
    }
  }
}
​
// Moving: Moves toward the target, adding a debug line. Once close enough, transitions to Idling.
class MovingState {
  constructor(agent) {
    this.agent = agent;
    this.name = 'moving'
    this.speed = this.agent.speed;
  }
  
  tick() {
    let diff = p5.Vector.sub(this.agent.target, this.agent.pos);
    
    // Push a debug drawing: line from the current position to the target.
    this.agent.drawQueue.push({
      type: 'line',
      x1: this.agent.pos.x,
      y1: this.agent.pos.y,
      x2: this.agent.target.x,
      y2: this.agent.target.y
    });
    
    if (diff.mag() < this.speed) {
      // Arrived at target: snap to target and switch to Idling.
      this.agent.pos = this.agent.target.copy();
      this.agent.setState(IdlingState);
    } else {
      diff.normalize().mult(this.speed);
      this.agent.pos.add(diff);
    }
  }
}
​
// Idling: Once the target is reached, idles for 5-6 seconds (~300-360 ticks at 60 fps) before restarting.
class IdlingState {
  constructor(agent) {
    this.agent = agent;
    this.name = 'idling'
    this.ticksRemaining = int(random(300, 360));
  }
  
  tick() {
    this.ticksRemaining--;
    if (this.ticksRemaining <= 0) {
      // Restart the cycle with a new search.
      this.agent.setState(SearchingState);
    }
  }
}
​
​
class Agent {
  constructor(x, y) {
    this.pos = createVector(x, y);
    this.state = 'searching';
    this.state_ticks = int(random(10, 20));  // 10–20 ticks for searching
    this.target = createVector(random(0, width), random(0, height));
    
    // F I F O
    this.drawQueue = []; 
    
    this.speed = 2; // Speed for moving state
  }
​
  tick() {
    switch (this.state) {
      case 'searching':
        // Remain in "searching" state for a short period
        this.state_ticks--;
        if (this.state_ticks <= 0) {
          this.state = 'moving';
        }
        break;
​
      case 'moving': {
        // Calculate the movement vector from current position to target
        let diff = p5.Vector.sub(this.target, this.pos);
​
        // Add a debug drawing instruction: a line from the agent to its target
        this.drawQueue.push({
          type: 'line',
          x1: this.pos.x,
          y1: this.pos.y,
          x2: this.target.x,
          y2: this.target.y
        });
​
        // If the agent is close enough to the target, snap to it
        if (diff.mag() < this.speed) {
          this.pos = this.target.copy();
          this.state = 'idling';
          // Idle for 5–6 seconds (300–360 ticks at 60 fps)
          this.state_ticks = int(random(300, 360));
        } else {
          // Move a step towards the target
          // ---------------------------------------------
          // diff = distance from agent to target
          // normalize = make a 'unit_vector' = length 1
          // mult speed
          // ---------------------------------------------
          diff.normalize().mult(this.speed);
          this.pos.add(diff);
        }
        break;
      }
​
      case 'idling':
        // Agent idles (does nothing) for the designated ticks
        this.state_ticks--;
        if (this.state_ticks <= 0) {
          // Restart: transition back to searching and pick a new target
          this.state = 'searching';
          this.state_ticks = int(random(10, 20));
          this.target = createVector(random(0, width), random(0, height));
        }
        break;
    }
  }
​
  draw() {
    push();
    // Draw all items from the draw queue (e.g. debug lines)
  
    stroke(255, 0, 0);
    strokeWeight(2);
    for (let item of this.drawQueue) {
      if (item.type === 'line') {
        line(item.x1, item.y1, item.x2, item.y2);
      }
    }
    pop();
​
    // Draw the agent as a circle
    push();
    fill(0);
    strokeWeight(1);
    circle(this.pos.x, this.pos.y, 15);
    pop();
    text(this.state, this.pos.x,this.pos.y-10)
​
    // Clear the draw queue after drawing
    this.drawQueue = [];
  }
}
​
​
function setup() {
  // the setup function runs only 1 time at the start
  createCanvas(400, 400);
  
  agent_1 = new Agent(100, 100);
  robust_agent_1 = new RobustAgent(200, 200);
  
  agents = [agent_1, robust_agent_1]
  
}
​
function draw() {
  // draw function runs some ~60 times per second (depending FPS)
  background(220);
  
  for (const a of agents){
    a.tick()
  }
  
  for (const a of agents){
    a.draw()
  }
  
}
Cookies

Sketch Files

Console

Preview
