local void gravsub(nodeptr q)
{
real drab, phii, mor3;
vector ai, quaddr;
real dr5inv, phiquad, drquaddr;
if (q != (long) qmem) { /* cant use memorized data? */
SUBV(dr, Pos(q), pos0); /* then compute sep. */
DOTVP(drsq, dr, dr); /* and sep. squared */
}
drsq += eps*eps; /* use standard softening */
drab = rsqrt(drsq);
phii = Mass(q) / drab;
mor3 = phii / drsq;
MULVS(ai, dr, mor3);
phi0 -= phii; /* add to total grav. pot. */
ADDV(acc0, acc0, ai); /* ... and to total accel. */
if (usequad && Type(q) == CELL) { /* if cell, add quad term */
cellptr SAFE c= TC(q);
dr5inv = 1.0/(drsq * drsq * drab); /* form dr^-5 */
MULMV(quaddr, Quad(c), dr); /* form Q * dr */
DOTVP(drquaddr, dr, quaddr); /* form dr * Q * dr */
phiquad = -0.5 * dr5inv * drquaddr; /* get quad. part of phi */
phi0 = phi0 + phiquad; /* increment potential */
phiquad = 5.0 * phiquad / drsq; /* save for acceleration */
MULVS(ai, dr, phiquad); /* components of acc. */
SUBV(acc0, acc0, ai); /* increment */
MULVS(quaddr, quaddr, dr5inv);
SUBV(acc0, acc0, quaddr); /* acceleration */
}
}
bool subdivp(register nodeptr p, real dsq, long ProcessId)
{
SUBV(Local[ProcessId].dr, Pos(p), Local[ProcessId].pos0);
DOTVP(Local[ProcessId].drsq, Local[ProcessId].dr, Local[ProcessId].dr);
Local[ProcessId].pmem = p;
return (tolsq * Local[ProcessId].drsq < dsq);
}
local bool subdivp(nodeptr n)
{
cellptr SAFE q = TC(n);
SUBV(dr, PosC(q), pos0); /* compute displacement */
DOTVP(drsq, dr, dr); /* and find dist squared */
qmem = q; /* remember we know them */
return (drsq < Rcrit2(q)); /* apply standard rule */
}
int point_in_triangle(vec2d A, vec2d B, vec2d C, vec2d P)
{
vec2d v0; SUBV(v0, C, A);
vec2d v1; SUBV(v1, B, A);
vec2d v2; SUBV(v2, P, A);
double dot00; DOTVP(dot00, v0, v0);
double dot01; DOTVP(dot01, v0, v1);
double dot02; DOTVP(dot02, v0, v2);
double dot11; DOTVP(dot11, v1, v1);
double dot12; DOTVP(dot12, v1, v2);
double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
return (u > 0.0) && (v > 0.0) && (u + v < 1.0);
}
bool isInBetw(vector pt, vector begin, vector end) {
float dot, crossLength, eBLength;
vector eB, pB, cross;
SUBV(eB, end, begin);
SUBV(pB, pt, begin);
DOTVP(dot, eB, pB);
CROSSVP(cross, eB, pB);
ABSV(eBLength, eB);
ABSV(crossLength, cross);
return (crossLength < 0.0001f && dot >= 0.0000f && dot < eBLength * eBLength);
}
// Same overall approach taken from Blender mathutils library.
float * intersect_ray_tri(vector v1, vector v2, vector v3, vector ray, vector origin) {
float det, inv_det, u, v, t, absv;
vector dir, e1, e2, tvec, qvec, pvec;
NORMV(dir, ray);
SUBV(e1, v2, v1);
SUBV(e2, v3, v1);
CROSSVP(pvec, dir, e2);
DOTVP(det, e1, pvec);
if (det > -0.000001f && det < 0.000001f) {
return 0;
}
inv_det = 1.0f / det;
SUBV(tvec, origin, v1);
DOTVP(u, tvec, pvec);
u = u * inv_det;
if(u < 0.0f || u > 1.0f){
return 0;
}
CROSSVP(qvec, tvec, e1);
DOTVP(v, dir, qvec);
v = v * inv_det;
if (v < 0.0f || u + v > 1.0f) {
return 0;
}
DOTVP(t, e2, qvec);
t = t * inv_det;
MULVS(dir, dir, t);
ADDV(pvec, origin, dir);
float *result = new float[3];
result[0] = pvec[0];
result[1] = pvec[1];
result[2] = pvec[2];
return result;
}
void gravsub(register nodeptr p, long ProcessId)
{
real drabs, phii, mor3;
vector ai;
if (p != Local[ProcessId].pmem) {
SUBV(Local[ProcessId].dr, Pos(p), Local[ProcessId].pos0);
DOTVP(Local[ProcessId].drsq, Local[ProcessId].dr, Local[ProcessId].dr);
}
Local[ProcessId].drsq += epssq;
drabs = sqrt((double) Local[ProcessId].drsq);
phii = Mass(p) / drabs;
Local[ProcessId].phi0 -= phii;
mor3 = phii / Local[ProcessId].drsq;
MULVS(ai, Local[ProcessId].dr, mor3);
ADDV(Local[ProcessId].acc0, Local[ProcessId].acc0, ai);
if(Type(p) != BODY) { /* a body-cell/leaf interaction? */
Local[ProcessId].mynbcterm++;
#ifdef QUADPOLE
dr5inv = 1.0/(Local[ProcessId].drsq * Local[ProcessId].drsq * drabs);
MULMV(quaddr, Quad(p), Local[ProcessId].dr);
DOTVP(drquaddr, Local[ProcessId].dr, quaddr);
phiquad = -0.5 * dr5inv * drquaddr;
Local[ProcessId].phi0 += phiquad;
phiquad = 5.0 * phiquad / Local[ProcessId].drsq;
MULVS(ai, Local[ProcessId].dr, phiquad);
SUBV(Local[ProcessId].acc0, Local[ProcessId].acc0, ai);
MULVS(quaddr, quaddr, dr5inv);
SUBV(Local[ProcessId].acc0, Local[ProcessId].acc0, quaddr);
#endif
}
else { /* a body-body interaction */
Local[ProcessId].myn2bterm++;
}
}
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);
}
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);
}
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);
}
