ryosuzuki · October 22, 2023 18:29
diff --git a/rvo.js b/rvo.js
 /*
 * RVO Demonstration
 * cf. http://gamma.cs.unc.edu/RVO/icra2008.pdf
 */

 // parameters
 const acceleration = 1;
 const avoidanceTendency = 10;

 class Agent {
  constructor(x, y, tx, ty, v) {
    this.size = 10;
    this.x = x;
    this.y = y;
    this.targetX = tx;
    this.targetY = ty;
    this.preferredSpeed = v;
    this.velocityX = 0;
    this.velocityY = 0;
    this.history = [];
  }
  
  setVelocity(vx, vy) {
    this.velocityX = vx;
    this.velocityY = vy;
  }

  update(dt) {
    this.x += dt * this.velocityX;
    this.y += dt * this.velocityY;
    this.history.push(this.x, this.y);
  }
 }

 class Simulator {
  constructor(canvas) {
    this.canvas = canvas;
    this.ctx = canvas.getContext('2d');
    this.agents = [];
  }
  
  getCollisionTime(index, vx, vy) {
    let tmin = Infinity;
    const a = this.agents[index];

    for (let i = 0; i < this.agents.length; i++) {
      if (i == index) continue;

      const b = this.agents[i];
      const ux = 2 * vx - a.velocityX - b.velocityX;
      const uy = 2 * vy - a.velocityY - b.velocityY;
      const dx = b.x - a.x;
      const dy = b.y - a.y;
      const s = a.size + b.size;
      const c2 = ux * ux + uy * uy;
      const c1 = -2 * (ux * dx + uy * dy);
      const c0 = dx * dx + dy * dy - s * s;
      
      let t = Infinity;
      if (c2 == 0) {
        t = -c0 / c1;
      }
      else {
        const discriminant = c1 * c1 - 4 * c2 * c0;
        if (discriminant >= 0) {
          const sq = Math.sqrt(discriminant);
          const t1 = (-c1 - sq) / (2 * c2);
          const t2 = (-c1 + sq) / (2 * c2);
          if (c0 < 0) {
            t = 0;  // Already collided!
          }
          else if (c1 <= 0) {
            t = t1;
          }
        }
      }

      if (t < tmin) {
        tmin = t;
      }
    }
    return tmin;
  }
  
  getRvoVelocity(index, dt) {
    const accel = acceleration;
    const w = avoidanceTendency;

    const a = this.agents[index];
    let preferredVx = a.targetX - a.x;
    let preferredVy = a.targetY - a.y;
    const l = len(preferredVx, preferredVy);
    if (l > 1) {
      preferredVx *= a.preferredSpeed / l;
      preferredVy *= a.preferredSpeed / l;
    }

    const scale = 20;
    this.ctx.fillRect(a.x + scale * preferredVx - 1, a.y + scale * preferredVy - 1, 3, 3);
    
    let rvoVx = preferredVx;
    let rvoVy = preferredVy;
    let min = Infinity;

    for (let i = 0; i < 100; i++) {
      const vx = a.velocityX + accel * dt * (2 * Math.random() - 1);
      const vy = a.velocityY + accel * dt * (2 * Math.random() - 1);
      const collisionTime = this.getCollisionTime(index, vx, vy);
      const penalty = w / collisionTime + len(vx - preferredVx, vy - preferredVy);
      if (penalty < min) {
        rvoVx = vx;
        rvoVy = vy;
        min = penalty;
      }
    }

    this.ctx.beginPath();
    this.ctx.moveTo(a.x, a.y);
    this.ctx.lineTo(a.x + scale * rvoVx, a.y + scale * rvoVy);
    this.ctx.stroke();

    return { x: rvoVx, y: rvoVy };
  }
  
  draw() {
    this.ctx.clearRect(0, 0, this.canvas.width, this.canvas.height);

    const dt = 1;
    for (let i = 0; i < this.agents.length; i++) {
      const a = this.agents[i];
      const v = this.getRvoVelocity(i, dt);
      a.setVelocity(v.x, v.y);
      
      a.update(dt);
      this.ctx.beginPath();
      this.ctx.arc(a.x, a.y, a.size, 2*Math.PI, false);
      this.ctx.stroke();
      
      this.ctx.strokeStyle = hsv(i / this.agents.length, 1, 1);
      this.ctx.beginPath();
      this.ctx.moveTo(a.history[0], a.history[1]);
      for (let j = 1; j < a.history.length / 2; j++) {
        this.ctx.lineTo(a.history[2 * j], a.history[2 * j + 1]);
      }
      this.ctx.stroke();

      this.ctx.strokeStyle = '#000';
    }
  }
 }

 let sim;

 function init() {
  sim = new Simulator(document.getElementById('canvas'));
  
  const cx = sim.canvas.width / 2;
  const cy = sim.canvas.height / 2;
  const r = 100;
  const n = parseInt(document.getElementById('num').value);
  for (let i = 0; i < n; i++) {
    const t = 2 * Math.PI * i / n;
    const x = r * Math.cos(t);
    const y = r * Math.sin(t);
    sim.agents.push(new Agent(cx + x, cy + y, cx - x, cy - y, 1));
  }

  sim.draw();
 }


 function len(x, y) {
  return Math.sqrt(x * x + y * y);
 }

 // https://stackoverflow.com/questions/17242144/javascript-convert-hsb-hsv-color-to-rgb-accurately
 function hsv(h, s, v) {
  const i = Math.floor(h * 6);
  const f = h * 6 - i;
  const p = v * (1 - s);
  const q = v * (1 - f * s);
  const t = v * (1 - (1 - f) * s);
  let r, g, b;
  switch (i % 6) {
    case 0: r = v, g = t, b = p; break;
    case 1: r = q, g = v, b = p; break;
    case 2: r = p, g = v, b = t; break;
    case 3: r = p, g = q, b = v; break;
    case 4: r = t, g = p, b = v; break;
    case 5: r = v, g = p, b = q; break;
  }
  r = Math.round(255 * r);
  g = Math.round(255 * g);
  b = Math.round(255 * b);
  return `rgb(${r},${g},${b})`;
 }


 init();

 let interval;

 document.getElementById('reset').addEventListener('click', () => { init(); });
 document.getElementById('start').addEventListener('click', () => { if (!interval) interval = setInterval(() => { sim.draw(); }, 10); });
 document.getElementById('step').addEventListener('click', () => { sim.draw(); });
 document.getElementById('stop').addEventListener('click', () => { clearInterval(interval); interval = 0; });
	/*
	* RVO Demonstration
	* cf. http://gamma.cs.unc.edu/RVO/icra2008.pdf
	*/

	// parameters
	const acceleration = 1;
	const avoidanceTendency = 10;

	class Agent {
	constructor(x, y, tx, ty, v) {
	this.size = 10;
	this.x = x;
	this.y = y;
	this.targetX = tx;
	this.targetY = ty;
	this.preferredSpeed = v;
	this.velocityX = 0;
	this.velocityY = 0;
	this.history = [];
	}

	setVelocity(vx, vy) {
	this.velocityX = vx;
	this.velocityY = vy;
	}

	update(dt) {
	this.x += dt * this.velocityX;
	this.y += dt * this.velocityY;
	this.history.push(this.x, this.y);
	}
	}

	class Simulator {
	constructor(canvas) {
	this.canvas = canvas;
	this.ctx = canvas.getContext('2d');
	this.agents = [];
	}

	getCollisionTime(index, vx, vy) {
	let tmin = Infinity;
	const a = this.agents[index];

	for (let i = 0; i < this.agents.length; i++) {
	if (i == index) continue;

	const b = this.agents[i];
	const ux = 2 * vx - a.velocityX - b.velocityX;
	const uy = 2 * vy - a.velocityY - b.velocityY;
	const dx = b.x - a.x;
	const dy = b.y - a.y;
	const s = a.size + b.size;
	const c2 = ux * ux + uy * uy;
	const c1 = -2 * (ux * dx + uy * dy);
	const c0 = dx * dx + dy * dy - s * s;

	let t = Infinity;
	if (c2 == 0) {
	t = -c0 / c1;
	}
	else {
	const discriminant = c1 * c1 - 4 * c2 * c0;
	if (discriminant >= 0) {
	const sq = Math.sqrt(discriminant);
	const t1 = (-c1 - sq) / (2 * c2);
	const t2 = (-c1 + sq) / (2 * c2);
	if (c0 < 0) {
	t = 0; // Already collided!
	}
	else if (c1 <= 0) {
	t = t1;
	}
	}
	}

	if (t < tmin) {
	tmin = t;
	}
	}
	return tmin;
	}

	getRvoVelocity(index, dt) {
	const accel = acceleration;
	const w = avoidanceTendency;

	const a = this.agents[index];
	let preferredVx = a.targetX - a.x;
	let preferredVy = a.targetY - a.y;
	const l = len(preferredVx, preferredVy);
	if (l > 1) {
	preferredVx *= a.preferredSpeed / l;
	preferredVy *= a.preferredSpeed / l;
	}

	const scale = 20;
	this.ctx.fillRect(a.x + scale * preferredVx - 1, a.y + scale * preferredVy - 1, 3, 3);

	let rvoVx = preferredVx;
	let rvoVy = preferredVy;
	let min = Infinity;

	for (let i = 0; i < 100; i++) {
	const vx = a.velocityX + accel * dt * (2 * Math.random() - 1);
	const vy = a.velocityY + accel * dt * (2 * Math.random() - 1);
	const collisionTime = this.getCollisionTime(index, vx, vy);
	const penalty = w / collisionTime + len(vx - preferredVx, vy - preferredVy);
	if (penalty < min) {
	rvoVx = vx;
	rvoVy = vy;
	min = penalty;
	}
	}

	this.ctx.beginPath();
	this.ctx.moveTo(a.x, a.y);
	this.ctx.lineTo(a.x + scale * rvoVx, a.y + scale * rvoVy);
	this.ctx.stroke();

	return { x: rvoVx, y: rvoVy };
	}

	draw() {
	this.ctx.clearRect(0, 0, this.canvas.width, this.canvas.height);

	const dt = 1;
	for (let i = 0; i < this.agents.length; i++) {
	const a = this.agents[i];
	const v = this.getRvoVelocity(i, dt);
	a.setVelocity(v.x, v.y);

	a.update(dt);
	this.ctx.beginPath();
	this.ctx.arc(a.x, a.y, a.size, 2*Math.PI, false);
	this.ctx.stroke();

	this.ctx.strokeStyle = hsv(i / this.agents.length, 1, 1);
	this.ctx.beginPath();
	this.ctx.moveTo(a.history[0], a.history[1]);
	for (let j = 1; j < a.history.length / 2; j++) {
	this.ctx.lineTo(a.history[2 * j], a.history[2 * j + 1]);
	}
	this.ctx.stroke();

	this.ctx.strokeStyle = '#000';
	}
	}
	}

	let sim;

	function init() {
	sim = new Simulator(document.getElementById('canvas'));

	const cx = sim.canvas.width / 2;
	const cy = sim.canvas.height / 2;
	const r = 100;
	const n = parseInt(document.getElementById('num').value);
	for (let i = 0; i < n; i++) {
	const t = 2 * Math.PI * i / n;
	const x = r * Math.cos(t);
	const y = r * Math.sin(t);
	sim.agents.push(new Agent(cx + x, cy + y, cx - x, cy - y, 1));
	}

	sim.draw();
	}


	function len(x, y) {
	return Math.sqrt(x * x + y * y);
	}

	// https://stackoverflow.com/questions/17242144/javascript-convert-hsb-hsv-color-to-rgb-accurately
	function hsv(h, s, v) {
	const i = Math.floor(h * 6);
	const f = h * 6 - i;
	const p = v * (1 - s);
	const q = v * (1 - f * s);
	const t = v * (1 - (1 - f) * s);
	let r, g, b;
	switch (i % 6) {
	case 0: r = v, g = t, b = p; break;
	case 1: r = q, g = v, b = p; break;
	case 2: r = p, g = v, b = t; break;
	case 3: r = p, g = q, b = v; break;
	case 4: r = t, g = p, b = v; break;
	case 5: r = v, g = p, b = q; break;
	}
	r = Math.round(255 * r);
	g = Math.round(255 * g);
	b = Math.round(255 * b);
	return `rgb(${r},${g},${b})`;
	}


	init();

	let interval;

	document.getElementById('reset').addEventListener('click', () => { init(); });
	document.getElementById('start').addEventListener('click', () => { if (!interval) interval = setInterval(() => { sim.draw(); }, 10); });
	document.getElementById('step').addEventListener('click', () => { sim.draw(); });
	document.getElementById('stop').addEventListener('click', () => { clearInterval(interval); interval = 0; });