sics/tasublib.c

/**
 * This is a library of functions and data structures for performing 
 * triple axis spectrometer angle calculations using the UB-matrix
 * formalism as described by Mark Lumsden, to appear in Acta Cryst.
 *
 * copyright: see file COPYRIGHT
 *
 * Mark Koennecke, April 2005
 */
#include <math.h>
#include <stdlib.h>
#include <assert.h>
#include "trigd.h"
#include "vector.h"
#include "tasublib.h"

#define ABS(x) (x < 0 ? -(x) : (x))
#define PI 3.141592653589793
#define ECONST 2.072
#define DEGREE_RAD      (PI/180.0)      /* Radians per degree */
#define VERT 0
#define HOR  1
#define INPLANEPREC 0.01
/*============== monochromator/analyzer stuff =========================*/
double energyToK(double energy)
{
  double K;

  K = sqrt(energy / ECONST);
  return K;
}

/*---------------------------------------------------------------------*/
double KtoEnergy(double k)
{
  double energy;

  energy = ECONST * k * k;
  return energy;
}

/*-------------------------------------------------------------------*/
static double calcCurvature(double B1, double B2, double theta, int ori)
{
  assert(ori == VERT || ori == HOR);
  if (ori == VERT) {
    return B1 + B2 / Sind(ABS(theta));
  } else {
    return B1 + B2 * Sind(ABS(theta));
  }
}

/*--------------------------------------------------------------------*/
int maCalcTwoTheta(maCrystal data, double k, double *two_theta)
{
  double fd, theta;

  /* fd = k/(2.*data.dd); */
  fd = PI / (data.dd * k);
  if (fd > 1.0) {
    return ENERGYTOBIG;
  }
  theta = Asind(fd) * data.ss;
  *two_theta = 2. * theta;
  return 1;
}

/*--------------------------------------------------------------------*/
double maCalcVerticalCurvature(maCrystal data, double two_theta)
{
  return calcCurvature(data.VB1, data.VB2, two_theta / 2., VERT);
}

/*-------------------------------------------------------------------*/
double maCalcHorizontalCurvature(maCrystal data, double two_theta)
{
  return calcCurvature(data.HB1, data.HB2, two_theta / 2., HOR);
}

/*--------------------------------------------------------------------*/
double maCalcK(maCrystal data, double two_theta)
{
  double k;
  k = ABS(data.dd * Sind(two_theta / 2));
  if (ABS(k) > .001) {
    k = PI / k;
  } else {
    k = .0;
  }
  return k;
}

/*==================== reciprocal space ==============================*/
static MATRIX tasReflectionToHC(tasQEPosition r, MATRIX B)
{
  MATRIX h = NULL, hc = NULL;

  h = makeVector();
  if (h == NULL) {
    return NULL;
  }
  vectorSet(h, 0, r.qh);
  vectorSet(h, 1, r.qk);
  vectorSet(h, 2, r.ql);

  hc = mat_mul(B, h);
  killVector(h);
  return hc;
}

/*------------------------------------------------------------------
 a quadrant dependent tangens
 ------------------------------------------------------------------*/
static double rtan(double y, double x)
{
  double val;

  if ((x == 0.) && (y == 0.)) {
    return .0;
  }
  if (x == 0.) {
    if (y < 0.) {
      return -PI / 2.;
    } else {
      return PI / 2.;
    }
  }
  if (ABS(y) < ABS(x)) {
    val = atan(ABS(y / x));
    if (x < 0.) {
      val = PI - val;
    }
    if (y < 0.) {
      val = -val;
    }
    return val;
  } else {
    val = PI / 2. - atan(ABS(x / y));
    if (x < 0.) {
      val = PI - val;
    }
    if (y < 0.) {
      val = -val;
    }
  }
  return val;
}

/*---------------------------------------------------------------*/
static double calcTheta(double ki, double kf, double two_theta)
{
  /**
   *               |ki| - |kf|cos(two_theta)
   * tan(theta) = --------------------------
   *                |kf|sin(two_theta)
   */
  return rtan(ABS(ki) - ABS(kf) * Cosd(two_theta),
              ABS(kf) * Sind(two_theta)) / DEGREE_RAD;
}

