Clean up the collisions a bit. Momentum transfer uses actual momentum now :-)

2 files changed, 97 insertions, 21 deletions

diff --git a/src/simulation/Simulation.cpp b/src/simulation/Simulation.cpp
index 07bde47..63c3afc 100644
--- a/src/simulation/Simulation.cpp
+++ b/src/simulation/Simulation.cpp
@@ -4750,8 +4750,8 @@ int Simulation::GetParticleType(std::string type)
 void Simulation::update_solids()
 {
     int i, pp, nc, bb, bd, nx, ny;
-    float cdx, cdy, adx, ady, mmx, mmy, dd, ee, rmx, rmy, vx, vy;
-    float cvrot, svrot;
+    float adx, ady, mmx, mmy, dd, ee, rmx, rmy, vx, vy, av;
+    float vrot, cvrot, svrot;
     Solid *sld;
 
     for(i=0; i<NSOLID; i++) {
@@ -4759,9 +4759,24 @@ void Simulation::update_solids()
         if(sld->first == -1)
             continue;
 
+        if(!sld->bf) {
+            cvrot = cosf(sld->vrot) - 1.0f;
+            svrot = sinf(sld->vrot);
+            for(pp=sld->first; pp!=-1; pp=parts[pp].snext) {
+                adx = parts[pp].x - sld->cdx;
+                ady = parts[pp].y - sld->cdy;
+                vx = sld->vx + adx*cvrot + ady*svrot;
+                vy = sld->vy + ady*cvrot - adx*svrot;
+                parts[pp].x += vx;
+                parts[pp].y += vy;
+            }
+        }
+
         for(pp=sld->first; pp!=-1; pp=parts[pp].snext) {
             nx = (int)(parts[pp].x + 0.5f);
             ny = (int)(parts[pp].y + 0.5f);
+            if(nx < CELL || ny < CELL || nx >= XRES-CELL || ny >= YRES-CELL)
+                continue;
             if(!pmap[ny-1][nx])
                 parts[pp].vy += pv[ny/CELL][nx/CELL-1];
             if(!pmap[ny+1][nx])
@@ -4779,14 +4794,14 @@ void Simulation::update_solids()
 
         if(sld->bf) {
             for(bd=0; bd<4; bd++) {
-                cdx = sld->bpx[bd];
-                cdy = sld->bpy[bd];
-                ee = cdy*sld->ax - cdx*sld->ay;
+                mmx = sld->bpx[bd];
+                mmy = sld->bpy[bd];
+                ee = mmy*sld->ax - mmx*sld->ay;
                 for(bb=0; bb<4; bb++)
                     if(bd != bb && (sld->bpx[bd] != sld->bpx[bb] || sld->bpy[bd] != sld->bpy[bb])) {
-                        cdx = sld->bpx[bb] - sld->bpx[bd];
-                        cdy = sld->bpy[bb] - sld->bpy[bd];
-                        dd = cdy*sld->bdx[bb] - cdx*sld->bdy[bb];
+                        mmx = sld->bpx[bb] - sld->bpx[bd];
+                        mmy = sld->bpy[bb] - sld->bpy[bd];
+                        dd = mmy*sld->bdx[bb] - mmx*sld->bdy[bb];
                         if(dd * ee < 0.0f)
                             break;
                     }
@@ -4794,6 +4809,65 @@ void Simulation::update_solids()
                     break;
             }
             if(bd >= 4) {
+                printf("Solid %d: blocked\n", i);
+
+                for(bd=0; bd<4; bd++) {
+                    for(bb=0; bb<bd; bb++)
+                        if(sld->bpx[bd] == sld->bpx[bb] && sld->bpy[bd] == sld->bpy[bb])
+                            break;
+                    if(bb < bd)
+                        break;
+
+                    mmx = sld->bpx[bd] + sld->cx;
+                    mmy = sld->bpy[bd] + sld->cy;
+                    av = 0.0f;
+                    for(pp=sld->first; pp!=-1; pp=parts[pp].snext) {
+                        dd = (parts[pp].x - mmx)*(parts[pp].x - mmx) + (parts[pp].y - mmy)*(parts[pp].y - mmy);
+                        if(dd > 0.5f) {
+                            vrot += (parts[pp].vx*(parts[pp].y - mmy) - parts[pp].vy*(parts[pp].x - mmx));
+                            av += dd;
+                        }
+                    }
+                    if(sld->an)
+                        vrot = atanf(vrot / av);
+
+                    cvrot = cosf(vrot) - 1.0f;
+                    svrot = sinf(vrot);
+                    rmx = rmy = 0.0f;
+                    for(pp=sld->first; pp!=-1; pp=parts[pp].snext) {
+                        adx = parts[pp].x - mmx;
+                        ady = parts[pp].y - mmy;
+                        vx = adx*cvrot + ady*svrot;
+                        vy = ady*cvrot - adx*svrot;
+                        rmx += parts[pp].vx - vx;
+                        rmy += parts[pp].vy - vy;
+                        parts[pp].vx = vx;
+                        parts[pp].vy = vy;
+                    }
+
+                    pp = sld->bp[bd];
+                    if(pp == -1 || (!(parts[pp].sld && parts[pp].type) && (!(elements[parts[pp].type].Properties & TYPE_SOLID) && parts[pp].type)))
+                        continue;
+
+                    if(parts[pp].sld && parts[pp].type) {
+                        if(parts[pp].sld-1 != i) {
+                            printf("     reaction momentum: %f %f (solid-solid %d)\n", rmx, rmy, parts[pp].sld-1);
+                            parts[pp].vx += rmx;
+                            parts[pp].vy += rmy;
+                        }
+                    } else if(!(elements[parts[pp].type].Properties & TYPE_SOLID) && parts[pp].type) {
+                        ee = hypotf(rmx, rmy) * 0.01f;
+                        if(ee > 1.0f) {
+                            ee = 1.0f / ee;
+                            rmx *= ee;
+                            rmy *= ee;
+                        }
+                        printf("     reaction momentum: %f %f (solid-particle %s)\n", rmx, rmy, elements[parts[pp].type].Name);
+                        parts[pp].vx += rmx;
+                        parts[pp].vy += rmy;
+                    }
+                }
+
                 nc = 0;
                 for(pp=sld->first; pp!=-1; pp=parts[pp].snext) {
                     sld->cx += parts[pp].x;
@@ -4815,8 +4889,8 @@ void Simulation::update_solids()
                 continue;
             }
 
-            cdx = sld->cx + sld->bpx[bd];
-            cdy = sld->cy + sld->bpy[bd];
+            sld->cdx = sld->cx + sld->bpx[bd];
+            sld->cdy = sld->cy + sld->bpy[bd];
             mmx = -sld->bdx[bd];
             mmy = -sld->bdy[bd];
         } else
@@ -4833,8 +4907,8 @@ void Simulation::update_solids()
         sld->cy /= nc;
 
         if(!sld->bf) {
-            cdx = sld->cx;
-            cdy = sld->cy;
+            sld->cdx = sld->cx;
+            sld->cdy = sld->cy;
         }
 
         if(sld->an) {
@@ -4847,15 +4921,18 @@ void Simulation::update_solids()
             }
         }
 
+        av = 0.0f;
         for(pp=sld->first; pp!=-1; pp=parts[pp].snext) {
-            dd = (parts[pp].x - cdx)*(parts[pp].x - cdx) + (parts[pp].y - cdy)*(parts[pp].y - cdy);
-            if(dd > 0.5f)
-                sld->arot += ((parts[pp].vx - sld->vx)*(parts[pp].y - cdy) - (parts[pp].vy - sld->vy)*(parts[pp].x - cdx)) / dd;
+            dd = (parts[pp].x - sld->cdx)*(parts[pp].x - sld->cdx) + (parts[pp].y - sld->cdy)*(parts[pp].y - sld->cdy);
+            if(dd > 0.5f) {
+                sld->arot += ((parts[pp].vx - sld->vx)*(parts[pp].y - sld->cdy) - (parts[pp].vy - sld->vy)*(parts[pp].x - sld->cdx));
+                av += dd;
+            }
         }
 
         if(sld->an) {
-            sld->vrot = atanf(sld->arot / sld->an);
-            printf("Solid %d: count %d, vx %f, vy %f, vrot %f, b %d/%d, cdx %f, cdy %f\n", i, sld->an, sld->vx, sld->vy, sld->vrot, sld->bf, bd, cdx, cdy);
+            sld->vrot = atanf(sld->arot / av);
+            printf("Solid %d: count %d, vx %f, vy %f, vrot %f, b %d/%d, cdx %f, cdy %f\n", i, sld->an, sld->vx, sld->vy, sld->vrot, sld->bf, bd, sld->cdx, sld->cdy);
             if(sld->bf) {
                 printf("   ");
                 for(bb=0; bb<4; bb++)
@@ -4868,16 +4945,14 @@ void Simulation::update_solids()
         svrot = sinf(sld->vrot);
         rmx = rmy = 0.0f;
         for(pp=sld->first; pp!=-1; pp=parts[pp].snext) {
-            adx = parts[pp].x - cdx;
-            ady = parts[pp].y - cdy;
+            adx = parts[pp].x - sld->cdx;
+            ady = parts[pp].y - sld->cdy;
             vx = sld->vx + adx*cvrot + ady*svrot;
             vy = sld->vy + ady*cvrot - adx*svrot;
             rmx += parts[pp].vx - vx;
             rmy += parts[pp].vy - vy;
             parts[pp].vx = vx;
             parts[pp].vy = vy;
-            parts[pp].x += parts[pp].vx + mmx;
-            parts[pp].y += parts[pp].vy + mmy;
         }
 
         if(sld->bf && sld->bp[bd] != -1) {
diff --git a/src/simulation/Solid.h b/src/simulation/Solid.h
index f4b932c..3a552ce 100644
--- a/src/simulation/Solid.h
+++ b/src/simulation/Solid.h
@@ -10,6 +10,7 @@ struct Solid
 {
     int first, last;
     float vx, vy, vrot;
+    float cdx, cdy;             // center of rotation
     float cx, cy;               // centroid position
     float ax, ay, arot, arad;   // accumulators
     int an;