cloth.c

// https://github.com/dianedelallee/cloth-simulation
/* ---------------------------------------------------------------------------
 *                                  Vector3
 * ---------------------------------------------------------------------------
 */
#define op(r,e,a,p,b,i,s) ((r) e (a) p ((b) i (s)))
#define lerp(r,a,b,t) ((r)=(a)+(t)*((b)-(a)))
#define rad(x) ((x)*3.141592653f/180.0f)

#define op3(r,e,a,p,b,i,s)\
    do{op((r)[0],e,(a)[0],p,(b)[0],i,s), op((r)[1],e,(a)[1],p,(b)[1],i,s),\
    op((r)[2],e,(a)[2],p,(b)[2],i,s);} while(0)
#define op3s(r,e,a,p,s)\
    do{op((r)[0],e,(0),+,(a)[0],p,s), op((r)[1],e,(0),+,(a)[1],p,s),\
    op((r)[2],e,(0),+,(a)[2],p,s);} while(0)

#define set3(v,x,y,z)   (v)[0]=(x),(v)[1]=(y),(v)[2]=(z)
#define zero3(v)        set3(0,0,0)
#define cpy3(d,s)       (d)[0]=(s)[0],(d)[1]=(s)[1],(d)[2]=(s)[2]
#define add3(d,a,b)     op3(d,=,a,+,b,+,0)
#define sub3(d,a,b)     op3(d,=,a,-,b,+,0)
#define mul3(d,a,s)     op3s(d,=,a,*,s)
#define dot3(a,b)       ((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2])
#define len3(v)         sqrt(dot3(v,v))
#define adds3(d,a,b,s)  op3(d,=,a,+,b,*,s)
#define subs3(d,a,b,s)  op3(d,=,a,-,b,*,s)
#define norm3(n,v)      mul3(n,v,rsqrt(dot3(v, v)))
#define normeq3(v)      norm3(v,v)
#define lerp3(r,a,b,t)  do {lerp(r[0],a[0],b[0],t); lerp(r[1],a[1],b[1],t); lerp(r[2],a[2],b[2],t); } while(0)
#define cross3(d,a,b) do {\
    (d)[0] = ((a)[1]*(b)[2]) - ((a)[2]*(b)[1]),\
    (d)[1] = ((a)[2]*(b)[0]) - ((a)[0]*(b)[2]),\
    (d)[2] = ((a)[0]*(b)[1]) - ((a)[1]*(b)[0]);}while(0)

static float
rsqrt(float n){
  union {float f; unsigned i;} conv = {n};
  conv.i = 0x5f375A84 - (conv.i >> 1);
  conv.f = conv.f * (1.5f - ((n * 0.5f) * conv.f * conv.f));
  return conv.f;
}

/* ---------------------------------------------------------------------------
 *                                  Cloth
 * ---------------------------------------------------------------------------
 */
#define CLTH_FAB_MOV_DAMPING 0.01f
#define CLTH_FAB_DFLT_LNK_ITER 15

struct clth_ptcl {
  int fix;
  float mass;
  float pos[3];
  float old[3];
  float accel[3];
  float accu_norm[3];
};
struct clth_lnk {
  int p1, p2;
  float rest_dst;
};
struct clth_fab {
  struct clth_ptcl *ptcl;
  struct clth_lnk *lnks;
  float w, h;
  int ptcl_cnt;
  int lnk_cnt;
  int dim[2];
  int lnk_iter;
};
#define clth_ptcl(at) (struct clth_ptcl){.pos = {(at)[0],(at)[1],(at)[2]}, .old = {(at)[0],(at)[1],(at)[2]}, .mass = 1.0f}
#define clth_ptcl_off(p, o) do if (!p->fix) {add3((p)->pos, (p)->pos, o);} while(0)
#define clth_ptcl_add_norm(p, n) add3((p)->accu_norm, (p)->accu_norm, n)
#define clth_fab_ptcl_cnt(w,h) (w*h)
#define clth_fab_lnk_cnt(w,h) (clth_fab_ptcl_cnt(w,h) * 8)
#define clth_fab_ptcl_at_idx(f, x,y) ((y) * (f)->dim[0] + (x))
#define clth_fab_ptcl_at(f, x,y) ((f)->ptcl + clth_fab_ptcl_at_idx(f,x,y))