/*-------------------------------------------------------------------------*/
double tasAngleBetweenReflections(MATRIX B, tasReflection r1,
                                  tasReflection r2)
{
  MATRIX chi1, chi2, h1, h2;
  double angle = .0;

  h1 = makeVector();
  if (h1 == NULL) {
    return -9999.99;
  }
  h1[0][0] = r1.qe.qh;
  h1[1][0] = r1.qe.qk;
  h1[2][0] = r1.qe.ql;

  h2 = makeVector();
  if (h2 == NULL) {
    return -999.99;
  }
  h2[0][0] = r2.qe.qh;
  h2[1][0] = r2.qe.qk;
  h2[2][0] = r2.qe.ql;

  chi1 = mat_mul(B, h1);
  chi2 = mat_mul(B, h2);
  if (chi1 != NULL && chi2 != NULL) {
    angle = tasAngleBetween(chi1, chi2);
    killVector(chi1);
    killVector(chi2);
  }
  killVector(h1);
  killVector(h2);
  return angle;
}

/*--------------------------------------------------------------------*/
static MATRIX uFromAngles(double om, double sgu, double sgl)
{
  MATRIX u;

  u = makeVector();
  if (u == NULL) {
    return NULL;
  }
  vectorSet(u, 0, Cosd(om) * Cosd(sgl));
  vectorSet(u, 1, -Sind(om) * Cosd(sgu) + Cosd(om)* Sind(sgl)*Sind(sgu));
  vectorSet(u, 2,
            Sind(om) * Sind(sgu) + Cosd(om) * Sind(sgl) * Cosd(sgu));

  return u;
}

/*---------------------------------------------------------------*/
MATRIX calcTasUVectorFromAngles(tasReflection rr)
{
  double theta, om, ss;
  MATRIX m;
  tasReflection r;
 

  r = rr;
  if(r.angles.sample_two_theta < .0){
    ss = -1.;
  } else {
    ss = 1.;
  }
  r.angles.sample_two_theta = ABS(r.angles.sample_two_theta);
  

  theta = calcTheta(r.qe.ki, r.qe.kf, r.angles.sample_two_theta);
  om = r.angles.a3 - ss*theta;
  /*
    This here may have to do with scattering sense.....
  if (om < -180.){
    om += 180;
  }
  */
  m = uFromAngles(om, r.angles.sgu, ss*r.angles.sgl); 
  /*
  printf("Q = %f, %f, %f inwards Uv = %f %f %f, om = %f\n", r.qe.qh, r.qe.qk, r.qe.ql, 
	 m[0][0], m[1][0],m[2][0], om); 
  */
  return m ;
}

/*-----------------------------------------------------------------------------*/
static MATRIX tasReflectionToQC(tasQEPosition r, MATRIX UB)
{
  MATRIX Q, QC;

  Q = makeVector();
  if (Q == NULL) {
    return NULL;
  }
  vectorSet(Q, 0, r.qh);
  vectorSet(Q, 1, r.qk);
  vectorSet(Q, 2, r.ql);
  QC = mat_mul(UB, Q);
  killVector(Q);
  return QC;
}

