210531a

function setup() {

  createCanvas(800, 800);

  colorMode(HSB, 360, 100, 100, 100);

  // angleMode(DEGREES);

}

​

function draw() {

  background(0, 0, 90);

​

  let offset = width / 10;

  let xMin = offset;

  let yMin = offset;

  let xMax = height - offset;

  let yMax = width - offset;

  let x = xMin;

  let y = yMin;

  let yDistance = yMax - yMin;

  let xDistance = xMax - xMin;

  let xStep, yStep;

​

  let start_x;

  let start_y;

  let start_yaw;

  let end_x;

  let end_y;

  let end_yaw;

  let curvature;

​

  while (y < yMax) {

    x = xMin;

    yStep = random(yDistance / 10, yDistance / 5);

    if (y + yStep > yMax || yMax - (y + yStep) < yMin) {

      yStep = yMax - y;

    }

    beginShape();

    start_x = x;

    start_y = y + yStep / 2 + (sin(radians(x)) * yStep) / 2;

    while (x < xMax) {

      xStep = random(xDistance / 15, xDistance / 5);

      if (x + xStep > xMax || xMax - (x + xStep) < xMin) {

        xStep = xMax - x;

      }

      end_x = x + xStep;

      end_y = y + yStep / 2 + (sin(radians(end_x)) * yStep) / 2;

      start_yaw = radians(180);

​

      end_yaw = radians(180);

      curvature = random(yStep / 5);

      let arrs = dubins_path_planning(

        start_x,

        start_y,

        start_yaw,

        end_x,

        end_y,

        end_yaw,

        curvature

      );

      let px = arrs[0];

      let py = arrs[1];

​

      for (let i = 0; i < px.length; i++) {

        let nx = px[i];

        let ny = py[i];

        vertex(nx, ny);

        // line(nx,ny,nx, y + yStep/2);

      }

      start_x = end_x;

      start_y = end_y;

​

      noFill();

      stroke(0, 0, 100);

      rect(x, y, xStep, yStep);

      stroke(0, 0, 0);

      x += xStep;

    }

    push();

    drawingContext.shadowColor = color(0, 0, 0, 50);

    drawingContext.shadowBlur = offset / 5;

    strokeWeight(2);

    endShape();

    pop();

    y += yStep;

  }

  // noLoop();

  frameRate(1);

}

​

//0〜TWO_PIで値を返す

function mod2pi(theta) {

  return theta - 2.0 * PI * floor(theta / 2.0 / PI);

}

​

function pi_2_pi(angle) {

  while (angle >= PI) {

    angle = angle - TWO_PI;

  }

  while (angle <= -PI) {

    angle = angle + TWO_PI;

  }

  return angle;

}

​

function LSL(alpha, beta, d) {

  let sa = sin(alpha);

  let sb = sin(beta);

  let ca = cos(alpha);

  let cb = cos(beta);

  let c_ab = cos(alpha - beta);

  let tmp0 = d + sa - sb;

​

  let mode = ["L", "S", "L"];

  let p_squared = 2 + d * d - 2 * c_ab + 2 * d * (sa - sb);

  if (p_squared < 0) {

    return [null, null, null, mode];

  }

  let tmp1 = atan2(cb - ca, tmp0);

  let t = mod2pi(-alpha + tmp1);

  let p = sqrt(p_squared);

  let q = mod2pi(beta - tmp1);

​

  return [t, p, q, mode];

}

​

function RSR(alpha, beta, d) {

  let sa = sin(alpha);

  let sb = sin(beta);

  let ca = cos(alpha);

  let cb = cos(beta);

  let c_ab = cos(alpha - beta);

​

  let tmp0 = d - sa + sb;

  let mode = ["R", "S", "R"];

  let p_squared = 2 + d * d - 2 * c_ab + 2 * d * (sb - sa);

  if (p_squared < 0) {

    return [null, null, null, mode];

  }

  let tmp1 = atan2(ca - cb, tmp0);

  let t = mod2pi(alpha - tmp1);

  let p = sqrt(p_squared);

  let q = mod2pi(-beta + tmp1);

​

  return [t, p, q, mode];

}

