2d-pbd experiment - run with budo

2d-pbd.js

const ctx = require('fc')(render, 1)
const center = require('ctx-translate-center')
const {vec2, vec3, mat2, mat3 } = require('gl-matrix')
const renderGrid = require('ctx-render-grid-lines')
const segseg = require('segseg')
const ndarray = require('ndarray')
const raySlab = require('ray-aabb-slab')

const particles = []
const constraints = []
const v2scratch = [0, 0]
const constraintMap = {
  distance(constraint) {
    const a = particles[constraint.pair[0]]
    const b = particles[constraint.pair[1]]
    if (!a || !b) {
      return
    }
    const dir = [0, 0]
    vec2.normalize(dir, vec2.subtract(dir, a, b))

    // TODO: currently we only support particles with mass=1
    const massSum = 2.0;

    const d = vec2.distance(a, b)

    // TODO: we should probably return some sort of encoding here so we can
    //       constrain against multiple things, but for the time being we'll
    //       just blindly apply a transformation immediately in a pairwise
    //       fashion

    var delta = 0.0
    if (d < constraint.value) {
      delta = constraint.compressionStiffness * (d - constraint.value) / massSum;
    } else {
      delta = constraint.stretchStiffness * (d - constraint.value) / massSum;
    }

    // TODO: this is where we'd apply the mass
    vec2.subtract(v2scratch, a, vec2.set(v2scratch, dir[0] * delta, dir[1] * delta))
    grid.moveParticle(constraint.pair[0], v2scratch)

    vec2.add(v2scratch, b, vec2.set(v2scratch, dir[0] * delta, dir[1] * delta))
    grid.moveParticle(constraint.pair[1], v2scratch)

  },
  fixed(c) {
    grid.moveParticle(c.pair[0], c.value)
  },
  // point on line works
  pointOnLine(c) {
    const point = particles[c.pair[0]]
    const start = particles[c.pair[1]]
    const end = particles[c.pair[2]]

    const px = point[0]
    const py = point[1]
    const x1 = start[0]
    const y1 = start[1]
    const x2 = end[0]
    const y2 = end[1]
    const stiffness = c.stiffness

    var dx = x2 - x1;
    var dy = y2 - y1;

    var m = dy / dx; //Slope
    var n = dx / dy; //1/Slope

    var dx = 0.0;
    var dy = 0.0;

    if(m <= 1 && m >= -1) {
      //Calculate the expected y point given the x coordinate of the point
      const y = y1 + m * (px - x1);
      dx = y - py;
      v2scratch[0] = y
    } else {
      const x = x1 + n * (py - y1);
      var dx = x - px;
      v2scratch[0] = x
    }

    // TODO: in order for this to work we have to move each point to be
    //       on the line. I think the two endpoints move less than the center

    // v2scratch[0] = point[0] + dx// * 0.75
    // v2scratch[1] = point[1] + dy// * 0.75
    grid.moveParticle(c.pair[0], v2scratch)

    // v2scratch[0] = start[0] + dx * 0.125
    // v2scratch[1] = start[1] + dy * 0.125
    // grid.moveParticle(c.pair[1], v2scratch)
    //
    // v2scratch[0] = end[0] + dx * 0.125
    // v2scratch[1] = end[1] + dy * 0.125
    // grid.moveParticle(c.pair[2], v2scratch)
  }
}

var dragConstraint = null

const particleRadius = 10.0;
const mouse = [0, 0];
var hoveredParticle = -1
var dragParticle = null
const floor = Math.floor
const oldPosScratch = [0, 0]
const grid = {
  array: ndarray([], [64, 64]),
  cellSize: particleRadius,
  newParticle(pos) {
    const idx = particles.length
    particles.push(pos)
    this.add(idx, pos)
    return idx
  },

  add(index) {
    const pos = particles[index]
    var v = this.cell(pos)
    if (!v) {
      v = []
      this.cell(pos, v)
    }

    v.push(index)
  },

  cell(pos, v) {
    if (v) {
      return this.array.set(
        floor(pos[0]/this.cellSize + this.array.shape[0]/2),
        floor(pos[1]/this.cellSize + this.array.shape[1]/2),
        v
      )
    } else {
      return this.array.get(
        floor(pos[0]/this.cellSize + this.array.shape[0]/2),
        floor(pos[1]/this.cellSize + this.array.shape[1]/2)
      )
    }
  },

  remove(index, pos) {
    //const pos = particles[index]
    const v = this.cell(pos)
    if (!v) {
      return
    }
    const idx = v.indexOf(index)
    if (!Array.isArray(v) || idx === -1) {
      return
    }

    v.splice(idx, 1)
  },

  moveParticle(index, newPos) {
    const pos = particles[index]
    const sameCell = floor(pos[0]) === floor(newPos[0]) && floor(pos[1]) === floor(newPos[1])

    oldPosScratch[0] = pos[0]
    oldPosScratch[1] = pos[1]


    pos[0] = newPos[0]
    pos[1] = newPos[1]

    if (sameCell) {
      return
    }

    this.remove(index, oldPosScratch)
    this.add(index)
  }
}

