void polymodel(void)
{
gsl_interp_accel *pmsplacc = gsl_interp_accel_alloc();
bodyptr p;
real rad, phi, vel, psi, vr, vp, a, E, J;
vector rhat, vtmp, vper;
for (p = btab; p < NthBody(btab, nbody); p = NextBody(p)) {
rad = rad_m(xrandom(0.0, mtot));
phi = gsl_spline_eval(pmspline, (double) rad, pmsplacc);
vel = pick_v(phi);
psi = pick_psi();
vr = vel * rcos(psi);
vp = vel * rsin(psi);
Mass(p) = mtot / nbody;
pickshell(rhat, NDIM, 1.0);
MULVS(Pos(p), rhat, rad);
pickshell(vtmp, NDIM, 1.0);
a = dotvp(vtmp, rhat);
MULVS(vper, rhat, - a);
ADDV(vper, vper, vtmp);
a = absv(vper);
MULVS(vper, vper, vp / a);
MULVS(Vel(p), rhat, vr);
ADDV(Vel(p), Vel(p), vper);
Phi(p) = phi;
E = phi + 0.5 * rsqr(vel);
J = rad * ABS(vp);
Aux(p) = Kprime * rpow(phi1 - E, npol - 1.5) * rpow(J, 2 * mpol);
}
gsl_interp_accel_free(pmsplacc);
}
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 */
}
}
void gspmodel(void)
{
real beta_a, mcut, vcut, vfac;
static real *sig2 = NULL;
real r, vmax2, sigr, sig, x, y, vr, v1, v2;
bodyptr bp;
vector rhat, vec1, vec2, vtmp;
beta_a = getdparam("beta_a");
assert(beta_a <= 1.0);
nbody = getiparam("nbody");
assert(nbody > 0);
mcut = getdparam("mcut");
assert(0.0 < mcut && mcut <= 1.0);
vcut = getdparam("vcut");
assert(vcut > 0.0);
if (sig2 == NULL)
sig2 = calc_sig2_gsp(gsp, ggsp, beta_a);
if (btab == NULL)
btab = (bodyptr) allocate(nbody * SizeofBody);
vfac = rsqrt(1 - beta_a);
for (bp = btab; bp < NthBody(btab, nbody); bp = NextBody(bp)) {
Mass(bp) = gsp->mtot / nbody;
r = r_mass_gsp(gsp, xrandom(0.0, mcut * gsp->mtot));
vmax2 = -2 * rsqr(vcut) * phi_gsp(ggsp, r);
if (vfac > 1.0)
vmax2 = vmax2 / rsqr(vfac);
sigr = rsqrt(sig2_gsp(gsp, ggsp, beta_a, sig2, r));
sig = fixsigma(sigr, rsqrt(vmax2));
do {
vr = grandom(0.0, sig);
v1 = grandom(0.0, sig);
v2 = grandom(0.0, sig);
} while (vr*vr + v1*v1 + v2*v2 > vmax2);
picktriad(rhat, vec1, vec2);
MULVS(Pos(bp), rhat, r);
MULVS(Vel(bp), rhat, vr);
MULVS(vtmp, vec1, v1 * vfac);
ADDV(Vel(bp), Vel(bp), vtmp);
MULVS(vtmp, vec2, v2 * vfac);
ADDV(Vel(bp), Vel(bp), vtmp);
Phi(bp) = phi_gsp(ggsp, r);
Aux(bp) = Phi(bp) + 0.5 * dotvp(Vel(bp), Vel(bp));
}
if (getbparam("besort"))
qsort(btab, nbody, SizeofBody, berank);
if (getbparam("zerocm"))
snapcenter(btab, nbody, MassField.offset);
if (! strnull(getparam("auxvar")))
setauxvar(btab, nbody);
}
void snaprect(bodyptr btab, int nbody)
{
matrix qmat, tmpm;
bodyptr bp;
vector frame[3], tmpv;
static vector oldframe[3] =
{ { 1.0, 0.0, 0.0, }, { 0.0, 1.0, 0.0, }, { 0.0, 0.0, 1.0, }, };
int i;
snapcenter(btab, nbody, WeightField.offset);
CLRM(qmat);
for (bp = btab; bp < NthBody(btab, nbody); bp = NextBody(bp)) {
OUTVP(tmpm, Pos(bp), Pos(bp));
MULMS(tmpm, tmpm, Weight(bp));
ADDM(qmat, qmat, tmpm);
}
eigenvect(frame[0], frame[1], frame[2], qmat);
if (dotvp(oldframe[0], frame[0]) < 0.0)
MULVS(frame[0], frame[0], -1.0);
if (dotvp(oldframe[2], frame[2]) < 0.0)
MULVS(frame[2], frame[2], -1.0);
CROSSVP(frame[1], frame[2], frame[0]);
printvect("e_x:", frame[0]);
printvect("e_y:", frame[1]);
printvect("e_z:", frame[2]);
for (bp = btab; bp < NthBody(btab, nbody); bp = NextBody(bp)) {
if (PosField.offset != BadOffset) {
for (i = 0; i < NDIM; i++)
tmpv[i] = dotvp(Pos(bp), frame[i]);
SETV(Pos(bp), tmpv);
}
if (VelField.offset != BadOffset) {
for (i = 0; i < NDIM; i++)
tmpv[i] = dotvp(Vel(bp), frame[i]);
SETV(Vel(bp), tmpv);
}
if (AccField.offset != BadOffset) {
for (i = 0; i < NDIM; i++)
tmpv[i] = dotvp(Acc(bp), frame[i]);
SETV(Acc(bp), tmpv);
}
if (AuxVecField.offset != BadOffset) {
for (i = 0; i < NDIM; i++)
tmpv[i] = dotvp(AuxVec(bp), frame[i]);
SETV(AuxVec(bp), tmpv);
}
}
for (i = 0; i < NDIM; i++)
SETV(oldframe[i], frame[i]);
}
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 gspforces(bodyptr btab, int nbody, gsprof *gravgsp)
{
bodyptr bp;
real r, mr3i;
for (bp = btab; bp < NthBody(btab, nbody); bp = NextBody(bp)) {
r = absv(Pos(bp));
Phi(bp) = phi_gsp(gravgsp, r);
mr3i = mass_gsp(gravgsp, r) / rqbe(r);
MULVS(Acc(bp), Pos(bp), -mr3i);
}
}
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);
}
local void hqmforces(bodyptr btab, int nbody, real M, real a, real b,
real tol)
{
bodyptr bp;
double r, mr3i, params[4], phi0, aR0, az0, abserr[3];
static gsl_integration_workspace *wksp = NULL;
gsl_function FPhi, F_aR, F_az;
static double maxerr = 0.0;
int stat[3];
if (a == b) { // spherical case is easy!
for (bp = btab; bp < NthBody(btab, nbody); bp = NextBody(bp)) {
r = absv(Pos(bp));
Phi(bp) = - M / (a + r);
mr3i = M * rsqr(r / (a + r)) / rqbe(r);
MULVS(Acc(bp), Pos(bp), - mr3i);
}
} else { // flattened case is harder
if (wksp == NULL) { // on first call, initialze
wksp = gsl_integration_workspace_alloc(1000);
gsl_set_error_handler_off(); // handle errors below
}
FPhi.function = &intPhi;
F_aR.function = &int_aR;
F_az.function = &int_az;
FPhi.params = F_aR.params = F_az.params = params;
a2(params) = rsqr(a);
b2(params) = rsqr(b);
for (bp = btab; bp < NthBody(btab, nbody); bp = NextBody(bp)) {
R(params) = rsqrt(rsqr(Pos(bp)[0]) + rsqr(Pos(bp)[1]));
z(params) = Pos(bp)[2];
stat[0] = gsl_integration_qagiu(&FPhi, 0.0, tol, 0.0,
1000, wksp, &phi0, &abserr[0]);
stat[1] = gsl_integration_qagiu(&F_aR, 0.0, tol, 0.0,
1000, wksp, &aR0, &abserr[1]);
stat[2] = gsl_integration_qagiu(&F_az, 0.0, tol, 0.0,
1000, wksp, &az0, &abserr[2]);
if (stat[0] || stat[1] || stat[2]) // any errors reported?
for (int i = 0; i < 3; i++)
if (stat[i] != 0 && abserr[i] > maxerr) {
eprintf("[%s.hqmforces: warning: %s abserr[%d] = %g]\n",
getprog(), gsl_strerror(stat[i]), i+1, abserr[i]);
maxerr = abserr[i]; // adjust reporting threshold
}
Phi(bp) = - M * phi0;
Acc(bp)[0] = - M * (Pos(bp)[0] / R(params)) * aR0;
Acc(bp)[1] = - M * (Pos(bp)[1] / R(params)) * aR0;
Acc(bp)[2] = - M * az0;
}
}
}
local void centersnap(Body *btab, int nb)
{
real mtot;
vector cmphase[2], tmp;
Body *bp;
mtot = 0.0;
CLRV(cmphase[0]);
CLRV(cmphase[1]);
for (bp = btab; bp < btab + nb; bp++) {
mtot = mtot + Mass(bp);
MULVS(tmp, Phase(bp)[0], Mass(bp));
ADDV(cmphase[0], cmphase[0], tmp);
MULVS(tmp, Phase(bp)[1], Mass(bp));
ADDV(cmphase[1], cmphase[1], tmp);
}
MULVS(cmphase[0], cmphase[0], 1.0/mtot);
MULVS(cmphase[1], cmphase[1], 1.0/mtot);
for (bp = btab; bp < btab + nb; bp++) {
SUBV(Phase(bp)[0], Phase(bp)[0], cmphase[0]);
SUBV(Phase(bp)[1], Phase(bp)[1], cmphase[1]);
}
}
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++;
}
}
void polymodel1(void)
{
bodyptr p;
real r, x, v;
for (p = btab; p < NthBody(btab, nbody); p = NextBody(p)) {
Mass(p) = 1.0 / nbody;
r = xrandom(0.0, 2.0);
pickshell(Pos(p), 3, r);
x = SQRT2 * rcos(PI * (2.0 - xrandom(0.0, 1.0)) / 4.0);
v = (xrandom(-1.0, 1.0) < 0.0 ? -1.0 : 1.0) *
(1 - x*x) * rsqrt(rlog(2 / r));
MULVS(Vel(p), Pos(p), v/r);
Phi(p) = 0.5 * rlog(r / 2.0) - 0.5;
}
bodyfields[4] = NULL; // don't output Aux data
}
// 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;
}