​

function LSR(alpha, beta, d) {

  let sa = sin(alpha);

  let sb = sin(beta);

  let ca = cos(alpha);

  let cb = cos(beta);

  let c_ab = cos(alpha - beta);

​

  let p_squared = -2 + d * d + 2 * c_ab + 2 * d * (sa + sb);

  let mode = ["L", "S", "R"];

  if (p_squared < 0) {

    return [null, null, null, mode];

  }

  let p = sqrt(p_squared);

  let tmp2 = atan2(-ca - cb, d + sa + sb) - atan2(-2.0, p);

  let t = mod2pi(-alpha + tmp2);

  let q = mod2pi(-mod2pi(beta) + tmp2);

​

  return [t, p, q, mode];

}

​

function RSL(alpha, beta, d) {

  let sa = sin(alpha);

  let sb = sin(beta);

  let ca = cos(alpha);

  let cb = cos(beta);

  let c_ab = cos(alpha - beta);

​

  let p_squared = d * d - 2 + 2 * c_ab - 2 * d * (sa + sb);

  let mode = ["R", "S", "L"];

  if (p_squared < 0) {

    return [null, null, null, mode];

  }

  let p = sqrt(p_squared);

  let tmp2 = atan2(ca + cb, d - sa - sb) - atan2(2.0, p);

  let t = mod2pi(alpha - tmp2);

  let q = mod2pi(beta - tmp2);

  return [t, p, q, mode];

}

​

function RLR(alpha, beta, d) {

  let sa = sin(alpha);

  let sb = sin(beta);

  let ca = cos(alpha);

  let cb = cos(beta);

  let c_ab = cos(alpha - beta);

​

  let mode = ["R", "L", "R"];

  let tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0;

  if (abs(tmp_rlr) > 1.0) {

    return [null, null, null, mode];

  }

​

  let p = mod2pi(2 * PI - acos(tmp_rlr));

  let t = mod2pi(alpha - atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0));

  let q = mod2pi(alpha - beta - t + mod2pi(p));

  return [t, p, q, mode];

}

​

function LRL(alpha, beta, d) {

  let sa = sin(alpha);

  let sb = sin(beta);

  let ca = cos(alpha);

  let cb = cos(beta);

  let c_ab = cos(alpha - beta);

​

  let mode = ["L", "R", "L"];

  let tmp_lrl = (6 - d * d + 2 * c_ab + 2 * d * (-sa + sb)) / 8;

  if (abs(tmp_lrl) > 1.0) {

    return [null, null, null, mode];

  }

  let p = mod2pi(2 * PI - acos(tmp_lrl));

  let t = mod2pi(-alpha - atan2(ca - cb, d + sa - sb) + p / 2);

  let q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p));

​

  return [t, p, q, mode];

}

​

function dubins_path_planning_from_origin(ex, ey, eyaw, c) {

  //# nomalize

  let dx = ex;

  let dy = ey;

  let e = sqrt(dx ** 2 + dy ** 2);

  let d = e / c;

  // print(dx, dy, e, d)

​

  let theta = mod2pi(atan2(dy, dx));

  let alpha = mod2pi(-theta);

  let beta = mod2pi(eyaw - theta);

​

  // print(theta, alpha, beta, d)

​

  let planners = [LSL, RSR, LSR, RSL, RLR, LRL];

  let bcost = Infinity;

  let bt;

  let bp;

  let bq;

  let bmode;

  for (planner of planners) {

    // print(planner);

    let arr = planner(alpha, beta, d);

    // print(arr);

    let t = arr[0];

    let p = arr[1];

    let q = arr[2];

    let mode = arr[3];

    if (t == null) {

      // print(mode + " cannot generate path");

      continue;

    }

​

    let cost = abs(t) + abs(p) + abs(q);

    if (bcost > cost) {

      bt = t;

      bp = p;

      bq = q;

      bmode = mode;

      bcost = cost;

    }

  }

​

  let arr2 = generate_course([bt, bp, bq], bmode, c);

​

  let px = arr2[0];

  let py = arr2[1];

  let pyaw = arr2[2];

  // print(px, py, pyaw, bmode, bcost);

​

  return [px, py, pyaw, bmode, bcost];

}