const camera = require('ctx-camera')(ctx, window, {
  mousemove(e, defaultFn) {
    // make sure we keep tracking this.mouse state!
    defaultFn(e)
    if (!dragParticle) {
      return
    }

    // particles[dragParticle.id][1] = mouse[1] + dragParticle.offset[1]
    // particles[dragParticle.id][0] = mouse[0] + dragParticle.offset[0]
    dragConstraint.value[0] = mouse[0] + dragParticle.offset[0]
    dragConstraint.value[1] = mouse[1] + dragParticle.offset[1]
  },

  mousedown(e, defaultFn) {
    if (hoveredParticle < 0 || dragConstraint) {
      return defaultFn(e)
    }

    const p = particles[hoveredParticle]

    dragParticle = {
      id: hoveredParticle,
      offset: [
        p[0] - mouse[0],
        p[1] - mouse[1]
      ]
    }

    dragConstraint = {
      pair: [hoveredParticle],
      type: 'fixed',
      value: [mouse[0], mouse[1]],
      strenth: 10000
    }
  },

  mouseup(e, defaultFn) {
    dragParticle = null
    dragConstraint = null
    defaultFn(e)
  }
})

const TAU = Math.PI*2

const lineTo = ctx.lineTo.bind(ctx)
const moveTo = ctx.moveTo.bind(ctx)
const arc = ctx.arc.bind(ctx)
const translate = ctx.translate.bind(ctx)

const view = mat3.create()
const model = mat3.create()
const invModel = mat3.create()
const v2tmp = vec2.create()
const v2tmp2 = vec2.create()
const v2dir = vec2.create()
const v2offset = vec2.create()
const v2gridPos = vec2.create()

var dims = [8, 8]
var spacing = particleRadius;

const cornerDist = vec2.length([particleRadius, particleRadius])
const strength = 1.0

const solids = []

for (var y = 0; y<5; y++) {
  var start = particles.length
  var end = start-1
  for (var x = 0; x<spacing*10; x+=spacing) {
    /*
      (1)   (2)

      (0)   (3)
    */
    grid.newParticle([x, y * spacing*3])
    grid.newParticle([x, y * spacing*3 + spacing])
    end+=2
  }
  solids.push([start, end])

  for (var l = start; l<particles.length; l++) {
    if (l < particles.length - 2 && l-start >= 0) {
      addTriangleConstraint(l + 0, l + 1, l + 2)
      if (l-start > 1) {
        addTriangleConstraint(l + 0, l + 1, l - 2)
      }
    }

    if (l-start > 2) {
       addTriangleConstraint(l, l - 2, l - 1)
      if (l % 2 === 1) {
        addTriangleConstraint(l, l - 3, l - 2)
      } else if (l < particles.length - 2) {
        addTriangleConstraint(l, l + 3, l + 1)
      }
    }
  }
}