/*-----------------------------------------------------------------*/
int makeAuxReflection(MATRIX B, tasReflection r1, tasReflection * r2,
                      int ss)
{
  double theta, om, q, cos2t;
  MATRIX QC;

  r2->angles = r1.angles;
  r2->qe.ki = r1.qe.ki;
  r2->qe.kf = r1.qe.kf;

  theta = calcTheta(r1.qe.ki, r1.qe.kf, 
      ss*r1.angles.sample_two_theta);
  om = r1.angles.a3 - ss*theta;
  om += tasAngleBetweenReflections(B, r1, *r2);

  QC = tasReflectionToHC(r2->qe, B);
  if (QC == NULL) {
    return UBNOMEMORY;
  }

  q = vectorLength(QC);
  q = 2. * PI * vectorLength(QC);
  cos2t = (r1.qe.ki * r1.qe.ki + r1.qe.kf * r1.qe.kf - q * q) /
      (2. * ABS(r1.qe.ki) * ABS(r1.qe.kf));
  if (ABS(cos2t) > 1.) {
    killVector(QC);
    return TRIANGLENOTCLOSED;
  }
  r2->angles.sample_two_theta = ss * Acosd(cos2t);
  theta = calcTheta(r1.qe.ki, r1.qe.kf, ss*r2->angles.sample_two_theta);
  r2->angles.a3 = om + ss*theta;

  r2->angles.a3 = fmod(r2->angles.a3 + ss*180.,360.) - ss*180.; 

  /*
  r2->angles.a3 -= 180.;
  if (r2->angles.a3 < -180.) {
    r2->angles.a3 += 360.;
  }
  */
  mat_free(QC);

  return 1;
}

/*------------------------------------------------------------------*/
int calcTwoTheta(MATRIX B, tasQEPosition ref, int ss, double *value)
{
  MATRIX QC;
  double cos2t, q;

  QC = tasReflectionToHC(ref, B);
  if (QC == NULL) {
    return UBNOMEMORY;
  }

  q = vectorLength(QC);
  q = 2. * PI * vectorLength(QC);
  killVector(QC);

  cos2t = (ref.ki * ref.ki + ref.kf * ref.kf - q * q) /
      (2. * ABS(ref.ki) * ABS(ref.kf));
  if (ABS(cos2t) > 1.) {
    return TRIANGLENOTCLOSED;
  }
  *value = ss * Acosd(cos2t);

  return 1;
}

