double
_energyInterParticle(pullTask *task, pullPoint *me, pullPoint *she,
/* output */
double egrad[4]) {
char meme[]="_energyInterParticle";
double spadist, sparad, diff[4], rr, enr, frc, *parm;
ELL_4V_SUB(diff, she->pos, me->pos);
spadist = ELL_3V_LEN(diff);
sparad = task->pctx->radiusSpace;
rr = spadist/(2*sparad);
/*
fprintf(stderr, "!%s: rr(%u,%u) = %g\n", meme, me->idtag, she->idtag, rr);
*/
if (rr > 1) {
ELL_4V_SET(egrad, 0, 0, 0, 0);
return 0;
}
if (rr == 0) {
fprintf(stderr, "%s: pos of pts %u, %u equal: (%g,%g,%g,%g)\n",
meme, me->idtag, she->idtag,
me->pos[0], me->pos[1], me->pos[2], me->pos[3]);
ELL_4V_SET(egrad, 0, 0, 0, 0);
return 0;
}
parm = task->pctx->energySpec->parm;
enr = task->pctx->energySpec->energy->eval(&frc, rr, parm);
frc *= -1.0/(2*sparad*spadist);
ELL_3V_SCALE(egrad, frc, diff);
egrad[3] = 0;
/*
fprintf(stderr, "%s: %u <-- %u = %g,%g,%g -> egrad = %g,%g,%g, enr = %g\n",
meme, me->idtag, she->idtag,
diff[0], diff[1], diff[2],
egrad[0], egrad[1], egrad[2], enr);
*/
return enr;
}
double
_energyFromPoints(pullTask *task, pullBin *bin, pullPoint *point,
/* output */
double egradSum[4]) {
/* char me[]="_energyFromPoints"; */
double energySum, distSqSum, spaDistSqMax, wghtSum;
int nopt, /* optimiziation: we sometimes re-use neighbor lists */
ntrue; /* we search all possible neighbors, stored in the bins
(either because !nopt, or, this iter we learn true
subset of interacting neighbors). This could also
be called "dontreuse" or something like that */
unsigned int nidx,
nnum; /* how much of task->neigh[] we use */
/* set nopt and ntrue */
if (task->pctx->neighborTrueProb < 1) {
nopt = AIR_TRUE;
if (egradSum) {
/* We allow the neighbor list optimization only when we're also asked
to compute the energy gradient. When we're not getting the energy
gradient, we're being called to test the waters at possible new
locations, in which case we can't be changing the effective particle
neighborhood */
ntrue = (0 == task->pctx->iter
|| airDrandMT_r(task->rng) < task->pctx->neighborTrueProb);
} else {
ntrue = AIR_FALSE;
}
} else {
nopt = AIR_FALSE;
ntrue = AIR_TRUE;
}
/*
fprintf(stderr, "!%s(%u), nopt = %d, ntrue = %d\n", me, point->idtag,
nopt, ntrue);
*/
/* set nnum and task->neigh[] */
if (ntrue) {
nnum = _neighBinPoints(task, bin, point);
if (nopt) {
airArrayLenSet(point->neighArr, 0);
}
} else {
/* (nopt true) this iter we re-use existing neighbor list */
nnum = point->neighNum;
for (nidx=0; nidx<nnum; nidx++) {
task->neighPoint[nidx] = point->neighPoint[nidx];
}
}
/* loop through neighbor points */
spaDistSqMax = 4*task->pctx->radiusSpace*task->pctx->radiusSpace;
/*
fprintf(stderr, "%s: radiusSpace = %g -> spaDistSqMax = %g\n", me,
task->pctx->radiusSpace, spaDistSqMax);
*/
wghtSum = 0;
energySum = 0;
distSqSum = 0;
point->neighInterNum = 0;
point->neighDist = 0.0;
point->neighMode = 0.0;
if (egradSum) {
ELL_4V_SET(egradSum, 0, 0, 0, 0);
}
for (nidx=0; nidx<nnum; nidx++) {
double diff[4], spaDistSq, enr, egrad[4];
pullPoint *herPoint;
herPoint = task->neighPoint[nidx];
ELL_4V_SUB(diff, herPoint->pos, point->pos);
spaDistSq = ELL_3V_DOT(diff, diff);
/*
fprintf(stderr, "!%s: %u:%g,%g,%g <-- %u:%g,%g,%g = sqd %g %s %g\n", me,
point->idtag, point->pos[0], point->pos[1], point->pos[2],
herPoint->idtag,
herPoint->pos[0], herPoint->pos[1], herPoint->pos[2],
spaDistSq, spaDistSq > spaDistSqMax ? ">" : "<=", spaDistSqMax);
*/
if (spaDistSq > spaDistSqMax) {
continue;
}
if (AIR_ABS(diff[3] > task->pctx->radiusScale)) {
continue;
}
enr = _energyInterParticle(task, point, herPoint, egrad);
/*
fprintf(stderr, "!%s: energySum = %g + %g = %g\n", me,
energySum, enr, energySum + enr);
*/
energySum += enr;
if (egradSum) {
ELL_4V_INCR(egradSum, egrad);
if (ELL_4V_DOT(egrad, egrad)) {
point->neighInterNum++;
point->neighDist = spaDistSq;
if (task->pctx->ispec[pullInfoTangentMode]) {
double w, m;
m = _pullPointScalar(task->pctx, herPoint, pullInfoTangentMode,
NULL, NULL);
w = 1.0/spaDistSq;
point->neighMode += w*m;
wghtSum += w;
}
if (nopt && ntrue) {
unsigned int ii;
ii = airArrayLenIncr(point->neighArr, 1);
point->neighPoint[ii] = herPoint;
}
}
}
}
/* finish computing things averaged over neighbors */
if (point->neighInterNum) {
point->neighDist = sqrt(point->neighDist/point->neighInterNum);
point->neighMode /= wghtSum;
} else {
point->neighDist = -1;
point->neighMode = AIR_NAN;
}
return energySum;
}
int
main(int argc, char *argv[]) {
float angleA_f, axisA_f[3], angleB_f, axisB_f[3],
qA_f[4], qB_f[4], qC_f[4],
mat3A_f[9], mat4A_f[16], mat3B_f[9], mat4B_f[16], mat3C_f[9], mat4C_f[16],
pntA_f[4], pntB_f[4], pntC_f[4];
double angleA_d, axisA_d[3], angleB_d, axisB_d[3],
qA_d[4], qB_d[4], qC_d[4],
mat3A_d[9], mat4A_d[16], mat3B_d[9], mat4B_d[16], mat3C_d[9], mat4C_d[16],
pntA_d[4], pntB_d[4], pntC_d[4];
int I, N;
double tmp, det, frob;
me = argv[0];
N = 100000;
AIR_UNUSED(pntA_d);
AIR_UNUSED(pntB_d);
AIR_UNUSED(pntC_d);
AIR_UNUSED(mat4C_d);
AIR_UNUSED(mat3C_d);
AIR_UNUSED(mat4B_d);
AIR_UNUSED(mat3B_d);
AIR_UNUSED(mat4A_d);
AIR_UNUSED(mat3A_d);
AIR_UNUSED(qC_d);
AIR_UNUSED(qB_d);
AIR_UNUSED(qA_d);
AIR_UNUSED(axisB_d);
AIR_UNUSED(angleB_d);
AIR_UNUSED(axisA_d);
AIR_UNUSED(angleA_d);
AIR_UNUSED(argc);
for (I=0; I<N; I++) {
/* make a rotation (as a quaternion) */
ELL_3V_SET(axisA_f, 2*airDrandMT()-1, 2*airDrandMT()-1, 2*airDrandMT()-1);
ELL_3V_NORM(axisA_f, axisA_f, tmp); /* yea, not uniform, so what */
angleA_f = AIR_PI*(2*airDrandMT()-1);
ell_aa_to_q_f(qA_f, angleA_f, axisA_f);
/* convert to AA and back, and back */
angleB_f = ell_q_to_aa_f(axisB_f, qA_f);
if (ELL_3V_DOT(axisB_f, axisA_f) < 0) {
ELL_3V_SCALE(axisB_f, -1, axisB_f);
angleB_f *= -1;
}
ELL_3V_SUB(axisA_f, axisA_f, axisB_f);
printf(" aa -> q -> aa error: %g, %g\n",
CA + AIR_ABS(angleA_f - angleB_f), CA + ELL_3V_LEN(axisA_f));
/* convert to 3m and back, and back */
ell_q_to_3m_f(mat3A_f, qA_f);
ell_3m_to_q_f(qB_f, mat3A_f);
if (ELL_4V_DOT(qA_f, qB_f) < 0) {
ELL_4V_SCALE(qB_f, -1, qB_f);
}
ELL_4V_SUB(qC_f, qA_f, qB_f);
ELL_Q_TO_3M(mat3B_f, qA_f);
ELL_3M_SUB(mat3C_f, mat3B_f, mat3A_f);
printf(" q -> 3m -> q error: %g, %g\n",
CA + ELL_4V_LEN(qC_f), CA + ELL_3M_FROB(mat3C_f));
/* convert to 4m and back, and back */
ell_q_to_4m_f(mat4A_f, qA_f);
ell_4m_to_q_f(qB_f, mat4A_f);
if (ELL_4V_DOT(qA_f, qB_f) < 0) {
ELL_4V_SCALE(qB_f, -1, qB_f);
}
ELL_4V_SUB(qC_f, qA_f, qB_f);
ELL_Q_TO_4M(mat4B_f, qA_f);
ELL_4M_SUB(mat4C_f, mat4B_f, mat4A_f);
printf(" q -> 4m -> q error: %g, %g\n",
CA + ELL_4V_LEN(qC_f), CA + ELL_4M_FROB(mat4C_f));
/* make a point that we'll rotate */
ELL_3V_SET(pntA_f, 2*airDrandMT()-1, 2*airDrandMT()-1, 2*airDrandMT()-1);
/* effect rotation in two different ways, and compare results */
ELL_3MV_MUL(pntB_f, mat3A_f, pntA_f);
ell_q_3v_rotate_f(pntC_f, qA_f, pntA_f);
ELL_3V_SUB(pntA_f, pntB_f, pntC_f);
printf(" rotation error = %g\n", CA + ELL_3V_LEN(pntA_f));
/* mix up inversion with conversion */
ell_3m_inv_f(mat3C_f, mat3A_f);
ell_3m_to_q_f(qB_f, mat3C_f);
ell_q_mul_f(qC_f, qA_f, qB_f);
if (ELL_4V_DOT(qA_f, qC_f) < 0) {
ELL_4V_SCALE(qC_f, -1, qC_f);
}
printf(" inv mul = %g %g %g %g\n", qC_f[0],
CA + qC_f[1], CA + qC_f[2], CA + qC_f[3]);
ell_q_inv_f(qC_f, qB_f);
ELL_4V_SUB(qC_f, qB_f, qB_f);
printf(" inv diff = %g %g %g %g\n", CA + qC_f[0],
CA + qC_f[1], CA + qC_f[2], CA + qC_f[3]);
/* exp and log */
ell_q_log_f(qC_f, qA_f);
ell_q_log_f(qB_f, qC_f);
ell_q_exp_f(qC_f, qB_f);
ell_q_exp_f(qB_f, qC_f);
ELL_4V_SUB(qC_f, qB_f, qA_f);
printf(" exp/log diff = %g %g %g %g\n", CA + qC_f[0],
CA + qC_f[1], CA + qC_f[2], CA + qC_f[3]);
/* pow, not very exhaustive */
ell_q_to_3m_f(mat3A_f, qA_f);
ell_3m_post_mul_f(mat3A_f, mat3A_f);
ell_3m_post_mul_f(mat3A_f, mat3A_f);
ell_q_pow_f(qB_f, qA_f, 4);
ell_q_to_3m_f(mat3B_f, qB_f);
ELL_3M_SUB(mat3B_f, mat3B_f, mat3A_f);
printf(" pow diff = %g\n", CA + ELL_3M_FROB(mat3B_f));
if (ELL_3M_FROB(mat3B_f) > 2) {
printf(" start q = %g %g %g %g\n", qA_f[0], qA_f[1], qA_f[2], qA_f[3]);
angleA_f = ell_q_to_aa_f(axisA_f, qA_f);
printf(" --> aa = %g (%g %g %g)\n", angleA_f,
axisA_f[0], axisA_f[1], axisA_f[2]);
printf(" q^3 = %g %g %g %g\n", qB_f[0], qB_f[1], qB_f[2], qB_f[3]);
angleA_f = ell_q_to_aa_f(axisA_f, qB_f);
printf(" --> aa = %g (%g %g %g)\n", angleA_f,
axisA_f[0], axisA_f[1], axisA_f[2]);
exit(1);
}
/* make sure it looks like a rotation matrix */
ell_q_to_3m_f(mat3A_f, qA_f);
det = ELL_3M_DET(mat3A_f);
frob = ELL_3M_FROB(mat3A_f);
ELL_3M_TRANSPOSE(mat3B_f, mat3A_f);
ell_3m_inv_f(mat3C_f, mat3A_f);
ELL_3M_SUB(mat3C_f, mat3B_f, mat3C_f);
printf(" det = %g; size = %g; err = %g\n", det, frob*frob/3,
CA + ELL_3M_FROB(mat3C_f));
}
exit(0);
}
