/*
** _pullConstraintTangent
**
** eigenvectors (with non-zero eigenvalues) of output proj are
** (hopefully) approximate tangents to the manifold to which particles
** are constrained. It is *not* the local tangent of the directions
** along which particles are allowed to move during constraint
** satisfaction (that is given by creaseProj for creases)
**
** this can assume that probe() has just been called
*/
void
_pullConstraintTangent(pullTask *task, pullPoint *point,
/* output */
double proj[9]) {
double vec[4], nvec[3], outer[9], len, posproj[9], negproj[9];
ELL_3M_IDENTITY_SET(proj); /* NOTE: we are starting with identity . . . */
switch (task->pctx->constraint) {
case pullInfoHeight:
creaseProj(task, point,
!!task->pctx->ispec[pullInfoTangent1],
!!task->pctx->ispec[pullInfoTangent2],
!!task->pctx->ispec[pullInfoNegativeTangent1],
!!task->pctx->ispec[pullInfoNegativeTangent2],
posproj, negproj);
/* .. and subracting out output from creaseProj */
ELL_3M_SUB(proj, proj, posproj);
ELL_3M_SUB(proj, proj, negproj);
break;
case pullInfoHeightLaplacian:
case pullInfoIsovalue:
if (pullInfoHeightLaplacian == task->pctx->constraint) {
/* using gradient of height as approx normal to laplacian 0-crossing */
pullPointScalar(task->pctx, point, pullInfoHeight, vec, NULL);
} else {
pullPointScalar(task->pctx, point, pullInfoIsovalue, vec, NULL);
}
ELL_3V_NORM(nvec, vec, len);
if (len) {
/* .. or and subracting out tensor product of normal with itself */
ELL_3MV_OUTER(outer, nvec, nvec);
ELL_3M_SUB(proj, proj, outer);
}
break;
}
return;
}
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);
}