work in progress

*/

​

class Segment {

  constructor(length, angle = 0, speed = 0.05, parent = null) {

    this.length = length;

    this.angle = angle;

    this.speed = speed; // Nieuwe speed parameter

    this.parent = parent;

    this.base = parent ? parent.end.copy() : createVector(0, 0);

    this.end = createVector(0, 0);

    this.calculateEnd();

  }

​

  calculateEnd() {

    let dx = this.length * cos(this.angle);

    let dy = this.length * sin(this.angle);

    this.end = p5.Vector.add(this.base, createVector(dx, dy));

  }

​

  follow(target) {

    let dir = p5.Vector.sub(target, this.base);

    let targetAngle = dir.heading();

    let distanceToTarget = this.end.dist(target);

​

    // Bepaal de dynamische snelheid op basis van de afstand tot het doel

    let dynamicSpeed;

    if (distanceToTarget > this.length) {

      dynamicSpeed = this.speed; // Maximale snelheid

    } else {

      dynamicSpeed = map(distanceToTarget, 0, this.length, 0.01, this.speed);

    }

    dynamicSpeed = constrain(dynamicSpeed, 0.01, this.speed);

​

    this.angle = lerp(this.angle, targetAngle, dynamicSpeed);

    dir.setMag(this.length);

    dir.mult(-1);

    this.base = p5.Vector.add(target, dir);

    this.calculateEnd();

  }

​

  setBase(newBase) {

    this.base = newBase.copy();

    this.calculateEnd();

  }

​

  update() {

    this.calculateEnd();

  }

​

  display() {

    stroke(0);

    strokeWeight(2);

    line(this.base.x, this.base.y, this.end.x, this.end.y);

  }

}

​

class Arm {

  constructor(config) {

    this.base = createVector(config.baseX, config.baseY);

    this.segments = [];

    this.minDistance = config.minDistance;

    this.maxPathPoints = config.pathPlanning.maxPathPoints;

    this.pathTolerance = config.pathPlanning.pathTolerance

    this.path = [];

    this.buildSegments(config.segments);

    this.target = false;

  }

​

  buildSegments(configs) {

    configs.forEach((config, index) => {

      let parent = index === 0 ? null : this.segments[index - 1];

      // Voeg de speed parameter toe vanuit de configuratie

      this.segments.push(

        new Segment(config.length, config.angle, config.speed, parent)

      );

    });

  }

  computePath(target) {

    this.path = [];

    let currentPoint = this.segments[this.segments.length - 1].end.copy();

    let totalLength = this.segments.reduce((sum, seg) => sum + seg.length, 0);

​

    while (p5.Vector.dist(currentPoint, target) > this.pathTolerance && this.path.length < this.maxPathPoints) {

      let direction = p5.Vector.sub(target, currentPoint).normalize();

      let nextPoint = p5.Vector.add(currentPoint, direction.mult(min(p5.Vector.dist(currentPoint, target), totalLength)));

      this.path.push(nextPoint);

      currentPoint = nextPoint.copy();

    }

​

    if (this.path.length === 0) {

      console.log("No path possible");

    }

  }

  reach(target) {

    if (!this.target || p5.Vector.dist(this.segments[this.segments.length - 1].end, target) < p5.Vector.dist(this.segments[this.segments.length - 1].end, this.target)) {

      this.computePath(target);

    }

​

    this.target = target;

​

    if (this.path.length > 0) {

      let nextPoint = this.path.shift();

      let endEffector = this.segments[this.segments.length - 1];

​

      endEffector.follow(target);

      endEffector.update();

​

    for (let i = this.segments.length - 2; i >= 0; i--) {

      this.segments[i].follow(this.segments[i + 1].base);

      this.segments[i].update();

    }

​

    this.segments[0].setBase(this.base);

​

    for (let i = 1; i < this.segments.length; i++) {

      this.segments[i].setBase(this.segments[i - 1].end);

    }

  }

  }

​

  display() {

    for (let segment of this.segments) {

      segment.display();

    }

  }

​

  displayDebugOverlay() {

    if (this.path.length > 0) {

      for (const p of this.path){

        const px = p.x.toFixed();

        const py = p.y.toFixed();

        text(`path next point: (${px}, ${py})`, this.base.x, this.base.y);

      stroke('rgba(6,244,6,0.62)')

      strokeWeight(1)

      const end_of_end = this.segments[this.segments.length-1].end;

​

      line(end_of_end.x, end_of_end.y, px, py);

      circle(px, py, 5);

      }

    } else {

      text("no path", this.base.x, this.base.y);

    }

​

    if (this.target) {

      text(

        `target: (${this.target.x.toFixed(1)}, ${this.target.y.toFixed(1)})`,

        this.base.x,

        this.base.y - 15

      );

    } else {

      text("no target", this.base.x, this.base.y - 15);

    }

    this.segments.forEach((segment, index) => {

      // Teken de basis en eindpunten van het segment

      fill(255, 0, 0);

      circle(segment.base.x, segment.base.y, 5);

      fill(0, 255, 0);

      circle(segment.end.x, segment.end.y, 5);

​

      // Toon de hoek en snelheid van het segment

      fill(0);

      textSize(12);

      text(

        `Segment ${index}: Angle = ${nf(segment.angle, 1, 2)}, Speed = ${nf(

          segment.speed,

          1,

          2

        )}`,

        segment.base.x + 10,

        segment.base.y + 10

      );

    });

  }

}

​

let arms = [];

let debugCheckBox;

​

function setup() {

  createCanvas(600, 600);

  frameRate(30);

  textAlign(CENTER);

​

  debugCheckBox = createCheckbox("debug").checked(true);

​

  let config = {

    arm: {

      baseX: width - width / 10,

      baseY: height - height / 10,

      minDistance: 25,

      segments: [

        { length: 100, angle: 0, speed: 0.0005 },

        { length: 50, angle: 0, speed: 0.0005 },

        { length: 25, angle: 0, speed: 0.0005 },

        { length: 25, angle: 0, speed: 0.0005 },

      ],

      pathPlanning: {

        maxPathPoints: 20,

        pathTolerance :10

      },

    },

  };

​

  config.arm.segments.forEach((e) => (e.angle = random(-TWO_PI, TWO_PI)));

  arms.push(new Arm(config.arm));

​

  config.arm.baseX = width / 10;

  config.arm.baseY = height / 10;

  config.arm.segments.reverse();

  config.arm.segments.forEach((e) => (e.angle = random(-TWO_PI, TWO_PI)));

  arms.push(new Arm(config.arm));

​

  config.arm.baseX = width / 10;

  config.arm.baseY = height - height / 10;

  config.arm.segments.reverse();

  config.arm.segments.forEach((e) => (e.angle = random(-TWO_PI, TWO_PI)));

  arms.push(new Arm(config.arm));

​

  config.arm.baseX = width - width / 10;

  config.arm.baseY = height / 10;

  config.arm.segments.reverse();

  config.arm.segments.forEach((e) => (e.angle = random(-TWO_PI, TWO_PI)));

  arms.push(new Arm(config.arm));

}

t = 0;

dt = 0.009;

​

function draw() {

  background(220);

  const radius = width / 3;

​

  let target = createVector(

    width / 2 + sin(t) * radius,

    height / 2 + cos(t) * radius

  );

​

  circle(target.x, target.y, 5);

​

  arms.forEach((arm) => {

    arm.reach(target);

    arm.display();

    if (debugCheckBox.checked()) arm.displayDebugOverlay();

  });

​

  t += dt;

}

​