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() : m_R(3, 3, true), m_initFromR(false) {}
 
Goniometer::Goniometer(const DblMatrix &rot) {
  bool isRot = rot.isRotation(); // this can also throw if matrix is invalid rotation, but that is fine
  if (!isRot) {
    // constructor should fail if the matrix is invalid
    throw std::invalid_argument("rot has not been evaluated to be a valid rotation matrix");
  }
  m_R = rot;
  m_initFromR = true;
}
 
Goniometer::Goniometer(const Goniometer &other)
    : m_R(other.m_R), m_motors(other.m_motors), m_initFromR(other.m_initFromR) {}
 
Goniometer &Goniometer::operator=(const Goniometer &other) {
  if (this != &other) {
    m_R = other.m_R;
    m_motors = other.m_motors;
    m_initFromR = other.m_initFromR;
  }
  return *this;
}
 
// Move constructor
Goniometer::Goniometer(Goniometer &&other) noexcept
    : m_R(std::move(other.m_R)), m_motors(std::move(other.m_motors)), m_initFromR(other.m_initFromR) {}
 
// Move assignment operator
Goniometer &Goniometer::operator=(Goniometer &&other) noexcept {
  if (this != &other) {
    m_R = std::move(other.m_R);
    m_motors = std::move(other.m_motors);
    m_initFromR = other.m_initFromR;
  }
  return *this;
}
 
const Kernel::DblMatrix &Goniometer::getR() const { return m_R; }
 
void Goniometer::setR(Kernel::DblMatrix rot) {
  m_R = std::move(rot);
  m_initFromR = true;
}
 
bool Goniometer::isDefined() const { return m_initFromR || (!m_motors.empty()); }
 
bool Goniometer::operator==(const Goniometer &other) const { return this->m_R == other.m_R; }
 
bool Goniometer::operator!=(const Goniometer &other) const { return this->m_R != other.m_R; }
 
std::string Goniometer::axesInfo() {
  if (m_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;
 
    if (m_motors.empty()) {
      info << "No axis is found\n";
    } else {
      info << "Name \t Direction \t Sense \t Angle \n";
      std::string strCW("CW"), strCCW("CCW");
      for (it = m_motors.begin(); it < m_motors.end(); ++it) {
        std::string 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 (m_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;
    }
    // check if such axis is already defined
    const auto it =
        std::find_if(m_motors.cbegin(), m_motors.cend(), [&name](const auto &axis) { return axis.name == name; });
    if (it != m_motors.cend()) {
      throw std::invalid_argument("Motor name already defined");
    }
    GoniometerAxis a(name, V3D(axisx, axisy, axisz), angle, sense, angUnit);
    m_motors.emplace_back(a);
  }
  recalculateR();
}
 
void Goniometer::setRotationAngle(const std::string &name, double value) {
  bool changed = false;
  std::vector<GoniometerAxis>::iterator it;
  for (it = m_motors.begin(); it < m_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 >= m_motors.size())
    throw std::out_of_range("Goniometer::setRotationAngle(): axis number specified is too large.");
  (m_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 m_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 {
  const auto it =
      std::find_if(m_motors.cbegin(), m_motors.cend(), [&axisname](const auto &axis) { return axis.name == axisname; });
  if (it != m_motors.cend()) {
    return *it;
  }
  throw std::invalid_argument("Motor name " + axisname + " not found");
}
 
size_t Goniometer::getNumberAxes() const { return m_motors.size(); }
 
void Goniometer::makeUniversalGoniometer() {
  m_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);
}
 
std::string Goniometer::getConventionFromMotorAxes() const {
  if (m_motors.size() != 3) {
    g_log.warning() << "Goniometerdoes not have 3 axes. Cannot determine "
                       "convention.\n";
    return "";
  }
 
  std::string convention;
  for (const auto &motor : m_motors) {
    switch (std::abs(motor.rotationaxis.masterDir())) {
    case 1:
      convention += "X";
      break;
    case 2:
      convention += "Y";
      break;
    case 3:
      convention += "Z";
      break;
    default:
      g_log.warning() << "Goniometer has an axis with a non-standard "
                         "rotation axis.\n";
      return "";
    }
  }
  return convention;
}
 
void Goniometer::recalculateR() {
  if (m_initFromR) {
    g_log.information() << "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 = m_motors.begin(); it < m_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();
  m_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(m_motors.size()));
  for (size_t i = 0; i < m_motors.size(); i++)
    m_motors[i].saveNexus(file, "axis" + Strings::toString(i));
  DblMatrix rMatrix = getR();
  // Flatten DblMatrix for saving
  std::vector<double> matrixData = rMatrix.getVector();
  file->writeData("rotation_matrix", matrixData);
  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);
  m_motors.clear();
  m_motors.reserve(num_axes);
  // If a rotation matrix is available and no rotation axes are provided load should behave like initFromR
  if (file->hasData("rotation_matrix") && (num_axes < 1)) {
    std::vector<double> matrixData;
    file->readData("rotation_matrix", matrixData);
    DblMatrix rotMat(matrixData);
    setR(std::move(rotMat));
    m_initFromR = true; // set as initFromR to prevent overwrite on R recalc
  } else {
    for (int i = 0; i < num_axes; i++) {
      GoniometerAxis newAxis;
      newAxis.loadNexus(file, "axis" + Strings::toString(i));
      m_motors.emplace_back(newAxis);
    }
  }
  file->closeGroup();
  // Refresh cached values
  recalculateR();
}
 
} // namespace Mantid::Geometry