/*-------------------------------------------------------------------*/
MATRIX calcPlaneNormal(tasReflection r1, tasReflection r2)
{
  MATRIX u1 = NULL, u2 = NULL, planeNormal = NULL;
  int i;

  u1 = calcTasUVectorFromAngles(r1);
  u2 = calcTasUVectorFromAngles(r2);
  if (u1 != NULL && u2 != NULL) {
    planeNormal = vectorCrossProduct(u1, u2);
    /*
       The plane normal has to point to the stars and not to the earth
       core in order for the algorithm to work.
     */
    /*
    if (planeNormal[2][0] < .0) {
      for (i = 0; i < 3; i++) {
        planeNormal[i][0] = -1. * planeNormal[i][0];
      }
    }
    */
    mat_free(u1);
    mat_free(u2);
    normalizeVector(planeNormal); 
    return planeNormal;
  } else {
    return NULL;
  }
}
/*-------------------------------------------------------------------*/
MATRIX calcPlaneNormalQOld(MATRIX UB, tasReflection r1, tasReflection r2)
{
  MATRIX u1 = NULL, u2 = NULL, planeNormal = NULL;
  int i;

  u1 = tasReflectionToQC(r1.qe, UB);
  u2 = tasReflectionToQC(r2.qe,UB);
  if (u1 != NULL && u2 != NULL) {
    planeNormal = vectorCrossProduct(u1, u2);
    /*
       The plane normal has to point to the stars and not to the earth
       core in order for the algorithm to work.
     */
    /*
    if (planeNormal[2][0] < .0) {
      for (i = 0; i < 3; i++) {
        planeNormal[i][0] = -1. * planeNormal[i][0];
      }
    }
    */
    mat_free(u1);
    mat_free(u2);
    normalizeVector(planeNormal); 
    return planeNormal;
  } else {
    return NULL;
  }
}
/*-------------------------------------------------------------------*/
MATRIX calcPlaneNormalQ(MATRIX UB, tasReflection r1, tasReflection r2)
{
  MATRIX q1 = NULL, q2 = NULL, q3 = NULL, planeNormal = NULL;
  int i;

  q1 = makeVector();
  q2 = makeVector();
  q1[0][0] = r1.qe.qh;
  q1[1][0] = r1.qe.qk;
  q1[2][0] = r1.qe.ql;
  q2[0][0] = r2.qe.qh;
  q2[1][0] = r2.qe.qk;
  q2[2][0] = r2.qe.ql;

  q3 = vectorCrossProduct(q1,q2);
  planeNormal = mat_mul(UB,q3);
  normalizeVector(planeNormal); 
  if (planeNormal[2][0] < .0) {
    for (i = 0; i < 3; i++) {
      planeNormal[i][0] = -1. * planeNormal[i][0];
    }
  }
  mat_free(q1);
  mat_free(q2);
  mat_free(q3);

  planeNormal[0][0] *= -1.;
  planeNormal[1][0] *= -1.;

  return planeNormal;
}
/*--------------------------------------------------------------------
  This is shot. The resulting UB is invalid. This needs more throught. 

*/
MATRIX calcUBFromAngles(MATRIX B, double om, double sgu, double sgl)
{

  MATRIX M, N, OM, UB;
  int status;

  M = mat_creat(3, 3, ZERO_MATRIX);
  N = mat_creat(3, 3, ZERO_MATRIX);
  OM = mat_creat(3, 3, ZERO_MATRIX);
 
  if(M == NULL || N == NULL || OM == NULL){
    mat_free(B);
    return NULL;
  }

  N[0][0] = 1.;
  N[1][1] = Cosd(sgu);
  N[1][2] = -Sind(sgu);
  N[2][1] = Sind(sgu);
  N[2][2] = Cosd(sgu);

  M[0][0] = Cosd(sgl);
  M[0][2] = Sind(sgl);
  M[1][1] = 1.;
  M[2][0] = -Sind(sgl);
  M[2][2] = Cosd(sgl);

  OM[0][0] = Cosd(om);
  OM[0][1] = -Sind(om);
  OM[1][0] = Sind(om);
  OM[1][1] = Cosd(om);
  OM[2][2] = 1.;

  /*
    Multiply....
  */
  UB = mat_mul(OM,M);
  UB = mat_mul(UB,N);
  UB = mat_mul(UB,B);

  mat_free(OM);
  mat_free(N);
  mat_free(M);

  return UB;
}
/*--------------------------------------------------------------------*/
MATRIX calcTestUB(lattice cell, double om, double sgu, double sgl)
{
  MATRIX B, UB;
  int status;

  /*
   * create matrices
   */
  B = mat_creat(3, 3, ZERO_MATRIX);

  status = calculateBMatrix(cell, B);
  if (status < 0) {
    return NULL;
  }
  UB = calcUBFromAngles(B,om,sgu,sgl);

  mat_free(B);
  return UB;
}
/*--------------------------------------------------------------------*/
MATRIX calcTestNormal(double sgu, double sgl)
{
  MATRIX M, N, Z, NORM;
  int status;


  M = mat_creat(3, 3, ZERO_MATRIX);
  N = mat_creat(3, 3, ZERO_MATRIX);
  Z = mat_creat(3, 1, ZERO_MATRIX);
 

  if(M == NULL || N == NULL || Z == NULL){
     return NULL;
  }

  N[0][0] = 1.;
  N[1][1] = Cosd(sgu);
  N[1][2] = -Sind(sgu);
  N[2][1] = Sind(sgu);
  N[2][2] = Cosd(sgu);

  M[0][0] = Cosd(sgl);
  M[0][2] = Sind(sgl);
  M[1][1] = 1.;
  M[2][0] = -Sind(sgl);
  M[2][2] = Cosd(sgl);

  Z[2][0] = 1.;

  NORM = mat_inv(N);
  NORM = mat_mul(NORM,mat_inv(M));
  NORM = mat_mul(NORM,Z);

  mat_free(N);
  mat_free(M);
  mat_free(Z);

  return NORM;
}

