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);
}
void setauxvar(bodyptr btab, int nbody)
{
bodyptr bp;
vector jvec;
real jtot, etot, r0, r1, r;
if (streq(getparam("auxvar"), "mass"))
for (bp = btab; bp < NthBody(btab, nbody); bp = NextBody(bp))
Aux(bp) = mass_gsp(ggsp, absv(Pos(bp)));
else if (streq(getparam("auxvar"), "rperi"))
for (bp = btab; bp < NthBody(btab, nbody); bp = NextBody(bp)) {
CROSSVP(jvec, Pos(bp), Vel(bp));
jtot = absv(jvec);
etot = 0.5 * dotvp(Vel(bp), Vel(bp)) + Phi(bp);
r0 = 0.0;
r1 = absv(Pos(bp));
r = 0.5 * (r0 + r1);
while ((r1 - r0) > TOL * r) {
if (rsqrt(2 * (etot - phi_gsp(ggsp, r))) > jtot/r)
r1 = r;
else
r0 = r;
r = 0.5 * (r0 + r1);
}
Aux(bp) = r;
}
else
error("%s: unknown auxvar option %s\n",
getargv0(), getparam("auxvar"));
}
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 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(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);
}
