I have coded a simple autonomous agents 3D environment and I trigger some events based on a "counter" a variable that takes integer values from 0 to 180. There is a public class for the basics and some subclasses that define some additional parameters for each group of agents. Every agent is being represented as a 3D shere. 

What I want to do is to create something like a trail of spheres (for each agent) that depicts its path while flying. Something like this but in 3D: 

So I tried to implement the map() function but the results are wrong. Any ideas how I can use it? Here is one of the subclasses and the public class:

abstract class Capsule {  //the super-class that defines the chararacteristics of the agents

  PVector location;

  PVector velocity;

  PVector acceleration;

  float r;  // the radius of the circles-spheres/ agents

  float wandertheta, wanderphi, wanderpsi;

  float maxforce;    // Maximum steering force

  float maxspeed;    // Maximum speed

  color fillColor;  // differentiation of agent types are based on complementary coloring

  int name;  // gives an ID - number to each different agent

  float angle, angleVel; 

  

  Capsule(float x, float y, float z, color c, int nam) {  // the class constructor

    acceleration = new PVector(0,0,0);

    velocity = new PVector(0,0,0);

    location = new PVector(x,y,z);

    r = 6;

    wandertheta = 0;

    wanderphi = 0;

    wanderpsi = 0;

    maxspeed = 0.5;

    maxforce = 0.05;

    fillColor = c;

    name = nam;

    angle=random(0, 6.28);

    angleVel = random(-0.1,0.1);

  }

 

  void update() {  // Method to update location

    velocity.add(acceleration);  // Update velocity

    velocity.limit(maxspeed);  // Limit speed

    location.add(velocity);  // Reset accelertion to 0 each cycle

    acceleration.mult(0);

  }

  void wander() {  // Method that makes the agent float into 2D space

    float wanderR = 25;  // Radius for our "wander circle"

    float wanderD = 200;  // Distance for our "wander circle"

    float change = 0.05;

    wandertheta += random(-change,change);  // Randomly change wander theta

    // calculating the new location to steer towards on the wander circle

    PVector circleloc = velocity.get();    // Start with velocity

    circleloc.normalize();   // Normalize to get heading

    circleloc.mult(wanderD);  // Multiply by distance

    circleloc.add(location);  // Make it relative to boid's location

    

    float h = velocity.heading2D();  // We need to know the heading to offset wandertheta

    PVector circleOffSet = new PVector(wanderR*cos(wandertheta+h),wanderR*sin(wandertheta+h));

    PVector target = PVector.add(circleloc,circleOffSet);

    seek(target);

  }  

  

  PVector wander3D() {

     float wanderR = 30.0;         // Radius for our "wander circle"

     float wanderD = 30.0;         // Distance for our "wander circle"

     float change = 0.5;

   

     wandertheta += random(-change, change);     // Randomly change wander theta

     wanderphi += random(-change, change);       // Randomly change wander phi

     wanderpsi += random(-change, change);       // Randomly change wander psi

     

     PVector circleloc = velocity.get();  // Start with velocity

     circleloc.normalize();            // Normalize to get heading

     circleloc.mult(wanderD);          // Multiply by distance

     circleloc.add(location);               // Make it relative to boid's location

     PVector circleOffSet = new PVector(wanderR*cos(wandertheta),wanderR*sin(wanderphi), wanderR*cos(wanderpsi));

     PVector target = PVector.add(circleloc,circleOffSet);

     return target; 

  }

  void applyForce(PVector force) {  // Newton's second law; We could add mass here if we want A = F / M

    acceleration.add(force);

  }

  // A method that calculates and applies a steering force towards a target

  // STEER = DESIRED MINUS VELOCITY

  void seek(PVector target) {

    PVector desired = PVector.sub(target,location);  // A vector pointing from the location to the target

    desired.normalize();  // Normalize desired and scale to maximum speed

    desired.mult(maxspeed);

    PVector steer = PVector.sub(desired,velocity);  // Steering = Desired minus Velocity

    steer.limit(maxforce);  // Limit to maximum steering force

    applyForce(steer);

  }

  

  // Method that lowers the speed of the agent by the time he reaches the target - see: "buffer zone"

  void arrive(PVector target) {

    PVector desired = PVector.sub(target,location);  // A vector pointing from the location to the target

    float d = desired.mag();

    desired.normalize();  // Normalize desired and scale with arbitrary damping within 100 pixels

    if (d < r) {

      float m = map(d,0,15,0,maxspeed);

      desired.mult(m);

    } else {

      desired.mult(maxspeed);

    }

    PVector steer = PVector.sub(desired,velocity);  // Steering = Desired minus Velocity

    steer.limit(5*maxforce);  // Limit to maximum steering force

    applyForce(steer);

  }

  

  void displayINFO() {

    advance();

    shapes2D();

  }

  

  void advance(){  // animates rotation for the buffer zone ellipse

    angle += angleVel; 

  }  

  

