Пример #1
0
/*
** _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;
}
Пример #2
0
Файл: tq.c Проект: rblake/seg3d2
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);
}