Goniometer.cpp

Go to the documentation of this file.

// Mantid Repository : https://github.com/mantidproject/mantid
//
// Copyright © 2018 ISIS Rutherford Appleton Laboratory UKRI,
//   NScD Oak Ridge National Laboratory, European Spallation Source,
//   Institut Laue - Langevin & CSNS, Institute of High Energy Physics, CAS
// SPDX - License - Identifier: GPL - 3.0 +
#include "MantidGeometry/Instrument/Goniometer.h"
#include "MantidKernel/ConfigService.h"
#include "MantidKernel/Logger.h"
#include "MantidKernel/Quat.h"
#include "MantidKernel/Strings.h"
#include <boost/algorithm/string.hpp>
#include <cstdlib>
#include <sstream>
#include <stdexcept>
#include <string>
#include <utility>
 
#include <vector>
 
using namespace Mantid::Kernel;
using Mantid::Kernel::Strings::toString;
 
namespace Mantid::Geometry {
using Kernel::DblMatrix;
using Kernel::Quat;
using Kernel::V3D;
 
Mantid::Kernel::Logger g_log("Goniometer");
 
void GoniometerAxis::saveNexus(::NeXus::File *file, const std::string &group) const {
  file->makeGroup(group, "NXmotor", true);
  file->writeData("name", name);
  file->writeData("angle", angle);
  file->openData("angle");
  std::string unit = (angleunit == angDegrees) ? "deg" : "rad";
  file->putAttr("unit", unit);
  std::string sen = (sense == CW) ? "CW" : "CCW";
  file->putAttr("sense", sen);
  file->closeData();
  rotationaxis.saveNexus(file, "rotationaxis");
  file->closeGroup();
}
 
void GoniometerAxis::loadNexus(::NeXus::File *file, const std::string &group) {
  file->openGroup(group, "NXmotor");
  file->readData("name", name);
  file->readData("angle", angle);
  file->openData("angle");
  std::string s;
  file->getAttr("sense", s);
  sense = (s == "CW") ? CW : CCW;
  file->getAttr("unit", s);
  angleunit = (s == "rad") ? angRadians : angDegrees;
  file->closeData();
  rotationaxis.loadNexus(file, "rotationaxis");
  file->closeGroup();
}
 
Goniometer::Goniometer() : R(3, 3, true), initFromR(false) {}
 
Goniometer::Goniometer(const DblMatrix &rot) {
  DblMatrix ide(3, 3), rtr(3, 3);
  rtr = rot.Tprime() * rot;
  ide.identityMatrix();
  if (rtr == ide) {
    R = rot;
    initFromR = true;
  } else
    throw std::invalid_argument("rot is not a rotation matrix");
}
 
const Kernel::DblMatrix &Goniometer::getR() const { return R; }
 
void Goniometer::setR(Kernel::DblMatrix rot) {
  R = std::move(rot);
  initFromR = true;
}
 
bool Goniometer::isDefined() const { return initFromR || (!motors.empty()); }
 
bool Goniometer::operator==(const Goniometer &other) const { return this->R == other.R; }
 
bool Goniometer::operator!=(const Goniometer &other) const { return this->R != other.R; }
 
std::string Goniometer::axesInfo() {
  if (initFromR) {
    return std::string("Goniometer was initialized from a rotation matrix. No "
                       "information about axis is available.\n");
  } else {
    std::stringstream info;
    std::vector<GoniometerAxis>::iterator it;
    std::string strCW("CW"), strCCW("CCW"), sense;
 
    if (motors.empty()) {
      info << "No axis is found\n";
    } else {
      info << "Name \t Direction \t Sense \t Angle \n";
      for (it = motors.begin(); it < motors.end(); ++it) {
        sense = ((*it).sense == CCW) ? strCCW : strCW;
        double angle = ((*it).angleunit == angDegrees) ? ((*it).angle) : ((*it).angle * rad2deg);
        info << (*it).name << "\t" << (*it).rotationaxis << "\t" << sense << "\t" << angle << '\n';
      }
    }
    return info.str();
  }
}
 
void Goniometer::pushAxis(const std::string &name, double axisx, double axisy, double axisz, double angle, int sense,
                          int angUnit) {
  if (initFromR) {
    throw std::runtime_error("Initialized from a rotation matrix, so no axes can be pushed.");
  } else {
    if (!std::isfinite(axisx) || !std::isfinite(axisy) || !std::isfinite(axisz) || !std::isfinite(angle)) {
      g_log.warning() << "NaN encountered while trying to push axis to "
                         "goniometer, Operation aborted"
                      << "\naxis name" << name << "\naxisx" << axisx << "\naxisy" << axisx << "\naxisz" << axisz
                      << "\nangle" << angle;
      return;
    }
    std::vector<GoniometerAxis>::iterator it;
    // check if such axis is already defined
    for (it = motors.begin(); it < motors.end(); ++it) {
      if (name == it->name)
        throw std::invalid_argument("Motor name already defined");
    }
    GoniometerAxis a(name, V3D(axisx, axisy, axisz), angle, sense, angUnit);
    motors.emplace_back(a);
  }
  recalculateR();
}
 
void Goniometer::setRotationAngle(const std::string &name, double value) {
  bool changed = false;
  std::vector<GoniometerAxis>::iterator it;
  for (it = motors.begin(); it < motors.end(); ++it) {
    if (name == it->name) {
      it->angle = value;
      changed = true;
    }
  }
  if (!changed) {
    throw std::invalid_argument("Motor name " + name + " not found");
  }
  recalculateR();
}
 
void Goniometer::setRotationAngle(size_t axisnumber, double value) {
  if (axisnumber >= motors.size())
    throw std::out_of_range("Goniometer::setRotationAngle(): axis number specified is too large.");
  (motors.at(axisnumber)).angle = value; // it will throw out of range exception
                                         // if axisnumber is not in range
  recalculateR();
}
 
void Goniometer::calcFromQSampleAndWavelength(const Mantid::Kernel::V3D &position, double wavelength, bool flip_x,
                                              bool inner) {
  V3D Q(position);
  if (Kernel::ConfigService::Instance().getString("Q.convention") == "Crystallography")
    Q *= -1;
 
  Matrix<double> starting_goniometer = getR();
 
  if (!inner)
    Q = starting_goniometer * Q;
 
  double wv = 2.0 * M_PI / wavelength;
  double norm_q2 = Q.norm2();
  double theta = acos(1 - norm_q2 / (2 * wv * wv)); // [0, pi]
  double phi = asin(-Q[1] / (wv * sin(theta)));     // [-pi/2, pi/2]
  V3D Q_lab(-wv * sin(theta) * cos(phi), -wv * sin(theta) * sin(phi), wv * (1 - cos(theta)));
 
  if (flip_x)
    Q_lab[0] *= -1;
 
  if (inner) {
    starting_goniometer.Invert();
    Q_lab = starting_goniometer * Q_lab;
  }
 
  // Solve to find rotation matrix, assuming only rotation around y-axis
  // A * X = B
  Matrix<double> A({Q[0], Q[2], Q[2], -Q[0]}, 2, 2);
  A.Invert();
  std::vector<double> B{Q_lab[0], Q_lab[2]};
  std::vector<double> X = A * B;
  double rot = atan2(X[1], X[0]);
 
  Matrix<double> goniometer(3, 3, true);
  goniometer[0][0] = cos(rot);
  goniometer[0][2] = sin(rot);
  goniometer[2][0] = -sin(rot);
  goniometer[2][2] = cos(rot);
  if (inner) {
    starting_goniometer.Invert();
    setR(starting_goniometer * goniometer);
  } else {
    setR(goniometer * starting_goniometer);
  }
}
 
const GoniometerAxis &Goniometer::getAxis(size_t axisnumber) const {
  return motors.at(axisnumber); // it will throw out of range exception if
                                // axisnumber is not in range
}
 
const GoniometerAxis &Goniometer::getAxis(const std::string &axisname) const {
  for (auto it = motors.begin(); it < motors.end(); ++it) {
    if (axisname == it->name) {
      return (*it);
    }
  }
  throw std::invalid_argument("Motor name " + axisname + " not found");
}
 
size_t Goniometer::getNumberAxes() const { return motors.size(); }
 
void Goniometer::makeUniversalGoniometer() {
  motors.clear();
  this->pushAxis("omega", 0., 1., 0., 0., Mantid::Geometry::CCW, Mantid::Geometry::angDegrees);
  this->pushAxis("chi", 0., 0., 1., 0., Mantid::Geometry::CCW, Mantid::Geometry::angDegrees);
  this->pushAxis("phi", 0., 1., 0., 0., Mantid::Geometry::CCW, Mantid::Geometry::angDegrees);
}
 
std::vector<double> Goniometer::getEulerAngles(const std::string &convention) {
  return Quat(getR()).getEulerAngles(convention);
}
 
void Goniometer::recalculateR() {
  if (initFromR) {
    g_log.warning() << "Goniometer was initialized from a rotation matrix. No "
                    << "recalculation from motors will be done.\n";
    return;
  }
  std::vector<GoniometerAxis>::iterator it;
  std::vector<double> elements;
  Quat QGlobal, QCurrent;
 
  for (it = motors.begin(); it < motors.end(); ++it) {
    double ang = (*it).angle * (*it).sense;
    if ((*it).angleunit == angRadians)
      ang *= rad2deg;
    QCurrent = Quat(ang, (*it).rotationaxis);
    QGlobal *= QCurrent;
  }
  elements = QGlobal.getRotation();
  R = DblMatrix(elements);
}
 
//--------------------------------------------------------------------------------------------
void Goniometer::saveNexus(::NeXus::File *file, const std::string &group) const {
  file->makeGroup(group, "NXpositioner", true);
  file->putAttr("version", 1);
  // Because the order of the axes is very important, they have to be written
  // and read out in the same order
  file->writeData("num_axes", int(motors.size()));
  for (size_t i = 0; i < motors.size(); i++)
    motors[i].saveNexus(file, "axis" + Strings::toString(i));
  file->closeGroup();
}
 
//--------------------------------------------------------------------------------------------
void Goniometer::loadNexus(::NeXus::File *file, const std::string &group) {
  file->openGroup(group, "NXpositioner");
  int num_axes;
  file->readData("num_axes", num_axes);
  motors.clear();
  motors.reserve(num_axes);
  for (int i = 0; i < num_axes; i++) {
    GoniometerAxis newAxis;
    newAxis.loadNexus(file, "axis" + Strings::toString(i));
    motors.emplace_back(newAxis);
  }
  file->closeGroup();
  // Refresh cached values
  recalculateR();
}
 
} // namespace Mantid::Geometry