sys/src/games/timmy/phys.c

   1 #include <u.h>
   2 #include <libc.h>
   3 #include <draw.h>
   4 #include "dat.h"
   5 #include "fns.h"
   6
   7 extern Obj runo;
   8 #define Grav 10
   9 #define Dt 0.01
  10 #define Beta 0.5
  11 int Steps = 4;
  12
  13 typedef struct Contact Contact;
  14 struct Contact {
  15         Obj *vo, *eo;
  16         Vec v, n;
  17         double pen;
  18         double impn, impt;
  19         double targun;
  20 };
  21 enum { CTSBLOCK = 64 };
  22
  23 Contact *cts;
  24 int icts, ncts;
  25
  26 static void
  27 colldect(Obj *a, Obj *b)
  28 {
  29         int i, j;
  30         Vec *x, *y, *z;
  31         double d, m;
  32         Poly *ap, *bp;
  33
  34         ap = a->poly;
  35         bp = b->poly;
  36         for(i = 0; i < ap->nvert; i++){
  37                 z = &ap->vert[i];
  38                 m = Inf(1);
  39                 for(j = 0; j < bp->nvert; j++){
  40                         x = &bp->vert[j];
  41                         y = x + 1;
  42                         d = -(z->x - x->x) * (y->y - x->y) + (z->y - x->y) * (y->x - x->x);
  43                         d /= vecdist(*x, *y);
  44                         if(d < -Slop) goto next;
  45                         if(d < m){
  46                                 if(m < 0) goto next;
  47                                 m = d;
  48                                 if(icts == ncts)
  49                                         cts = realloc(cts, sizeof(Contact) * (ncts += CTSBLOCK));
  50                                 cts[icts] = (Contact){a, b, *z, vecnormal(vecsub(*x, *y)), d, 0, 0, 0};
  51                         }else if(d < 0) goto next;
  52                 }
  53                 icts++;
  54         next: ;
  55         }
  56 }
  57
  58 static void
  59 collresp(Contact *p)
  60 {
  61         double μs, μd;
  62         Vec n, t, u, r0, r1, Δp;
  63         double ut, un, α, γ, γt, γn, pt, mt, mn;
  64         double r0n, r0t, r1n, r1t;
  65         double mv, me, Iv, Ie;
  66
  67         n = p->n;
  68         t = (Vec){n.y, -n.x};
  69         mv = p->vo->tab->imass;
  70         me = p->eo->tab->imass;
  71         Iv = mv * p->vo->poly->invI;
  72         Ie = me * p->eo->poly->invI;
  73         r0 = vecsub(p->v, p->vo->p);
  74         r1 = vecsub(p->v, p->eo->p);
  75         Δp.x = -(t.x * p->impt + n.x * p->impn);
  76         Δp.y = -(t.y * p->impt + n.y * p->impn);
  77         p->vo->v = vecadd(p->vo->v, vecmul(Δp, mv));
  78         p->vo->ω -= (Δp.x * r0.y - Δp.y * r0.x) * Iv;
  79         p->eo->v = vecadd(p->eo->v, vecmul(Δp, -me));
  80         p->eo->ω += (Δp.x * r1.y - Δp.y * r1.x) * Ie;
  81         u.x = p->vo->v.x - p->vo->ω * r0.y - p->eo->v.x + p->eo->ω * r1.y;
  82         u.y = p->vo->v.y + p->vo->ω * r0.x - p->eo->v.y - p->eo->ω * r1.x;
  83         ut = vecdot(u, t);
  84         un = vecdot(u, n);
  85         r0t = vecdot(r0, t);
  86         r0n = vecdot(r0, n);
  87         r1t = vecdot(r1, t);
  88         r1n = vecdot(r1, n);
  89         γ = 0; /* accumulated normal impulse */
  90         pt = 0; /* accumulated transverse impulse */
  91         μs = 0.5;
  92         μd = 0.3;
  93
  94         un += p->targun;
  95         if(un >= 0 && p->pen <= 0){
  96                 p->impt = 0;
  97                 p->impn = 0;
  98                 return;
  99         }
 100         if(p->pen > 0){
 101                 un -= Beta * p->pen / Dt;
 102                 if(un >= 0){
 103                         mn = mv + r0t * r0t * Iv;
 104                         mn += me + r1t * r1t * Ie;
 105                         γ = -un/mn;
 106                         un = 0;
 107                 }
 108         }
 109         while(un < 0){
 110                 /* calculate α, the effective coefficient of friction */
 111                 if(ut == 0){
 112                         α = r0t * r0n * Iv + r1t * r1n * Ie;
 113                         α /= mv + r0n * r0n * Iv + me + r1n * r1n * Ie;
 114                         if(α > μs) α = μd;
 115                         else if(α < -μs) α = -μd;
 116                 }else
 117                         α = ut < 0 ? μd : -μd;
 118
 119                 mt = α * mv + (r0n * r0n * α - r0t * r0n) * Iv;
 120                 mt += α * me + (r1n * r1n * α - r1t * r1n) * Ie;
 121                 mn = mv + (r0t * r0t - r0n * r0t * α) * Iv;
 122                 mn += me + (r1t * r1t - r1n * r1t * α) * Ie;
 123                 /* determine events which would change α */
 124                 if(ut == 0) γt = Inf(1);
 125                 else{
 126                         γt = γ - ut / mt;
 127                         if(γt < γ) γt = Inf(1);
 128                 }
 129                 γn = γ - un / mn;
 130                 if(γn < γ) γn = Inf(1);
 131                 /* choose earlier one */
 132                 if(γt < γn){
 133                         ut = 0;
 134                         un += mn * (γt - γ);
 135                         pt += (γt - γ) * α;
 136                         γ = γt;
 137                 }else{
 138                         assert(γn < Inf(1));
 139                         un = 0;
 140                         ut += mt * (γn - γ);
 141                         pt += (γn - γ) * α;
 142                         γ = γn;
 143                 }
 144         }
 145
 146         p->impt = pt;
 147         p->impn = γ;
 148         Δp.x = t.x * pt + n.x * γ;
 149         Δp.y = t.y * pt + n.y * γ;
 150         p->vo->v = vecadd(p->vo->v, vecmul(Δp, mv));
 151         p->vo->ω -= (Δp.x * r0.y - Δp.y * r0.x) * Iv;
 152         p->eo->v = vecadd(p->eo->v, vecmul(Δp, -me));
 153         p->eo->ω += (Δp.x * r1.y - Δp.y * r1.x) * Ie;
 154 }
 155
 156 extern Hinge *hinges;
 157
 158 static void
 159 hingeresp(Hinge *h)
 160 {
 161         Obj *a, *b;
 162         Vec u, Δp, r0, r1;
 163         double ma, mb, Ia, Ib;
 164         double mxx, mxy, myy, det;
 165
 166         a = h->o;
 167         b = h->cnext->o;
 168         ma = a->tab->imass;
 169         mb = b->tab->imass;
 170         Ia = ma * a->poly->invI;
 171         Ib = mb * b->poly->invI;
 172         r0 = vecsub(h->p, a->p);
 173         r1 = vecsub(h->cnext->p, b->p);
 174         u.x = a->v.x - a->ω * r0.y - b->v.x + b->ω * r1.y;
 175         u.y = a->v.y + a->ω * r0.x - b->v.y - b->ω * r1.x;
 176         u.x += Beta * (h->p.x - h->cnext->p.x) / Dt;
 177         u.y += Beta * (h->p.y - h->cnext->p.y) / Dt;
 178         mxx = ma + Ia * r0.x * r0.x + mb + Ib * r1.x * r1.x;
 179         mxy = Ia * r0.x * r0.y + Ib * r1.x * r1.y;
 180         myy = ma + Ia * r0.y * r0.y + mb + Ib * r1.y * r1.y;
 181         det = mxx * myy - mxy * mxy;
 182         Δp.x = (mxx * u.x + mxy * u.y) / det;
 183         Δp.y = (myy * u.y + mxy * u.x) / det;
 184         a->v = vecadd(a->v, vecmul(Δp, -ma));
 185         a->ω += (Δp.x * r0.y - Δp.y * r0.x) * Ia;
 186         b->v = vecadd(b->v, vecmul(Δp, mb));
 187         b->ω -= (Δp.x * r1.y - Δp.y * r1.x) * Ib;
 188         u.x = a->v.x - a->ω * r0.y - b->v.x + b->ω * r1.y;
 189         u.y = a->v.y + a->ω * r0.x - b->v.y - b->ω * r1.x;
 190 }
 191
 192 void
 193 physstep(void)
 194 {
 195         Obj *o, *a, *b;
 196         int i, j, k;
 197         Hinge *p;
 198
 199         for(k = 0; k < Steps; k++){
 200                 for(o = runo.next; o != &runo; o = o->next)
 201                         if(o->tab->imass != 0)
 202                                 o->v.y += Grav * Dt;
 203                 icts = 0;
 204                 for(a = runo.next; a != &runo; a = a->next)
 205                         for(b = a->next; b != &runo; b = b->next){
 206                                 if(!rectXrect(a->bbox, b->bbox) || a->poly == nil || b->poly == nil || hinged(a, b)) continue;
 207                                 colldect(a, b);
 208                                 colldect(b, a);
 209                         }
 210                 for(j = 0; j < 10; j++){
 211                         for(i = 0; i < icts; i++)
 212                                 collresp(&cts[i]);
 213                         for(p = hinges; p != nil; p = p->anext)
 214                                 hingeresp(p);
 215                 }
 216                 for(o = runo.next; o != &runo; o = o->next)
 217                         o->tab->move(o, o->p.x + o->v.x * Dt, o->p.y + o->v.y * Dt, o->θ + o->ω * Dt / DEG);
 218         }
 219 }