​

function generate_course(length, mode, c) {

  let px = [0];

  let py = [0];

  let pyaw = [0];

​

  for (let i = 0; i < length.length; i++) {

    let m = mode[i];

    let l = length[i];

    let pd = 0.0;

    if (m == "S") {

      d = 1.0 / c;

    } else {

      //# turning couse

      d = radians(1.0 / 2);

    }

    while (pd < abs(l - d)) {

      let pxv = px[px.length - 1] + d * c * cos(pyaw[pyaw.length - 1]);

      let pyv = py[py.length - 1] + d * c * sin(pyaw[pyaw.length - 1]);

      px.push(pxv);

      py.push(pyv);

      if (m == "L") pyaw.push(pyaw[pyaw.length - 1] + d);

      else if (m == "S") pyaw.push(pyaw[pyaw.length - 1]);

      else if (m == "R") pyaw.push(pyaw[pyaw.length - 1] - d);

      pd += d;

    }

    d = l - pd;

    px.push(px[px.length - 1] + d * c * cos(pyaw[pyaw.length - 1]));

    py.push(py[py.length - 1] + d * c * sin(pyaw[pyaw.length - 1]));

​

    if (m == "L") pyaw.push(pyaw[pyaw.length - 1] + d);

    else if (m == "S") pyaw.push(pyaw[pyaw.length - 1]);

    else if (m == "R") pyaw.push(pyaw[pyaw.length - 1] - d);

    pd += d;

  }

  return [px, py, pyaw];

}

​

function dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c) {

  // """

  // Dubins path plannner

  // input:

  //     sx x position of start point [m]

  //     sy y position of start point [m]

  //     syaw yaw angle of start point [rad]

  //     ex x position of end point [m]

  //     ey y position of end point [m]

  //     eyaw yaw angle of end point [rad]

  //     c curvature [1/m]

  // output:

  //     px

  //     py

  //     pyaw

  //     mode

  // """

​

  ex = ex - sx;

  ey = ey - sy;

​

  let lex = cos(syaw) * ex + sin(syaw) * ey;

  let ley = -sin(syaw) * ex + cos(syaw) * ey;

  let leyaw = eyaw - syaw;

  let arr = dubins_path_planning_from_origin(lex, ley, leyaw, c);

  // print(arr);

  let lpx, lpy, lpyaw, mode, clen;

  lpx = arr[0];

  lpy = arr[1];

  lpyaw = arr[2];

  mode = arr[3];

  clen = arr[4];

​

  let px = [];

  let py = [];

  let pyaw = [];

​

  for (let i = 0; i < lpx.length; i++) {

    let x = lpx[i];

    let y = lpy[i];

    px.push(cos(-syaw) * x + sin(-syaw) * y + sx);

    py.push(-sin(-syaw) * x + cos(-syaw) * y + sy);

  }

  for (let i = 0; i < lpyaw.length; i++) {

    iyaw = lpyaw[i];

    pyaw.push(pi_2_pi(iyaw + syaw));

  }

  //     #  print(syaw)

  //     #  pyaw = lpyaw

​

  //     #  plt.plot(pyaw, "-r")

  //     #  plt.plot(lpyaw, "-b")

  //     #  plt.plot(eyaw, "*r")

  //     #  plt.plot(syaw, "*b")

  //     #  plt.show()

​

  return [px, py, pyaw, mode, clen];

}