function addTriangleConstraint(tp1, tp2, tp3) {
  const p0 = particles[tp1]
  const p1 = particles[tp2]
  const p2 = particles[tp3]
  const eps = 0.0000001

  function setup() {
    const a = p1[0] - p0[0];
    const b = p2[0] - p0[0];
    const c = p1[1] - p0[1];
    const d = p2[1] - p0[1];

    // inverse
    const det = a*d - b*c

    if (Math.abs(det) < eps) {
      console.warn("det", det, tp1, tp2, tp3)
      return false
    }

    const s = 1.0 / det;
    return mat2.fromValues(
       d * s,
      -b * s,
      -c * s,
       a * s
    )
  }

  const invRestMat = setup()
  if (!invRestMat) {
    return console.warn("addTriangleConstraint: could not compute invRestMat")
  }
  invRestMat.get = function(x, y) {
    return invRestMat[x * 2 + y]
  }

  // scratch for fn
  const c = [vec2.create(), vec2.create()]
  const d = [vec3.create(), vec3.create(), vec3.create()];
  const r = [vec2.create(), vec2.create(), vec2.create()]
  const corr0 = vec3.create()
  const corr1 = vec3.create()
  const corr2 = vec3.create()
  const tmat3 = mat3.create()
  const v3scratch = vec3.create()

  const invMass0 = 10.0
  const invMass1 = 10.0
  const invMass2 = 10.0
  const normalizeStretch = true
  const normalizeShear = true
  const xxStiffness = 1.0
  const xyStiffness = 1.0
  const yyStiffness = 1.0

  function clean(v2) {
    if (Math.abs(v2[0]) < eps) {
      v2[0] = 0.0
    }
    if (Math.abs(v2[1]) < eps) {
      v2[1] = 0.0
    }
    return v2
  }

  constraints.push({
    pair: [tp1, tp2, tp3],
    type: 'distance',
    value: particleRadius*2,
    compressionStiffness: strength,
    stretchStiffness: strength,
    fn() {
      vec2.set(c[0], invRestMat.get(0, 0), invRestMat.get(1, 0))
      vec2.set(c[1], invRestMat.get(0, 1), invRestMat.get(1, 1))

      vec3.set(corr0, 0, 0, 0)
      vec3.set(corr1, 0, 0, 0)
      vec3.set(corr2, 0, 0, 0)

      for (var i = 0; i < 2; i++) {
        for (var j = 0; j <= i; j++) {
          vec2.set(r[0],
            (p1[0] + corr1[0]) - (p0[0] + corr0[0]),
            (p2[0] + corr2[0]) - (p0[0] + corr0[0])
          );
          vec2.set(r[1],
            (p1[1] + corr1[1]) - (p0[1] + corr0[1]),
            (p2[1] + corr2[1]) - (p0[1] + corr0[1])
          );
          // vec2.set(r[2],
          //   (p1[2] + corr1[2]) - (p0[2] + corr0[2]),
          //   (p2[2] + corr2[2]) - (p0[2] + corr0[2])
          // );


          var Sij = 0.0;
          for (var k = 0; k < 3; k++) {
            Sij += vec2.dot(r[k], c[i]) * vec2.dot(r[k], c[j]);
          }

          vec3.set(d[0], 0, 0, 0)
          for (var k = 0; k < 2; k++) {
            var ki = invRestMat.get(k, i)
            var kj = invRestMat.get(k, j)
            vec2.set(d[k+1],
              (vec2.dot(r[0], c[j]) * ki) + (vec2.dot(r[0], c[i]) * kj),
              (vec2.dot(r[1], c[j]) * ki) + (vec2.dot(r[1], c[i]) * kj),
              (vec2.dot(r[2], c[j]) * ki) + (vec2.dot(r[2], c[i]) * kj)
            );

            vec2.subtract(d[0], d[0], d[k+1]);
          }

          if (i != j && normalizeShear) {
            var fi2 = 0.0
            var fj2 = 0.0
            for (var k = 0; k < 3; k++) {
              fi2 += vec2.dot(r[k], c[i]) * vec2.dot(r[k], c[i]);
              fj2 += vec2.dot(r[k], c[j]) * vec2.dot(r[k], c[j]);
            }
            var fi = Math.sqrt(fi2);
            var fj = Math.sqrt(fj2);

            vec3.set(d[0], 0, 0, 0)
            var s = Sij / (fi2*fi*fj2*fj);
            mat3.set(
              tmat3,
              invRestMat[0],
              invRestMat[1],
              0,
              invRestMat[2],
              invRestMat[3],
              0,
              0,
              0,
              0
            )

            for (var k = 0; k < 2; k++) {
              vec3.scale(d[k+1], d[k+1], 1.0 / fi * fj);
              // d[k+1] -= fj*fj * Vector3r(r[0].dot(c[i]), r[1].dot(c[i]), r[2].dot(c[i])) * invRestMat(k, i) * s;
              vec3.subtract(d[k+1], d[k+1], vec3.set(v3scratch,
                 fj*fj * vec2.dot(r[0], c[i]) * invRestMat.get(k, i) * s,
                 fj*fj * vec2.dot(r[1], c[i]) * invRestMat.get(k, i) * s,
                 fj*fj * vec2.dot(r[2], c[i]) * invRestMat.get(k, i) * s
              ))

              // d[k+1] -= fi*fi * Vector3r(r[0].dot(c[j]), r[1].dot(c[j]), r[2].dot(c[j])) * invRestMat(k, j) * s;
              vec3.subtract(d[k+1], d[k+1], vec3.set(v3scratch,
                 fj*fj * vec2.dot(r[0], c[j]) * invRestMat.get(k, j) * s,
                 fj*fj * vec2.dot(r[1], c[j]) * invRestMat.get(k, j) * s,
                 fj*fj * vec2.dot(r[2], c[j]) * invRestMat.get(k, j) * s
              ))

              vec3.subtract(d[0], d[0], d[k+1]);
            }
            Sij = Sij / (fi * fj);
          }

          var lambda =
            invMass0 * vec3.squaredLength(d[0]) +
            invMass1 * vec3.squaredLength(d[1]) +
            invMass2 * vec3.squaredLength(d[2]);

          if (lambda == 0.0) {
            continue;
          }

          if (i == 0 && j == 0) {
            if (normalizeStretch) {
              var s = Math.sqrt(Sij);
              lambda = 2.0 * s * (s - 1.0) / lambda * xxStiffness;
            }
            else {
              lambda = (Sij - 1.0) / lambda * xxStiffness;
            }
          }
          else if (i == 1 && j == 1) {
            if (normalizeStretch) {
              var s = Math.sqrt(Sij);
              lambda = 2.0 * s * (s - 1.0) / lambda * yyStiffness;
            }
            else {
              lambda = (Sij - 1.0) / lambda * yyStiffness;
            }
          }
          else {
            lambda = Sij / lambda * xyStiffness;
          }

          vec2.set(corr0,
            (corr0[0] - lambda * invMass0 * d[0][0]) / 2.0,
            (corr0[1] - lambda * invMass0 * d[0][1]) / 2.0
          )
          vec2.set(corr1,
            (corr1[0] - lambda * invMass1 * d[1][0]) / 2.0,
            (corr1[1] - lambda * invMass1 * d[1][1]) / 2.0
          )
          vec2.set(corr2,
            (corr2[0] - lambda * invMass2 * d[2][0]) / 2.0,
            (corr2[1] - lambda * invMass2 * d[2][1]) / 2.0
          )

          vec2.add(p0, p0, clean(corr0))
          vec2.add(p1, p1, clean(corr1))
          vec2.add(p2, p2, clean(corr2))

          // TODO: apply the correction to the particles

        }
      }
    }
  })
}