  void display() {  // Draws an ellipse that represents an agent-capsule

    fill(fillColor);

    noStroke();

    pushMatrix();

    translate(location.x, location.y, location.z);

    sphere(r/2);

    popMatrix();

  }

  

  void shapes2D() {

    /////////////////////////billboarding/////////////////////////

    fill(0,0,0);

    noStroke();

    pushMatrix();

    translate(location.x, location.y, location.z);

    sphere(r/2);

    float[] rota = cam.getRotations();

    rotateX(rota[0]);

    rotateY(rota[1]);

    rotateZ(rota[2]);

    fill(160);

    String myText = "ID: - give agent characteristics -" + "\nx: " + location.x + "\ny: " + location.y + "\nz: " + location.z;

    textSize(3);

    text(myText,location.x/4,location.y/6, location.z);

    

    stroke(160); fill(0,0,0,0);strokeWeight(1);

    ellipse(0,0,8,8);

    

    line(-4,-6,-10,-12);  // drawing the Info

    line(-10,-12,-16,-12);

    stroke(0,255,0); fill(0,255,0);strokeWeight(0.3);

    ellipse(-4,-6,1,1);

    text("buffer zone", -25, -12);

    

    rotate(angle);

    stroke(0); fill(0);strokeWeight(0);  // 2D shape representing the buffer zone

    ellipse(0,0,0,0);

    line(0,0,0,0);

    line(0,0,0,0);

    line(0,0,0,0);

    line(0,0,0,0);

    

    popMatrix();

    //////////////////////////////////////////////////////////////

  }

  

  // Method that checkes the maximum floating range of the agents - bouncing on the edges of the cube

  void boundaries() {

    if (location.x < 0 || location.x > worldSize) {

      velocity.x *=-1;

    }  

    if (location.y < 0 || location.y > worldSize) {

      velocity.y *=-1;

    }  

    

    if (location.z < 0 || location.z > worldSize) {

      velocity.z *=-1;

    }  

  }

  

  // Method that separates the agents from one-another -- see: "buffer zone"

  void separate (ArrayList<Capsule> agent) {

    float desiredseparation = r*4;  // 4*r = separation ONLY for wandering

    PVector sum = new PVector();

    int count = 0;

    

    // For every agent in the system, check if it's too close

    for (Capsule v : agent) {

      float d = PVector.dist(location, v.location);

      

      // If the distance is greater than 0 and less than an arbitrary amount (0 when you are yourself)

      if ((d > 0) && (d < desiredseparation)) {

        

        // Calculate vector pointing away from neighbor

        PVector diff = PVector.sub(location, v.location);

        diff.normalize();

        diff.div(d);        // Weight by distance

        sum.add(diff);

        count++;            // Keep track of how many

      }

    }

    // Average -- divide by how many

    if (count > 0) {

      sum.div(count);

      sum.normalize();  // Our desired vector is the average scaled to maximum speed

      sum.mult(maxspeed);

      

      // Implement Reynolds: Steering = Desired - Velocity

      PVector steer = PVector.sub(sum, velocity);

      steer.limit(maxforce);

      applyForce(steer);

    }

  }  

  

  // Method that separates the agents from one-another -- see: "buffer zone"

  void separateSpecial (ArrayList<Capsule> agent) {

    float desiredseparation = r;  //

    PVector sum = new PVector();

    int count = 0;

    

    // For every agent in the system, check if it's too close

    for (Capsule v : agent) {

      float d = PVector.dist(location, v.location);

      

      // If the distance is greater than 0 and less than an arbitrary amount (0 when you are yourself)

      if ((d > 0) && (d < desiredseparation)) {

        

        // Calculate vector pointing away from neighbor

        PVector diff = PVector.sub(location, v.location);

        diff.normalize();

        diff.div(d);        // Weight by distance

        sum.add(diff);

        count++;            // Keep track of how many

      }

    }

    // Average -- divide by how many

    if (count > 0) {

      sum.div(count);

      sum.normalize();  // Our desired vector is the average scaled to maximum speed

      sum.mult(maxspeed);

      

      // Implement Reynolds: Steering = Desired - Velocity

      PVector steer = PVector.sub(sum, velocity);

      steer.limit(maxforce);

      applyForce(steer);

    }

  }

  

  // Alignment

  // For every nearby agent in the system, calculate the average velocity

  PVector align (ArrayList<Capsule> agent) {

    float neighbordist = 50;

    PVector sum = new PVector(0,0);

    int count = 0;

    for (Capsule v : agent) {

      float d = PVector.dist(location,v.location);

      if ((d > 0) && (d < neighbordist)) {

        sum.add(v.velocity);

        count++;

      }

    }

    if (count > 0) {

      sum.div((float)count);

      sum.normalize();

      sum.mult(maxspeed);

      PVector steer = PVector.sub(sum,velocity);

      steer.limit(maxforce);

      return steer;

    } else {

      return new PVector(0,0);

    }

  }

}