/*--------------------------------------------------------------------*/
MATRIX calcTasUBFromTwoReflections(lattice cell, tasReflection r1,
                                   tasReflection r2, int *errorCode)
{
  MATRIX B, HT, UT, U, UB, HTT;
  MATRIX u1, u2, h1, h2, planeNormal;
  double ud[3];
  int status;

  *errorCode = 1;

  /*
     calculate the B matrix and the HT matrix
   */
  B = mat_creat(3, 3, ZERO_MATRIX);

  status = calculateBMatrix(cell, B);
  if (status < 0) {
    *errorCode = status;
    return NULL;
  }
  /*
  printf("B-matrix\n");
  mat_dump(B);
  printf("\n");
  */

  h1 = tasReflectionToHC(r1.qe, B);
  h2 = tasReflectionToHC(r2.qe, B);
  if (h1 == NULL || h2 == NULL) {
    *errorCode = UBNOMEMORY;
    mat_free(B);
    return NULL;
  }
  HT = matFromTwoVectors(h1, h2);
  if (HT == NULL) {
    *errorCode = UBNOMEMORY;
    mat_free(B);
    mat_free(h1);
    mat_free(h2);
    return NULL;
  }

  /*
     calculate U vectors and UT matrix
   */
  u1 = calcTasUVectorFromAngles(r1);
  u2 = calcTasUVectorFromAngles(r2);
  if (u1 == NULL || u2 == NULL) {
    *errorCode = UBNOMEMORY;
    mat_free(B);
    mat_free(h1);
    mat_free(h2);
    return NULL;
  }
  UT = matFromTwoVectors(u1, u2);
  if (UT == NULL) {
    *errorCode = UBNOMEMORY;
    mat_free(B);
    mat_free(h1);
    mat_free(h2);
    mat_free(u1);
    mat_free(u2);
    mat_free(HT);
    return NULL;
  }

  /*
     UT = U * HT
   */
  HTT = mat_tran(HT);
  if (HTT == NULL) {
    *errorCode = UBNOMEMORY;
    mat_free(B);
    mat_free(h1);
    mat_free(h2);
    mat_free(u1);
    mat_free(u2);
    mat_free(HT);
    return NULL;
  }
  U = mat_mul(UT, HTT);
  if (U == NULL) {
    *errorCode = UBNOMEMORY;
    mat_free(B);
    mat_free(h1);
    mat_free(h2);
    mat_free(u1);
    mat_free(u2);
    mat_free(HT);
    mat_free(HTT);
    return NULL;
  }
  UB = mat_mul(U, B);
  if (UB == NULL) {
    mat_free(B);
    mat_free(h1);
    mat_free(h2);
    mat_free(u1);
    mat_free(u2);
    mat_free(HT);
    mat_free(HTT);
    mat_free(U);
    *errorCode = UBNOMEMORY;
  }

  /*
     clean up
   */
  killVector(h1);
  killVector(h2);
  mat_free(HT);
  mat_free(HTT);

  killVector(u1);
  killVector(u2);
  mat_free(UT);

  mat_free(U);
  mat_free(B);

  return UB;
}

/*-----------------------------------------------------------------------------*/
static MATRIX buildTVMatrix(MATRIX U1V, MATRIX U2V)
{
  MATRIX T, T3V;
  int i;

  normalizeVector(U2V);
  T3V = vectorCrossProduct(U1V, U2V);
  normalizeVector(T3V);
  if (T3V == NULL) {
    return NULL;
  }
  T = mat_creat(3, 3, ZERO_MATRIX);
  if (T == NULL) {
    killVector(T3V);
    return NULL;
  }
  for (i = 0; i < 3; i++) {
    T[i][0] = U1V[i][0];
    T[i][1] = U2V[i][0];
    T[i][2] = T3V[i][0];
  }
  /*
  printf("\n");
  printf("U1V\n");
  mat_dump(U1V);
  printf("U2V\n");
  mat_dump(U2V);
  printf("U3V\n");
  mat_dump(T3V);
  printf("TV-matrix\n");
  mat_dump(T);
  printf("\n");
  */

  killVector(T3V);
  return T;
}