function collectContactConstraints() {
  var contactConstraints = []
  for (var i=0; i<particles.length; i++) {
    for (var j=i+1; j<particles.length; j++) {
      contactConstraints.push({
        pair: [i, j],
        type: 'distance',
        value: particleRadius,
        compressionStiffness: 1.0,
        stretchStiffness: 0
      })
    }
  }

  // TODO: this is only slightly more optimal, but breaks due to not looking at
  //       neighboring cells
  // var contactConstraints = []
  // for (var x=0; x<grid.array.shape[0]; x++) {
  //   for (var y=0; y<grid.array.shape[1]; y++) {
  //     var v = grid.array.get(x, y)
  //     if (!v || !v.length) {
  //       continue
  //     }
  //
  //     // TODO: this is a stupid slow hack
  //     for (var i=0; i<v.length; i++) {
  //       var firstIdx = v[i]
  //       for (var j=i+1; j<v.length; j++) {
  //         contactConstraints.push({
  //           pair: [firstIdx, v[j]],
  //           type: 'distance',
  //           value: particleRadius,
  //           compressionStiffness: 1.0,
  //           stretchStiffness: 0
  //         })
  //       }
  //     }
  //   }
  // }
  return contactConstraints
}

// TODO: make this shape aware..
// const avoidanceConstraints = collectContactConstraints()
const avoidanceConstraints = []

