axesrot.c

Go to the documentation of this file.

/*
    This file is part of magpar.

    Copyright (C) 2002-2010 Werner Scholz

    www:   http://www.magpar.net/
    email: magpar(at)magpar.net

    magpar is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    magpar is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with magpar; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

static char const Id[] = "$Id: axesrot.c 2962 2010-02-04 19:50:44Z scholz $\n\n";
static char const Td[] = "$Today: " __FILE__ "  " __DATE__ "  "  __TIME__  " $\n\n";

#include "util.h"
#include "petscblaslapack.h"

int checkortho(PetscReal *axes1)
{
  PetscReal vec[ND];

  /* check norm of column vectors */
  if (
    my_dnrm2(ND,axes1,1)<D_EPS ||
    my_dnrm2(ND,axes1+ND,1)<D_EPS ||
    my_dnrm2(ND,axes1+2*ND,1)<D_EPS
  ) {
      PetscPrintf(PETSC_COMM_WORLD,"rotation matrix is singular!\n");
      return(0);
  }

  /* check that axes1 defines an orthogonal system */
  if (
    PetscAbsReal(my_ddot(ND,axes1,1,axes1+ND,1))>D_EPS ||
    PetscAbsReal(my_ddot(ND,axes1,1,axes1+2*ND,1))>D_EPS ||
    PetscAbsReal(my_ddot(ND,axes1+ND,1,axes1+2*ND,1))>D_EPS
  ) {
      PetscPrintf(PETSC_COMM_WORLD,"rotation matrix does not form an orthogonal system!\n");
      return(0);
  }

  /* check handedness */
  douter(ND,axes1,axes1+ND,vec);
  if (PetscAbsReal(my_ddot(ND,axes1+2*ND,1,vec,1))<D_EPS) {
    PetscPrintf(PETSC_COMM_WORLD,"rotation matrix left-handed or singular!\n");
    return(0);
  }

  return(1);
}

int AxesRotation(PetscReal *slice_p,PetscReal *slice_n,PetscReal *axes1,int inv,
      int n_vert,PetscReal *vertxyz,PetscReal *vertxyz2)
{
  MagparFunctionLogBegin;

  /* first, try to calculate rotation matrix based on slice_n */
  if (slice_n!=PETSC_NULL && my_dnrm2(ND,slice_n,1)>D_EPS) {

    PetscPrintf(PETSC_COMM_WORLD,
      "perpendicular plane: %g*x+%g*y+%g*z=%g\n",
      slice_n[0],
      slice_n[1],
      slice_n[2],
      my_ddot(ND,slice_n,1,slice_p,1)
    );

    PetscReal t_norm;
    t_norm=1.0/my_dnrm2(ND,slice_n,1);
    PetscPrintf(PETSC_COMM_WORLD,
      "normalized normal: (%g,%g,%g)\n",
      slice_n[0]/t_norm,slice_n[1]/t_norm,slice_n[2]/t_norm
    );

    /* calculate cross product of y-axis with slice_n and store in axes1 */
    PetscReal axes3[ND*ND]={1,0,0,0,1,0,0,0,1};
    douter(ND,axes3+ND*1,slice_n,axes1);
    t_norm=my_dnrm2(ND,axes1,1);

    /* if norm of axes1 does not vanish then renormalize */
    if (t_norm > 1000*D_EPS) {
      my_dscal(ND,1.0/t_norm,axes1,1);
    }
    else {
      /* calculate cross product of x-axis with slice_n and store in axes1 */
      douter(ND,axes3,slice_n,axes1);

      /* this time the norm must not vanish */
      t_norm=my_dnrm2(ND,axes1,1);
      assert(t_norm > D_EPS);
      my_dscal(ND,1.0/t_norm,axes1,1);
    }

    douter(ND,slice_n,axes1,axes1+ND*1);
    t_norm=1.0/my_dnrm2(ND,axes1+ND*1,1);
    my_dscal(ND,t_norm,axes1+ND*1,1);

    my_dcopy(ND,slice_n,1,axes1+ND*2,1);
    t_norm=1.0/my_dnrm2(ND,axes1+ND*2,1);
    my_dscal(ND,t_norm,axes1+ND*2,1);

    /* axes1 is now a set of three orthogonal axes
       where axes1+ND*2==slice_n
    */
  }

  /* check rotation matrix */
  if (!checkortho(axes1)) {

    /* try to fix third vector */
    douter(ND,axes1,axes1+ND,axes1+2*ND);

    assert(checkortho(axes1));
  }

  PetscPrintf(PETSC_COMM_WORLD,
    "rotation matrix:\n"
    "%10g %10g %10g\n"
    "%10g %10g %10g\n"
    "%10g %10g %10g\n",
    axes1[0],axes1[3],axes1[6],
    axes1[1],axes1[4],axes1[7],
    axes1[2],axes1[5],axes1[8]
  );

  /* double check validity */
  assert(checkortho(axes1));

  /* copy vertex coordinates into new array */
  ierr = PetscMemcpy(vertxyz2,vertxyz,ND*n_vert*sizeof(PetscReal));CHKERRQ(ierr);

  if (inv==0) {
    PetscPrintf(PETSC_COMM_WORLD,"applying normal rotation\n");

    /* translate the whole model to move the point into the origin */
    for (int i=0;i<n_vert;i++) {
      my_daxpy(ND,-1.0,slice_p,1,vertxyz2+ND*i,1);
    }
    PetscPrintf(PETSC_COMM_WORLD,
      "shifted by -(%g,%g,%g)\n",
      slice_p[0],slice_p[1],slice_p[2]
    );

    /* rotate vertxyz */
    for (int i=0;i<n_vert;i++) {
      PetscReal      DZero=0.0;
      PetscReal      DOne=1.0;
      int            IOne=1;
      int            nd=ND;
      PetscReal      p[ND];

/*
      cblas_dgemv(CblasColMajor,CblasTrans,ND,ND,1.0,axes1,ND,vertxyz2+ND*i,1,0.0,p,1);
*/
      BLASgemv_("T",&nd,&nd,&DOne,axes1,&nd,vertxyz2+ND*i,&IOne,&DZero,p,&IOne);
      my_dcopy(ND,p,1,vertxyz2+ND*i,1);
    }
  }
  else {
    PetscPrintf(PETSC_COMM_WORLD,"applying inverse rotation\n");

    /* rotate vertxyz */
    for (int i=0;i<n_vert;i++) {
      PetscReal      DZero=0.0;
      PetscReal      DOne=1.0;
      int            IOne=1;
      int            nd=ND;
      PetscReal      p[ND];

/*
      cblas_dgemv(CblasColMajor,CblasTrans,ND,ND,1.0,axes1,ND,vertxyz2+ND*i,1,0.0,p,1);
*/
      BLASgemv_("N",&nd,&nd,&DOne,axes1,&nd,vertxyz2+ND*i,&IOne,&DZero,p,&IOne);
      my_dcopy(ND,p,1,vertxyz2+ND*i,1);
    }

    /* translate the whole model */
    for (int i=0;i<n_vert;i++) {
      my_daxpy(ND,1.0,slice_p,1,vertxyz2+ND*i,1);
    }
    PetscPrintf(PETSC_COMM_WORLD,
      "shifted by +(%g,%g,%g)\n",
      slice_p[0],slice_p[1],slice_p[2]
    );
  }

  MagparFunctionLogReturn(0);
}