diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/simulation/Simulation.cpp | 117 | ||||
| -rw-r--r-- | src/simulation/Solid.h | 1 |
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; |