void calculateCenterOfMass(cellptr cell)
{
/* Center of mass position */
vector cmPos;
vector tempV;
nodeptr q;
int i;
/* Init the cell's total mass */
Mass(cell) = 0.0;
/* Init the cell's center of mass position */
CLRV(cmPos);
/* Loop over the subnodes */
for (i = 0; i < NCHILD; i++) {
/* Skipping empty child nodes */
if ((q = Child(cell)[i]) != NULL) {
/* If node is a cell */
if (Type(q) == CELL)
/* Recursively do the same */
calculateCenterOfMass((cellptr) q);
/* Accumulate total mass */
Mass(cell) = Mass(q);
/* Accumulate center of mass */
ADDMULVS(cmPos, Pos(q), Mass(q));
}
}
/* Usually, cell has mass */
if (Mass(cell) > 0.0) {
/* Find center of mass position */
DIVVS(cmPos, cmPos, Mass(cell));
} else { /* If no mass inside */
SETV(cmPos, Pos(cell));
}
SETV(Pos(cell), cmPos);
}
local void diagnostics()
{
bodyptr p;
real velsq, phi0;
vector tmpv;
matrix tmpt;
int ndim=NDIM;
mtot = 0.0; /* zero total mass */
etot[1] = etot[2] = 0.0; /* zero total KE and PE */
CLRM(keten); /* zero ke tensor */
CLRM(peten); /* zero pe tensor */
CLRM(amten); /* zero am tensor */
CLRV(cmphase[0]); /* zero c. of m. position */
CLRV(cmphase[1]); /* zero c. of m. velocity */
for (p = bodytab; p < bodytab+nbody; p++) { /* loop over all particles */
mtot += Mass(p); /* sum particle masses */
DOTVP(velsq, Vel(p), Vel(p)); /* square vel vector */
if (extpot) { /* external potential corr. */
(*extpot)(&ndim,Pos(p),tmpv,&phi0,&tnow);
phi0 = Phi(p) + phi0; /* extre correction */
} else
phi0 = Phi(p);
etot[1] += 0.5 * Mass(p) * velsq; /* sum current KE */
etot[2] += 0.5 * Mass(p) * phi0; /* and current PE */
MULVS(tmpv, Vel(p), 0.5 * Mass(p)); /* sum 0.5 m v_i v_j */
OUTVP(tmpt, tmpv, Vel(p));
ADDM(keten, keten, tmpt);
MULVS(tmpv, Pos(p), Mass(p)); /* sum m r_i a_j */
OUTVP(tmpt, tmpv, Acc(p));
ADDM(peten, peten, tmpt);
OUTVP(tmpt, tmpv, Vel(p)); /* sum m r_i v_j */
ADDM(amten, amten, tmpt);
MULVS(tmpv, Pos(p), Mass(p)); /* sum cm position */
ADDV(cmphase[0], cmphase[0], tmpv);
MULVS(tmpv, Vel(p), Mass(p)); /* sum cm momentum */
ADDV(cmphase[1], cmphase[1], tmpv);
}
etot[0] = etot[1] + etot[2]; /* sum KE and PE */
TRANM(tmpt, amten); /* anti-sym. AM tensor */
SUBM(amten, amten, tmpt);
DIVVS(cmphase[0], cmphase[0], mtot); /* normalize cm coords */
DIVVS(cmphase[1], cmphase[1], mtot);
}
void picktriad(vector x, vector y, vector z)
{
real a;
pickshell(x, NDIM, 1.0);
pickshell(z, NDIM, 1.0);
CROSSVP(y, x, z);
a = absv(y);
DIVVS(y, y, a);
CROSSVP(z, x, y);
}
local void diagnostics(void) {
bodyptr p1, p2, p;
real mp, velsq;
vector tmpv;
matrix tmpt;
mtot = 0.0; // zero total mass
etot[1] = etot[2] = 0.0; // zero total KE and PE
CLRM(keten); // zero ke tensor
CLRM(peten); // zero pe tensor
CLRV(amvec); // zero am vector
CLRV(cmpos); // zero c. of m. position
CLRV(cmvel); // zero c. of m. velocity
p1 = bodytab + MAX(nstatic, 0); // set dynamic body range
p2 = bodytab + nbody + MIN(nstatic, 0);
for (p = p1; p < p2; p++) { // loop over body range
mp = (testcalc ? 1.0 / (nbody - ABS(nstatic)) : Mass(p));
// use eq. mass in testcalc
mtot += mp; // sum particle masses
DOTVP(velsq, Vel(p), Vel(p)); // square vel vector
etot[1] += 0.5 * mp * velsq; // sum current KE
etot[2] += (testcalc ? 1.0 : 0.5) * mp * Phi(p);
// and PE, weighted right
MULVS(tmpv, Vel(p), 0.5 * mp); // sum 0.5 m v_i v_j
OUTVP(tmpt, tmpv, Vel(p));
ADDM(keten, keten, tmpt);
MULVS(tmpv, Pos(p), mp); // sum m r_i a_j
OUTVP(tmpt, tmpv, Acc(p));
ADDM(peten, peten, tmpt);
CROSSVP(tmpv, Vel(p), Pos(p)); // sum angular momentum
MULVS(tmpv, tmpv, mp);
ADDV(amvec, amvec, tmpv);
MULVS(tmpv, Pos(p), mp); // sum cm position
ADDV(cmpos, cmpos, tmpv);
MULVS(tmpv, Vel(p), mp); // sum cm momentum
ADDV(cmvel, cmvel, tmpv);
}
etot[0] = etot[1] + etot[2]; // sum KE and PE
DIVVS(cmpos, cmpos, mtot); // normalize cm coords
DIVVS(cmvel, cmvel, mtot);
}
void snaptrak(void)
{
bodyptr bp, gp;
int nzero;
if (traktab == NULL) {
ntrak = 0;
for (bp = bodytab; bp < NthBody(bodytab, nbody); bp = NextBody(bp))
ntrak = MAX(ntrak, Group(bp));
eprintf("[%s: allocating %d groups]\n", getprog(), ntrak);
traktab = (bodyptr) allocate(ntrak * SizeofBody);
}
for (gp = traktab; gp < NthBody(traktab, ntrak); gp = NextBody(gp)) {
Mass(gp) = 0.0;
CLRV(Pos(gp));
CLRV(Vel(gp));
Key(gp) = 0;
}
for (bp = bodytab; bp < NthBody(bodytab, nbody); bp = NextBody(bp)) {
if (Group(bp) > ntrak)
error("snaptrak: cant expand group array\n");
if (Group(bp) > 0) {
gp = NthBody(traktab, Group(bp) - 1);
Mass(gp) += Mass(bp);
ADDMULVS(Pos(gp), Pos(bp), Mass(bp));
ADDMULVS(Vel(gp), Vel(bp), Mass(bp));
Key(gp)++;
}
}
nzero = 0;
for (gp = traktab; gp < NthBody(traktab, ntrak); gp = NextBody(gp))
if (Mass(gp) != 0.0) {
DIVVS(Pos(gp), Pos(gp), Mass(gp));
DIVVS(Vel(gp), Vel(gp), Mass(gp));
} else
nzero++;
if (nzero > 0)
eprintf("[%s: %d groups have zero mass]\n", getprog(), nzero);
}
local void diagnostics()
{
int i;
Body *p;
real velsq;
vector tmpv;
matrix tmpt;
mtot = 0.0; /* zero total mass */
etot[1] = etot[2] = 0.0; /* zero total KE and PE */
CLRM(keten); /* zero KE tensor */
CLRM(peten); /* zero PE tensor */
CLRM(amten); /* zero AM tensor */
CLRV(cmphase[0]); /* zero c. of m. position */
CLRV(cmphase[1]); /* zero c. of m. velocity */
for (p = bodytab; p < bodytab+nbody; p++) { /* loop over all bodies */
mtot += Mass(p); /* sum body masses */
DOTVP(velsq, Vel(p), Vel(p)); /* square vel vector */
etot[1] += 0.5 * Mass(p) * velsq; /* sum current KE */
etot[2] += 0.5 * Mass(p) * Phi(p); /* and current PE */
MULVS(tmpv, Vel(p), 0.5 * Mass(p)); /* sum 0.5 m v_i v_j */
OUTVP(tmpt, tmpv, Vel(p));
ADDM(keten, keten, tmpt);
MULVS(tmpv, Pos(p), Mass(p)); /* sum m r_i a_j */
OUTVP(tmpt, tmpv, Acc(p));
ADDM(peten, peten, tmpt);
OUTVP(tmpt, tmpv, Vel(p)); /* sum m r_i v_j */
ADDM(amten, amten, tmpt);
MULVS(tmpv, Pos(p), Mass(p)); /* sum cm position */
ADDV(cmphase[0], cmphase[0], tmpv);
MULVS(tmpv, Vel(p), Mass(p)); /* sum cm momentum */
ADDV(cmphase[1], cmphase[1], tmpv);
}
etot[0] = etot[1] + etot[2]; /* sum KE and PE */
TRANM(tmpt, amten); /* antisymmetrize AM tensor */
SUBM(amten, amten, tmpt);
DIVVS(cmphase[0], cmphase[0], mtot); /* normalize cm coords */
DIVVS(cmphase[1], cmphase[1], mtot);
}
void snapcmacc(vector cmacc, bodyptr btab, int nbody, int woff)
{
double wtot, cmtmp[NDIM];
bodyptr bp;
wtot = 0.0;
CLRV(cmtmp);
for (bp = btab; bp < NthBody(btab, nbody); bp = NextBody(bp)) {
wtot = wtot + Weight(bp, woff);
ADDMULVS(cmtmp, Acc(bp), Weight(bp, woff));
}
DIVVS(cmacc, cmtmp, wtot);
}