/*----------------------------------------------------------------------------*/
static MATRIX buildRMatrix(MATRIX UB, MATRIX planeNormal,
                           tasQEPosition qe, int *errorCode)
{
  MATRIX U1V, U2V, TV, TVINV, M;


  *errorCode = 1;
  U1V = tasReflectionToQC(qe, UB);
  if (U1V == NULL) {
    *errorCode = UBNOMEMORY;
    return NULL;
  }
  normalizeVector(U1V);
  /*
  printf("Q = %f, %f, %f outwards Uv = %f %f %f\n", qe.qh, qe.qk, qe.ql, 
	 U1V[0][0], U1V[1][0],U1V[2][0]); 
  */

  U2V = vectorCrossProduct(planeNormal, U1V); 
  if (U2V == NULL) {
    killVector(U1V);
    *errorCode = UBNOMEMORY;
    return NULL;
  }
  if (vectorLength(U2V) < .0001) {
    *errorCode = BADUBORQ;
    killVector(U1V);
    killVector(U2V);
    return NULL;
  }

  TV = buildTVMatrix(U1V, U2V);
  if (TV == NULL) {
    killVector(U1V);
    killVector(U2V);
    *errorCode = UBNOMEMORY;
    return NULL;
  }

  TVINV = mat_inv(TV);
  if (TVINV == NULL) {
    *errorCode = BADUBORQ;
  }

  /*
  printf("TV after inversion\n");
  mat_dump(TVINV);
  printf("\n");
  */

  killVector(U1V);
  killVector(U2V);
  mat_free(TV);
  return TVINV;
}
/*-------------------------------------------------------------------------------*/
int calcTasMisalignment(ptasMachine machine, tasQEPosition qe,  double *misalign)
{
	MATRIX R;
    int errorCode = 1;
    double om;

    R = buildRMatrix(machine->UB, machine->planeNormal, qe, &errorCode);
    if (R == NULL) {
      return errorCode;
    }
    /**
     * See below, notes on om
     */
    om = Atan2d(R[1][0], R[0][0]);
    *misalign = om;
    mat_free(R);
    return 1;
}

/*-------------------------------------------------------------------------------*/
int calcTasQAngles(MATRIX UB, MATRIX planeNormal, int ss, double a3offset,
		tasQEPosition qe, ptasAngles angles)
{
  MATRIX R, QC;
  double om, q, theta, cos2t, cossgl;
  int errorCode = 1;

  R = buildRMatrix(UB, planeNormal, qe, &errorCode);
  if (R == NULL) {
    return errorCode;
  }

  /*    mat_dump(R);  */

  cossgl = sqrt(R[0][0]*R[0][0]+R[1][0]*R[1][0]);
  angles->sgl = ss*Atan2d(-R[2][0],cossgl);
  if (ABS(angles->sgl - 90.) < .5) {
    mat_free(R);
    return BADUBORQ;
  }
  /*
     Now, this is slightly different then in the publication by M. Lumsden.
     The reason is that the atan2 helps to determine the sign of om
     whereas the sin, cos formula given by M. Lumsden yield ambiguous signs 
     especially for om.
     sgu = atan(R[2][1],R[2][2]) where:
     R[2][1] = cos(sgl)sin(sgu)
     R[2][2] = cos(sgu)cos(sgl)
     om = atan(R[1][0],R[0][0]) where:
     R[1][0] = sin(om)cos(sgl)
     R[0][0] = cos(om)cos(sgl)
     The definitions of the R components are taken from M. Lumsden
     R-matrix definition.
   */

  om = Atan2d(R[1][0]/cossgl, R[0][0]/cossgl);
  angles->sgu = Atan2d(R[2][1]/cossgl, R[2][2]/cossgl);

  QC = tasReflectionToQC(qe, UB);
  if (QC == NULL) {
    mat_free(R);
    return UBNOMEMORY;
  }
  
  /*
   printf("Q = %f, %f, %f calculated om = %f\n", qe.qh, qe.qk, qe.ql, om);  
  */

  q = vectorLength(QC);
  q = 2. * PI * vectorLength(QC);
  cos2t =
      (qe.ki * qe.ki + qe.kf * qe.kf -
       q * q) / (2. * ABS(qe.ki) * ABS(qe.kf));
  if (ABS(cos2t) > 1.) {
    mat_free(R);
    killVector(QC);
    return TRIANGLENOTCLOSED;
  }
  theta = calcTheta(qe.ki, qe.kf, Acosd(cos2t));
  angles->sample_two_theta = ss * Acosd(cos2t);


  angles->a3 = om + ss*theta + a3offset;
  /*
     put a3 into -180, 180 properly. We can always turn by 180 because the
     scattering geometry is symmetric in this respect. It is like looking at
     the scattering plane from the other side
   */
  /*
  angles->a3 -= 180.;
  if (angles->a3 < -180 - a3offset) {
    angles->a3 += 360.;
  }
  */
  angles->a3 = fmod(angles->a3 + ss*180.,360.) - ss*180.; 

  killVector(QC);
  mat_free(R);

  return 1;
}