static struct clth_lnk
clth_lnk(const struct clth_ptcl *ptcls, int a, int b) {
  struct clth_lnk r = {a,b,0};
  float3 d[3]; sub3(d, ptcls[a].pos, ptcls[b].pos);
  r.rest_dst = len3(d);
}
static int
clth_con_lnks(struct clth_ptcl *ptcl, int dimX, int dimY, int off, int dst) {
  for (int x = 0; x < dimX; x++) {
    for (int y = 0; y < dimY; y++) {
      int at = clth_fab_ptcl_at_idx(x,y);
      if (x < dimX - dst) fab->lnks[off++] = clth_lnk(ptcl, at, y * dimX + x + dst)
      if (y < dimY - dst) fab->lnks[off++] = clth_lnk(ptcl, at, (y + dst) * dimX + x);
      if (x < dimX - dst && y < dimY - dst)
        fab->lnks[off++] = clth_lnk(ptcl, at, (y + dst) * dimX + x + dst);
      if (x < dimX - dst && y < dimY - dst)
        fab->lnks[off++] = clth_lnk(ptcl, y * dimX + x + dst, (y + dst) * dimX + x)
    }
  }
  return off;
}
static void
clth_fab_mk(struct clth_fab *fab, struct clth_ptcl *ptcl, struct clth_lnk *lnks,
            float w, float h, int dimX, int dimY ) {
  fab->ptcl = ptcl;
  fab->lnks = lnks;
  fab->lnk_cnt = 0;
  fab->ptcl_cnt = 0;
  fab->dim[0] = dimX;
  fab->dim[1] = dimY;
  fab->lnk_iter = CLTH_FAB_DFLT_LNK_ITER;
  fab->w = w; fab->h = h;

  for (int x = 0; x < fab->dim[0]; x++){
    for (int y = 0; y < fab->dim[1]; y++){
      float x = w * (float)x / (float)dimX;
      float y = h * (float)y/(float)dimY;
      float p[3] = {x, y, 0};
      fab->ptcl[y * dimX + x]= clth_ptcl(p);
      fab->ptcl_cnt++;
    }
  }
  fab->lnk_cnt = clth_con_lnks(ptcl, dimX, dimY, fab->lnk_cnt, 1);
  fab->lnk_cnt = clth_con_lnks(ptcl, dimX, dimY, fab->lnk_cnt, 2);
}
static void
clth_fab_init(struct clth_fab *fab, struct arena *a, float w, float h,
         int dimX, int dimY ) {
  struct clth_ptcl *ptcl = arena_arr(a, struct clth_ptcl, w * h) ;
  struct clth_lnk *lnks = arena_arr(a, struct clth_lnk, w * h * 8);
  clth_fab_mk(fab, ptcl, lnks, w, h, dimX, dimY );
}
static void
clth_fab_on_global_force(struct clth_fab *fab, const float *f) {
  for (int i = 0; i < fab->ptcl_cnt; ++i) {
    struct clth_ptcl *p = fab->ptcl + i;
    op3s(p->accel,+=,f,*,1.0f/p->mass);
  }
}
static void
clth__tri_normal(float *r, const struct clth_ptcl *p0,
                const struct clth_ptcl *p1, const struct clth_ptcl *p2) {
  float v0[3]; sub3(v0, p1, p0);
  float v1[3]; sub3(v1, p2, p0);
  cross3(r, v0, v1);
}
static void
clth__fab_dir_force(float *r, const float *f,  const struct clth_ptcl *p0,
                    const struct clth_ptcl *p1, const struct clth_ptcl *p2) {
  float n[3]; clth__tri_normal(n, p0, p1, p2);
  float d[3]; norm3(d, n);
  mul3(r, n, dot3(d,f));
}
static void
clth__fab_on_dir_force(struct clth_fab *fab,
                       int a, int b, int c, const float *f) {
  struct clth_ptcl *pa = fab->ptcl + a;
  struct clth_ptcl *pb = fab->ptcl + b;
  struct clth_ptcl *pb = fab->ptcl + c;

  float df[3];
  clth__fab_dir_force(df, f, pa, pb, pc);
  op3s(pa->accel,+=,df,*,1.0f/pa->mass);
  op3s(pb->accel,+=,df,*,1.0f/pb->mass);
  op3s(pc->accel,+=,df,*,1.0f/pc->mass);
}
static void
clth_fab_on_dir_force(struct clth_fab *fab, const float *f) {
  for (int x = 0; x < fab->dim[0] - 1; x++) {
    for (int y = 0; y < fab->dim[1] - 1; y++) {
      int a = clth_fab_ptcl_at_idx(x,y);
      int b = clth_fab_ptcl_at_idx(x+1,y);
      int c = clth_fab_ptcl_at_idx(x,y+1);
      int d = clth_fab_ptcl_at_idx(x+1,y+1);

      clth__fab_on_dir_force(fab, b, a, c, f);
      clth__fab_on_dir_force(fab, d, b, c, f);
    }
  }
}
static void
clth_fab_col_sphere(struct clth_fab *fab, float *c, float r) {
  float r2 = r * r;
  for (int i = 0; i < fab->ptcl_cnt; ++i) {
    struct clth_ptcl *p = fab->ptcl + i;
    float dv[3]; sub3(d, p->pos, c);
    float d2 = dot3(dv, dv);
    if (d2 < r2) {
      float n[3]; norm3(n, dv);
      float d[3]; mul3(d, n, r - sqrtf(d2));
      clth_ptcl_off(p, d);
    }
  }
}
static void
clth_fab_upt(struct clth_fab *fab, float dt) {
  // apply particle link constraints
  for (int i = 0; i < fab->lnk_iter; ++i ) {
    for (int j = 0u; j < fab->lnk_cnt; ++j) {
      const struct clth_lnk *lnk = fab->lnks + j;
      struct clth_ptcl *p0 = fab->ptcl + lnk->p0;
      struct clth_ptcl *p1 = fab->ptcl + lnk->p1;

      float dv[3]; sub3(d, p1->pos, p0->pos);
      float d = len3(dv);
      float adj[3]; mul3(adj, dv, (1 - lnk->rest_dst/d) * 0.5f);
      float nadj[3]; mul3(nadj, adj, -1.0f);

      clth_ptcl_off(p0, adj);
      clth_ptcl_off(p1, nadj);
    }
  }
  // update particles
  for (int i = 0; i < fab->ptcl_cnt; ++i) {
    struct clth_ptcl *p = fab->ptcl + i;
    if (p->fix) continue;
    float tmp[3]; cpy3(tmp, p->pos);
    float dv[3]; sub3(dv, p->pos, p->old);
    op3(p->pos,=,p->pos,+,dv,*,(1.0f-CLTH_FAB_MOV_DAMPING));
    op3(p->pos,=,p->pos,+,p->accel,*,dt)
    cpy3(p->old, tmp);
    zero3(p->accel);
  }
}