// setup avoidance on other solids
solids.forEach((s) => {
  solids.forEach((other) => {
    if (s === other) {
      return
    }

    for (var i=s[0]; i<=s[1]; i++) {
      for (var j=other[0]; j<=other[1]; j++) {
        avoidanceConstraints.push({
          pair: [i, j],
          type: 'distance',
          value: particleRadius,
          compressionStiffness: 1.0,
          stretchStiffness: 0
        })
      }
    }
  })
})


function step(iters) {
  iters = iters || 5;
  for (var i=0; i<iters; i++) {
    for (var j=0; j<constraints.length; j++) {
      var c = constraints[j]

      if (c.fn) {
        c.fn(ctx)
        continue;
      }

      if (!constraintMap[c.type]) {
        return console.warn("constraint type", c.type, "not defined")
      }

      constraintMap[c.type](c)
    }

    avoidanceConstraints.forEach((c) => {
      if (!constraintMap[c.type]) {
        return console.warn("constraint type", c.type, "not defined")
      }

      constraintMap[c.type](c)
    })



    if (dragConstraint && constraintMap[dragConstraint.type]) {
      constraintMap[dragConstraint.type](dragConstraint)
    }
  }
}

function render() {
  ctx.clear()
  ctx.fillStyle = "white"
  ctx.font = "22px monospace"
  ctx.fillText(`constraints: ${constraints.length + avoidanceConstraints.length}`, 10, 30)

  camera.begin()
    center(ctx)
    ctx.scale(2.0, -2.0)

    step(5)


    ctx.pointToWorld(mouse, camera.mouse.pos)

    ctx.save()
      ctx.lineWidth = 1.0

      ctx.fillStyle = "red"
      hoveredParticle = -1
      particles.forEach((p, i) => {
        ctx.beginPath()
          ctx.arc(p[0], p[1], particleRadius / 2.0, 0, Math.PI*2)

          if (vec2.distance(mouse, p) <= 5) {
            hoveredParticle = i
          }

          // if (i == centerIdx) {
          //   ctx.fillStyle = "yellow"
          // } else {
          ctx.fillStyle = hsl(i/particles.length * 0.9)
          ctx.fill();

          if (hoveredParticle === i || (dragParticle && dragParticle.id === i)) {
            ctx.beginPath()
              ctx.arc(p[0], p[1], particleRadius * 0.65, 0, Math.PI*2)
            ctx.strokeStyle = "#ccc"
            ctx.save()
            ctx.lineWidth = 1;
            ctx.stroke()
            ctx.restore()
          }
          // }

      })

      constraints.forEach((c, i) => {
        var srcIdx = c.pair[0]
        // if (srcIdx !== 3) {
        //   return;
        // }


        ctx.strokeStyle = hsl(srcIdx/particles.length * 0.9)
        const src = particles[srcIdx]
        for (var i=1; i<c.pair.length; i++) {
          var a = c.pair[i]
          var dest = particles[a]
          if (!dest) {
            return;
          }

          ctx.beginPath()
             ctx.moveTo(src[0], src[1])
             ctx.lineTo(dest[0], dest[1])
             ctx.stroke()
        }
      })

    ctx.restore()
  camera.end();
}

function hsl(p, a) {
  return `hsla(${p*360}, 100%, 70%, ${a||1})`
}

function tx(mat, x, y, fn) {
    vec2.set(v2tmp, x, y)
    vec2.transformMat3(v2tmp, v2tmp, mat)
    fn(v2tmp[0], v2tmp[1])
}


function square(mat, x, y, w, h) {
  tx(mat, x, y, moveTo)
  tx(mat, x, y + h, lineTo)
  tx(mat, x + w, y + h, lineTo)
  tx(mat, x + w, y, lineTo)
  tx(mat, x, y, lineTo)
}

function txo(out, mat, vec) {
    vec2.transformMat3(out, vec, mat)
    return out
}

package.json

{
 "dependencies": {
  "ctx-camera": "git+https://github.com/tmpvar/ctx-camera.git",
  "ctx-render-grid-lines": "^1.0.0",
  "ctx-translate-center": "^1.0.0",
  "fc": "^1.5.2",
  "gl-matrix": "^2.8.1",
  "ndarray": "^1.0.18",
  "ndarray-fill": "^1.0.2",
  "ray-aabb-slab": "^1.0.1",
  "segseg": "^0.2.2"
 }
}