/*------------------------------------------------------------------------*/
int calcTasQH(MATRIX UB, tasAngles angles, ptasQEPosition qe)
{
  MATRIX UBINV = NULL, QV = NULL, Q = NULL;
  double q;
  tasReflection r;
  int i;

  UBINV = mat_inv(UB);
  r.angles = angles;
  r.qe = *qe;
  QV = calcTasUVectorFromAngles(r);
  if (UBINV == NULL || QV == NULL) {
    return UBNOMEMORY;
  }
  /*
     normalize the QV vector to be the length of the Q vector
     Thereby take into account the physicists magic fudge
     2PI factor
   */
  q = sqrt(qe->ki * qe->ki + qe->kf * qe->kf -
           2. * qe->ki * qe->kf * Cosd(angles.sample_two_theta));
  qe->qm = q;
  q /= 2. * PI;

  for (i = 0; i < 3; i++) {
    QV[i][0] *= q;
  }

  Q = mat_mul(UBINV, QV);
  if (Q == NULL) {
    mat_free(UBINV);
    killVector(QV);
    return UBNOMEMORY;
  }
  qe->qh = Q[0][0];
  qe->qk = Q[1][0];
  qe->ql = Q[2][0];

  killVector(QV);
  killVector(Q);
  mat_free(UBINV);

  return 1;
}

/*---------------------------------------------------------------------*/
int calcAllTasAngles(ptasMachine machine, tasQEPosition qe,
                     ptasAngles angles)
{
  int status;
  tasReflection r;

  status = maCalcTwoTheta(machine->monochromator, qe.ki,
                          &angles->monochromator_two_theta);
  if (status != 1) {
    return status;
  }

  status =
      maCalcTwoTheta(machine->analyzer, qe.kf,
                     &angles->analyzer_two_theta);
  if (status != 1) {
    return status;
  }

  status = calcTasQAngles(machine->UB, machine->planeNormal,
                          machine->ss_sample,
                          machine->a3offset, qe, angles);
  if (status != 1) {
    return status;
  }

  return 1;
}

/*----------------------------------------------------------------------*/
int calcTasQEPosition(ptasMachine machine, tasAngles angles,
                      ptasQEPosition qe)
{
  int status;

  qe->ki = maCalcK(machine->monochromator, angles.monochromator_two_theta);
  qe->kf = maCalcK(machine->analyzer, angles.analyzer_two_theta);

  status = calcTasQH(machine->UB, angles, qe);
  if (status != 1) {
    return status;
  }
  return 1;
}

/*================== POWDER Implementation ===========================*/
int calcTasPowderAngles(ptasMachine machine, tasQEPosition qe,
                        ptasAngles angles)
{
  double cos2t;
  int status;
  tasReflection r;

  status = maCalcTwoTheta(machine->monochromator, qe.ki,
                          &angles->monochromator_two_theta);
  if (status != 1) {
    return status;
  }

  cos2t =
      (qe.ki * qe.ki + qe.kf * qe.kf -
       qe.qm * qe.qm) / (2. * ABS(qe.ki) * ABS(qe.kf));
  if (cos2t > 1.) {
    return TRIANGLENOTCLOSED;
  }
  angles->sample_two_theta = machine->ss_sample * Acosd(cos2t);


  status =
      maCalcTwoTheta(machine->analyzer, qe.kf,
                     &angles->analyzer_two_theta);
  if (status != 1) {
    return status;
  }

  return 1;
}

/*---------------------------------------------------------------------*/
int calcTasPowderPosition(ptasMachine machine, tasAngles angles,
                          ptasQEPosition qe)
{

  int status;

  qe->ki = maCalcK(machine->monochromator, angles.monochromator_two_theta);
  qe->kf = maCalcK(machine->analyzer, angles.analyzer_two_theta);

  qe->qm = sqrt(qe->ki * qe->ki + qe->kf * qe->kf -
                2. * qe->ki * qe->kf * Cosd(angles.sample_two_theta));
  return 1;
}

/*====================== Logic implementation =========================*/
void setTasPar(ptasQEPosition qe, int tasMode, int tasVar, double value)
{
  double et;

  assert(tasMode == KICONST || tasMode == KFCONST || tasMode == ELASTIC);

  switch (tasVar) {
  case KF:
    if (tasMode == ELASTIC) {
      qe->kf = qe->ki;
    } else {
      qe->kf = value;
    }
    break;
  case EF:
    if (tasMode == ELASTIC) {
      qe->kf = qe->ki;
    } else {
      qe->kf = energyToK(value);
    }
    break;
  case KI:
    qe->ki = value;
    if (tasMode == ELASTIC) {
      qe->kf = value;
    }
    break;
  case EI:
    qe->ki = energyToK(value);
    if (tasMode == ELASTIC) {
      qe->kf = qe->ki;
    }
    break;
  case QH:
    qe->qh = value;
    break;
  case QK:
    qe->qk = value;
    break;
  case QL:
    qe->ql = value;
    break;
  case EN:
    if (tasMode == KICONST) {
      et = KtoEnergy(qe->ki) - value;
      qe->kf = energyToK(et);
    } else if (tasMode == KFCONST) {
      et = KtoEnergy(qe->kf) + value;
      qe->ki = energyToK(et);
    } else if (tasMode == ELASTIC) {
      qe->kf = qe->ki;
    } else {
      assert(0);
    }
    break;
  case QM:
    qe->qm = value;
    break;
  default:
    assert(0);
    break;
  }
}

/*-------------------------------------------------------------------------*/
double getTasPar(tasQEPosition qe, int tasVar)
{
  switch (tasVar) {
  case EI:
    return KtoEnergy(qe.ki);
    break;
  case KI:
    return qe.ki;
    break;
  case EF:
    return KtoEnergy(qe.kf);
    break;
  case KF:
    return qe.kf;
    break;
  case QH:
    return qe.qh;
    break;
  case QK:
    return qe.qk;
    break;
  case QL:
    return qe.ql;
    break;
  case EN:
    return KtoEnergy(qe.ki) - KtoEnergy(qe.kf);
    break;
  case QM:
    return qe.qm;
    break;
  default:
    assert(0);
  }
}

/*-------------------------------------------------------------------------*/
int isInPlane(MATRIX planeNormal, tasQEPosition qe)
{
  MATRIX v = NULL;
  double val;

  v = makeVector();
  v[0][0] = qe.qh;
  v[1][0] = qe.qk;
  v[2][0] = qe.ql;
  val = vectorDotProduct(planeNormal, v);
  mat_free(v);
  if (ABS(val) > INPLANEPREC) {
    return 0;
  } else {
    return 1;
  }
}

/*--------------------------------------------------------------------*/
MATRIX calcScatteringPlaneNormal(tasQEPosition qe1, tasQEPosition qe2)
{
  MATRIX v1 = NULL, v2 = NULL, planeNormal;

  v1 = makeVector();
  v2 = makeVector();
  if (v1 != NULL && v2 != NULL) {
    v1[0][0] = qe1.qh;
    v1[1][0] = qe1.qk;
    v1[2][0] = qe1.ql;

    v2[0][0] = qe2.qh;
    v2[1][0] = qe2.qk;
    v2[2][0] = qe2.ql;

    planeNormal = vectorCrossProduct(v1, v2);
    normalizeVector(planeNormal);
    mat_free(v1);
    mat_free(v2);
    return planeNormal;
  } else {
    return NULL;
  }
}