LinearAlgebra.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
#include "LinearAlgebra.h"
 
// std library
#include <cmath>
 
// Qt Library
#include <QDebug>
#include <QtDebug>
#include <QtGlobal>
 
// boost library
#include <boost/assign/std/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
 
// Armadillo
#include <armadillo>
 
// other Isis library
#include "Angle.h"
#include "Constants.h"
#include "IException.h"
#include "IString.h"
 
 
namespace Isis {
  LinearAlgebra::LinearAlgebra() {
  }
 
 
  LinearAlgebra::~LinearAlgebra() {
  }
 
 
  bool LinearAlgebra::isIdentity(const Matrix &matrix) {
    if (matrix.size1() != matrix.size2()) return false;
 
    for (unsigned int row = 0; row < matrix.size1(); row++) {
      for (unsigned int col = 0; col < matrix.size2(); col++) {
        if (row == col) {
          if (!qFuzzyCompare(matrix(row, col), 1.0)) return false;
        }
        else {
          // When using qFuzzy compare against a number that is zero
          // you must offset the number per Qt documentation
          if (!qFuzzyCompare(1.0 + matrix(row, col), 1.0)) return false;
        }
      }
    }
    return true;
  }
 
 
  bool LinearAlgebra::isOrthogonal(const Matrix &matrix) {
    if (matrix.size1() != matrix.size2()) return false;
    LinearAlgebra::Matrix test = multiply(matrix, LinearAlgebra::transpose(matrix));
    if (isIdentity(test)) {
      // no need to test that transpose is both left and right inverse since the matrix is square
      return true;
    }
    return false;
  }
 
 
  // derived from naif's isrot routine
  bool LinearAlgebra::isRotationMatrix(const Matrix &matrix) {
    // rotation matrices must be square
    if (matrix.size1() != matrix.size2()) return false;
 
    /* An exerpt from NAIF's isrot() routine...
     *
     * A property of rotation matrices is that their columns form a
     * right-handed, orthonormal basis in 3-dimensional space.  The
     * converse is true:  all 3x3 matrices with this property are
     * rotation matrices.
     *
     * An ordered set of three vectors V1, V2, V3 forms a right-handed,
     * orthonormal basis if and only if
     *
     *    1)   || V1 ||  =  || V2 ||  =  || V3 ||  =  1
     *
     *    2)   V3 = V1 x V2.  Since V1, V2, and V3 are unit vectors,
     *         we also have
     *
     *         < V3, V1 x V2 > = 1.
     *
     *         This quantity is the determinant of the matrix whose
     *         colums are V1, V2 and V3.
     *
     * When finite precision numbers are used, rotation matrices will
     * usually fail to satisfy these criteria exactly.  We must use
     * criteria that indicate approximate conformance to the criteria
     * listed above.  We choose
     *
     *    1)   |   || Vi ||  -  1   |   <   NTOL,  i = 1, 2, 3.
     *                                  -
     *
     *    2)   Let
     *
     *                   Vi
     *            Ui = ------ ,   i = 1, 2, 3.
     *                 ||Vi||
     *
     *         Then we require
     *
     *            | < U3, U1 x U2 > - 1 |  <  DTOL;
     *                                     -
     *
     *         equivalently, letting U be the matrix whose columns
     *         are U1, U2, and U3, we insist on
     *
     *            | det(U) - 1 |  <  DTOL.
     *                            _
     * The columns of M must resemble unit vectors.  If the norms are
     * outside of the allowed range, M is not a rotation matrix.
     *
     * Also, the columns of M are required to be pretty nearly orthogonal.  The
     * discrepancy is gauged by taking the determinant of the matrix UNIT,
     * computed below, whose columns are the unitized columns of M.
     */
 
    // create a matrix whose columns are unit vectors of the columns of the
    // given matrix
    Matrix unitMatrix(matrix.size1(), matrix.size2());
 
    for (unsigned int i = 0; i < unitMatrix.size1(); i++) {
      // get the ith column vector from the given matrix
      Vector columnVector = column(matrix, i);
      // get the magnitude and if it is not near 1, this is not a rotation matrix
      double columnMagnitude = magnitude(columnVector);
      if ( columnMagnitude < 0.9 || columnMagnitude >  1.1 ) {
        return false;
      }
 
      // put the unitized row into the unitized matrix
      setColumn(unitMatrix, columnVector / columnMagnitude, i);
 
    }
 
    try {
      // get the determinant of the unitized matrix and if it is not near 1,
      // this is not a rotation matrix
      double det = determinant(unitMatrix);
      if ( det >= 0.9 && det <=  1.1 ) {
        return true;
      }
    }
    catch (IException &e) {
      // since we can only calculate the determinant for 2x2 or 3x3
      QString msg = "Unable to determine whether the given matrix is a rotation matrix.";
      throw IException(e, IException::Programmer, msg, _FILEINFO_);
    }
    return false;
  }
 
 
  bool LinearAlgebra::isZero(const Matrix &matrix) {
    for (unsigned int i = 0; i < matrix.size1(); i++) {
      for (unsigned int j = 0; j < matrix.size2(); j++) {
        // When using qFuzzy compare against a number that is zero
        // you must offset the number per Qt documentation
        if (!qFuzzyCompare(matrix(i, j) + 1.0, 1.0)) return false;
      }
    }
 
    return true;
  }
 
 
  bool LinearAlgebra::isZero(const Vector &vector) {
    for (unsigned int i = 0; i < vector.size(); i++) {
      // When using qFuzzy compare against a number that is zero
      // you must offset the number per Qt documentation
      if (!qFuzzyCompare(vector(i) + 1.0, 1.0)) return false;
    }
 
    return true;
  }
 
 
  bool LinearAlgebra::isEmpty(const LinearAlgebra::Vector &vector) {
    return vector.empty();
  }
 
 
  bool LinearAlgebra::isUnit(const Vector &vector) {
    if (qFuzzyCompare(dotProduct(vector, vector), 1.0)) {
      return true;
    }
    return false;
  }
 
 
  LinearAlgebra::Matrix LinearAlgebra::inverse(const Matrix &matrix) {
    try {
      if (isOrthogonal(matrix)) {
        return transpose(matrix);
      }
 
      // the determinant method will verify the size of the matrix is 2x2 or 3x3
      double det = determinant(matrix);
 
      if (qFuzzyCompare(det + 1.0, 1.0)) {
        // the inverse exists <==> the determinant is not 0.0
        QString msg = "The given matrix is not invertible. The determinant is 0.0.";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }
 
      // since the determinant is not zero, we can calculate the reciprocal
      double scale = 1 / det;
 
      LinearAlgebra::Matrix inverse = identity(matrix.size1());
      // find the inverse for 2x2
      if (matrix.size1() == 2) {
        inverse(0,  0) =  scale * matrix(1, 1);
        inverse(0,  1) = -scale * matrix(0, 1);
 
        inverse(1,  0) = -scale * matrix(1, 0);
        inverse(1,  1) =  scale * matrix(0, 0);
        return inverse;
      }
      // else we have a 3x3
      inverse(0,  0) = scale * ( matrix(1, 1) * matrix(2, 2) - matrix(2, 1) * matrix(1, 2) );
      inverse(0,  1) = scale * ( matrix(0, 2) * matrix(2, 1) - matrix(2, 2) * matrix(0, 1) );
      inverse(0,  2) = scale * ( matrix(0, 1) * matrix(1, 2) - matrix(1, 1) * matrix(0, 2) );
 
      inverse(1,  0) = scale * ( matrix(1, 2) * matrix(2, 0) - matrix(2, 2) * matrix(1, 0) );
      inverse(1,  1) = scale * ( matrix(0, 0) * matrix(2, 2) - matrix(2, 0) * matrix(0, 2) );
      inverse(1,  2) = scale * ( matrix(0, 2) * matrix(0, 1) - matrix(1, 2) * matrix(0, 0) );
 
      inverse(2,  0) = scale * ( matrix(1, 0) * matrix(2, 1) - matrix(2, 0) * matrix(1, 1) );
      inverse(2,  1) = scale * ( matrix(0, 1) * matrix(2, 0) - matrix(2, 1) * matrix(0, 0) );
      inverse(2,  2) = scale * ( matrix(0, 0) * matrix(1, 1) - matrix(1, 0) * matrix(0, 1) );
 
      return inverse;
    }
    catch (IException &e) {
      QString msg = "Unable to invert the given matrix.";
      throw IException(e, IException::Programmer, msg, _FILEINFO_);
    }
  }
 
 
  LinearAlgebra::Matrix LinearAlgebra::pseudoinverse(const Matrix &matrix) {
    // Copy values into Armadillo matrix
    arma::mat arMat(matrix.size1(), matrix.size2());
    for (size_t i = 0; i < matrix.size1(); i++) {
      for (size_t j = 0; j < matrix.size2(); j++) {
        arMat(i, j) = matrix(i, j);
      }
    }
 
    arma::mat invArMat = arma::pinv(arMat);
 
    // Copy values back to Boost matrix
    LinearAlgebra::Matrix inverse(invArMat.n_rows, invArMat.n_cols);
    for (size_t i = 0; i < invArMat.n_rows; i++) {
      for (size_t j = 0; j < invArMat.n_cols; j++) {
        inverse(i, j) = invArMat(i, j);
      }
    }
 
    return inverse;
  }
 
 
  LinearAlgebra::Matrix LinearAlgebra::transpose(const Matrix &matrix) {
    // no error testing required so call the boost transpose function
    return boost::numeric::ublas::trans(matrix);
  }
 
 
  LinearAlgebra::Matrix LinearAlgebra::identity(int size) {
    if (size < 1) {
      QString msg = "Can not create identity matrix of negative size ["
                    + toString((int) size) + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    LinearAlgebra::Matrix m(size, size, 0.0);
    for (int i = 0; i < size; i++) {
      m(i, i) = 1.0;
    }
 
    return m;
  }
 
 
  LinearAlgebra::Matrix LinearAlgebra::zeroMatrix(int rows, int columns) {
    boost::numeric::ublas::zero_matrix<double> m(rows, columns);
    return m;
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::zeroVector(int size) {
    boost::numeric::ublas::zero_vector<double> v(size);
    return v;
  }
 
 
  double LinearAlgebra::determinant(const Matrix &matrix) {
    if ( (matrix.size1() != matrix.size2()) || (matrix.size1() != 2 &&  matrix.size1() != 3) ) {
      QString msg = "Unable to calculate the determinant for the given matrix. "
                    "This method only calculates the determinant for 2x2 or 3x3 matrices."
                    "The given matrix is [" + toString((int) matrix.size1()) + "x"
                    + toString((int) matrix.size2()) + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    if (matrix.size1() == 2) {
      return matrix(0, 0) * matrix(1, 1) - matrix(1, 0) * matrix(0, 1);
    }
    else {
 
      return matrix(0, 0) * (matrix(1, 1)*matrix(2, 2) - matrix(1, 2)*matrix(2, 1))
           - matrix(0, 1) * (matrix(1, 0)*matrix(2, 2) - matrix(1, 2)*matrix(2, 0))
           + matrix(0, 2) * (matrix(1, 0)*matrix(2, 1) - matrix(1, 1)*matrix(2, 0));
    }
 
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::normalize(const Vector &vector) {
    // impossible to unitize the zero vector
    if (LinearAlgebra::isZero(vector)) {
      QString msg = "Unable to normalize the zero vector.";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }
    return  vector / LinearAlgebra::magnitude(vector);
  }
 
 
  double LinearAlgebra::magnitude(const Vector &vector) {
    double magnitude;
    // avoid dividing by maxNorm = 0
    // no stabilization is needed for the zero vector, magnitude is always 0.0
    if (LinearAlgebra::isZero(vector)) {
      magnitude = 0.0;
    }
    else {
      double maxNorm = LinearAlgebra::absoluteMaximum(vector);
      magnitude = maxNorm * boost::numeric::ublas::norm_2(vector / maxNorm);
    }
    return magnitude;
  }
 
 
  double LinearAlgebra::absoluteMaximum(const Vector &vector) {
    return boost::numeric::ublas::norm_inf(vector);
  }
 
 
  LinearAlgebra::Matrix LinearAlgebra::multiply(const Matrix &matrix1, const Matrix &matrix2) {
    // Check to make sure we can multiply
    if (matrix1.size2() != matrix2.size1()) {
      QString msg = "Unable to multiply matrices with mismatched dimensions. "
                    "The left matrix has [" + toString((int) matrix1.size2())
                    + "] columns and the right matrix has [" + toString((int) matrix2.size1())
                    + "] rows.";
 
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // Use boost product routine
    return boost::numeric::ublas::prod(matrix1, matrix2);
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::multiply(const Matrix &matrix, const Vector &vector) {
    // Check to make sure we can multiply
    if (matrix.size2() != vector.size()) {
      QString msg = "Unable to multiply matrix and vector with mismatched dimensions."
                    "The given vector has [" + toString((int) vector.size())
                    + "] components and the given matrix has ["
                    + toString((int) matrix.size2()) + "] columns.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // Use boost product routine
    return boost::numeric::ublas::prod(matrix, vector);
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::multiply(double scalar, const Vector &vector) {
    // no error checks necessary (use the boost operator* method)
    return scalar * vector;
  }
 
 
  LinearAlgebra::Matrix LinearAlgebra::multiply(double scalar, const Matrix &matrix) {
    // no error checks necessary
    return scalar * matrix;
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::add(const Vector &vector1, const Vector &vector2) {
    // Vectors best be the same size
    if (vector1.size() != vector2.size()) {
      QString msg = "Unable to add vectors with mismatched sizes."
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    return vector1 + vector2;
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::subtract(const Vector &vector1, const Vector &vector2) {
    // Vectors best be the same size
    if (vector1.size() != vector2.size()) {
      QString msg = "Unable to subtract vectors with mismatched sizes."
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    return vector1 - vector2;
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::crossProduct(const Vector &vector1, const Vector &vector2) {
    if ((vector1.size() != 3) || (vector2.size() != 3)) {
      QString msg = "Unable to calculate the cross product on vectors that are not size 3. "
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    Vector crossProd(3);
    crossProd(0) = vector1(1)*vector2(2) - vector1(2)*vector2(1);
    crossProd(1) = vector1(2)*vector2(0) - vector1(0)*vector2(2);
    crossProd(2) = vector1(0)*vector2(1) - vector1(1)*vector2(0);
 
    return crossProd;
  }
 
 
  // is this derived from naif's ucrss routine???
  LinearAlgebra::Vector LinearAlgebra::normalizedCrossProduct(const Vector &vector1,
                                                              const Vector &vector2) {
 
    double maxVector1 = LinearAlgebra::absoluteMaximum(vector1);
    double maxVector2 = LinearAlgebra::absoluteMaximum(vector2);
 
    LinearAlgebra::Vector normalizedVector1 = vector1;
    LinearAlgebra::Vector normalizedVector2 = vector2;
 
    if (maxVector1 != 0.0) {
      normalizedVector1 /= maxVector1;
    }
 
    if (maxVector2 != 0.0) {
      normalizedVector2 /= maxVector2;
    }
 
    LinearAlgebra::Vector vcross = crossProduct(normalizedVector1, normalizedVector2);
 
    return normalize(vcross);
  }
 
 
  LinearAlgebra::Matrix LinearAlgebra::outerProduct(const Vector &vector1, const Vector &vector2) {
    if (vector1.size() != vector2.size()) {
      QString msg = "Unable to compute the outer product for vectors with mismatched sizes."
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return boost::numeric::ublas::outer_prod(vector1, vector2);
  }
 
 
  double LinearAlgebra::dotProduct(const Vector &vector1, const Vector &vector2) {
    // no error check needed - this is done by innerProduct
    return LinearAlgebra::innerProduct(vector1, vector2);
  }
 
 
  double LinearAlgebra::innerProduct(const Vector &vector1, const Vector &vector2) {
    if (vector1.size() != vector2.size()) {
      QString msg = "Unable to compute the dot product for vectors with mismatched sizes."
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return boost::numeric::ublas::inner_prod(vector1, vector2);
  }
 
 
  // derived from naif's vproj routine
  LinearAlgebra::Vector LinearAlgebra::project(const Vector &vector1, const Vector &vector2) {
    if (vector1.size() != vector2.size()) {
      QString msg = "Unable to project vector1 onto vector2 with mismatched sizes."
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // if vector2 is the zero vector, then the projection of vector1 onto vector2
    // is also the zero vector
    if (LinearAlgebra::isZero(vector2)) return vector2;
 
    // find the dot product of the two vectors
    double v1Dotv2 = LinearAlgebra::dotProduct(vector1, vector2);
 
    // find the square of the length of vector2 (this is the dot product of the vector2 with itself)
    double v2Dotv2 = LinearAlgebra::dotProduct(vector2, vector2);
 
    return v1Dotv2/v2Dotv2 * vector2;
  }
 
 
  // derived from naif's vrotv routine
  LinearAlgebra::Vector LinearAlgebra::rotate(const Vector &vector, const Vector &axis,
                                              Angle angle) {
    if ((vector.size() != 3) || (axis.size() != 3)) {
      QString msg = "Unable to rotate vector about the given axis and angle. "
                    "Vectors must be of size 3 to perform rotation. "
                    "The given vector has [" + toString((int) vector.size())
                    + "] components and the given axis has ["
                    + toString((int) axis.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // if the given vector is the zero vector, then the rotation about the vector
    // is also the zero vector
    if (isZero(axis)) return vector;
 
    // Compute the unit vector that is codirectional with the given axis
    Vector axisUnitVector = normalize(axis);
 
    // Compute the projection of the given vector onto the axis unit vector.
    Vector projVectorOnAxis = project(vector,  axisUnitVector);
 
    // Compute the component of the input orthogonal to the AXIS.  Call it V1.
    Vector v1 = vector - projVectorOnAxis;
 
    // Rotate V1 by 90 degrees about the AXIS and call the result V2.
    Vector v2 = LinearAlgebra::crossProduct(axisUnitVector, v1);
 
    // Compute cos(angle)*v1 + sin(angle)*v2. This is v1 rotated about
    // the axis in the plane normal to the axis, call the result rplane
    double c = cos(angle.radians());
    double s = sin(angle.radians());
    Vector rplane = (c*v1) + (s*v2);
 
    // Add the rotated component in the normal plane to axis to the
    // projection of v onto axis to obtain rotation.
    return (rplane + projVectorOnAxis);
  }
 
 
  // Derived from NAIF's vperp routine
  // possible that we may need to restrict size to dimesion 3, but doesn't seem
  // necessary in the code.
  LinearAlgebra::Vector LinearAlgebra::perpendicular(const Vector &vector1, const Vector &vector2) {
 
    // if vector1 is the zero vector, just set p to zero and return.
    if ( isZero(vector1) ) {
       return vector1;
    }
 
    // if vector2 is the zero vector, just set p to vector1 and return.
    if ( isZero(vector2) ) {
       return vector1;
    }
 
    // normalize (using the max norm) the given vectors and project the first onto the second
    double max1 = absoluteMaximum(vector1);
    double max2 = absoluteMaximum(vector2);
    Vector parallelVector = project(vector1 / max1,  vector2 / max2);
 
    Vector perpendicularVector = vector1 - parallelVector * max1;
 
    return perpendicularVector;
 
  }
 
 
  // Derived from NAIF's raxisa routine
  LinearAlgebra::AxisAngle LinearAlgebra::toAxisAngle(const Matrix &rotationMatrix) {
 
    if ((rotationMatrix.size1() != 3) || (rotationMatrix.size2() != 3)) {
      QString msg = "Unable to convert the given matrix to an axis of rotation "
                    "and a rotation angle. A 3x3 matrix is required. The given matrix is ["
                    + toString((int) rotationMatrix.size1()) + "x"
                    + toString((int) rotationMatrix.size2()) + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // if given matrix is not a rotation matrix, we can't proceed.
    if ( !isRotationMatrix(rotationMatrix) ) {
      QString msg = "Unable to convert the given matrix to an axis of rotation "
                    "and a rotation angle. The given matrix is not a rotation matrix.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    Angle angle;
    Vector axis(3);
 
    // Construct the quaternion corresponding to the input rotation matrix
    Vector quaternion = LinearAlgebra::toQuaternion(rotationMatrix);
 
    LinearAlgebra::Vector subQuaternion(3);
    subQuaternion[0] = quaternion[1];
    subQuaternion[1] = quaternion[2];
    subQuaternion[2] = quaternion[3];
 
 
    // The quaternion we've just constructed is of the form:
    //
    //    cos(ANGLE/2) + sin(ANGLE/2) * AXIS
    //
    // We take a few precautions to handle the case of an identity
    // rotation.
    if (isZero(subQuaternion)) {
      axis(0) = 0.0;
      axis(1) = 0.0;
      axis(2) = 1.0;
 
      angle.setRadians(0.0);
    }
    else if (qFuzzyCompare(quaternion(0) + 1.0, 1.0)) {
      axis = subQuaternion;
      angle.setRadians(Isis::PI);
    }
    else {
      axis = LinearAlgebra::normalize(subQuaternion);
      angle.setRadians(2.0 * atan2(magnitude(subQuaternion), quaternion(0)));
    }
 
    return qMakePair(axis, angle);
  }
 
 
  // Derived from NAIF's m2eul routine
  QList<LinearAlgebra::EulerAngle> LinearAlgebra::toEulerAngles(const Matrix &rotationMatrix,
                                                                const QList<int> axes) {
 
    // check there are 3 axes in the set {1,2,3} with center axis not equal to first or last
    if (axes.size() != 3) {
      QString msg = "Unable to convert the given matrix to Euler angles. "
                    "Exactly 3 axis codes are required. The given list has ["
                    + toString((int) axes.size()) + "] axes.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    QSet<int> validAxes;
    validAxes << 1 << 2 << 3;
    if (!validAxes.contains(axes[0])
        || !validAxes.contains(axes[1])
        || !validAxes.contains(axes[2])) {
      QString msg = "Unable to convert the given matrix to Euler angles using the given axis codes "
                    "[" + toString(axes[0]) + ", " + toString(axes[1]) + ", " + toString(axes[2])
                    + "]. Axis codes must be 1, 2, or 3.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    if (axes[0] == axes[1] || axes[1] == axes[2]) {
      QString msg = "Unable to convert the given matrix to Euler angles using the given axis codes "
                    "[" + toString(axes[0]) + ", " + toString(axes[1]) + ", " + toString(axes[2])
                    + "]. The middle axis code must differ from its neighbors.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // check the matrix is 3x3 rotation
    if ((rotationMatrix.size1() != 3) || (rotationMatrix.size2() != 3)) {
      QString msg = "Unable to convert the given matrix to Euler angles. A 3x3 matrix is required. "
                    "The given matrix is ["
                    + toString((int) rotationMatrix.size1()) + "x"
                    + toString((int) rotationMatrix.size2()) + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    if ( !isRotationMatrix(rotationMatrix) ) {
      QString msg = "Unable to convert the given matrix to Euler angles. "
                    "The given matrix is not a rotation matrix.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // AXIS3, AXIS2, AXIS1 and R have passed their tests at this
    // point.  We take the liberty of working with TEMPROT, a
    // version of R that has unitized columns.
    //  DO I = 1, 3
    //     CALL VHAT ( R(1,I), TEMPROT(1,I) )
    //  END DO
 
    LinearAlgebra::Matrix tempRotation(3, 3);
    for (int i = 0; i < 3; i++) {
      Vector columnVector = column(rotationMatrix, i);
      // replace the ith column with a unitized column vector
      setColumn(tempRotation, normalize(columnVector), i);
    }
 
    QList<int> nextAxis;
    nextAxis << 2 << 3 << 1;
    double sign;
    // create a matrix of zeros and we will fill in the non zero components below
    LinearAlgebra::Matrix change = zeroMatrix(3, 3);
 
    Angle angle1, angle2, angle3;
    if ( axes[0] == axes[2] ) {
      if ( axes[1] == nextAxis[axes[0] - 1] ) {
         sign =  1.0;
      }
      else {
         sign = -1.0;
      }
      int c = 6 - axes[0] - axes[1];
 
      change( axes[0] - 1, 2 ) =         1.0;
      change( axes[1] - 1, 0 ) =         1.0;
      change( c       - 1, 1 ) =  sign * 1.0;
      LinearAlgebra::Matrix tempMatrix = multiply( tempRotation,  change );
      tempRotation = multiply( transpose(change),  tempMatrix );
 
      bool degen =    (    qFuzzyCompare(tempRotation(0, 2) + 1.0, 1.0)
                        && qFuzzyCompare(tempRotation(1, 2) + 1.0, 1.0) )
                   || (    qFuzzyCompare(tempRotation(2, 0) + 1.0, 1.0)
                        && qFuzzyCompare(tempRotation(2, 1) + 1.0, 1.0) )
                   || qFuzzyCompare( qAbs(tempRotation(2, 2)), 1.0 );
 
      if ( degen ) {
 
        angle3.setRadians( 0.0 );
        angle2.setRadians(  acos( tempRotation(2, 2) ) );
        angle1.setRadians( atan2( tempRotation(0, 1), tempRotation(0, 0) ) );
 
      }
      else {
      // the normal case.
        angle3.setRadians( atan2( tempRotation(0, 2),  tempRotation(1, 2) ) );
        angle2.setRadians(  acos( tempRotation(2, 2) ) );
        angle1.setRadians( atan2( tempRotation(2, 0), -tempRotation(2, 1) ) );
      }
    }
    else {
    //   The axis order is c-b-a.  We're going to find a matrix CHANGE
    //   such that
    //
    //            T
    //      CHANGE  R  CHANGE
      if ( axes[1] == nextAxis[axes[0] - 1] ) {
         sign =  1.0;
      }
      else {
         sign = -1.0;
      }
      change( axes[0] - 1, 0 ) =        1.0;
      change( axes[1] - 1, 1 ) =        1.0;
      change( axes[2] - 1, 2 ) = sign * 1.0;
      LinearAlgebra::Matrix tempMatrix = multiply( tempRotation,  change );
      tempRotation = multiply( transpose(change),  tempMatrix );
      bool degen =    (    qFuzzyCompare(tempRotation(0, 0) + 1.0, 1.0)
                        && qFuzzyCompare(tempRotation(0, 1) + 1.0, 1.0) )
                   || (    qFuzzyCompare(tempRotation(1, 2) + 1.0, 1.0)
                        && qFuzzyCompare(tempRotation(2, 2) + 1.0, 1.0) )
                   || qFuzzyCompare( qAbs(tempRotation(0, 2)), 1.0 );
 
      if ( degen ) {
 
        angle3.setRadians( 0.0 );
        angle2.setRadians(         asin( -tempRotation(0, 2) ) );
        angle1.setRadians( sign * atan2( -tempRotation(1, 0), tempRotation(1, 1) ) );
 
      }
      else {
      // the normal case.
        angle3.setRadians(        atan2(  tempRotation(1, 2), tempRotation(2, 2) ) );
        angle2.setRadians(         asin( -tempRotation(0, 2) ) );
        angle1.setRadians( sign * atan2(  tempRotation(0, 1), tempRotation(0, 0) ) );
      }
    }
 
    QList<LinearAlgebra::EulerAngle> eulerAngles;
    eulerAngles << qMakePair(angle3, axes[0])
                << qMakePair(angle2, axes[1])
                << qMakePair(angle1, axes[2]);
    return eulerAngles;
  }
 
 
  // Derived from NAIF's m2q routine
  LinearAlgebra::Vector LinearAlgebra::toQuaternion(const Matrix &rotationMatrix) {
 
    if ((rotationMatrix.size1() != 3) || (rotationMatrix.size2() != 3)) {
      QString msg = "Unable to convert the given matrix to a quaternion. "
                    "A 3x3 matrix is required. The given matrix is ["
                    + toString((int) rotationMatrix.size1()) + "x"
                    + toString((int) rotationMatrix.size2()) + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // if given matrix is not a rotation matrix, we can't proceed.
    if ( !isRotationMatrix(rotationMatrix) ) {
      QString msg = "Unable to convert the given matrix to an axis of rotation "
                    "and a rotation angle. The given matrix is not a rotation matrix.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // Let q = c + s1*i + s2*j + s3*k
    // where c is the constant and s1, s2, s3 are the coefficients of the imaginary parts
    // and |q| = 1, .
 
    // Denote the following for n,m in {1,2,3}
    // cc =  c  *  c
    // csn = c  *  sn
    // snm = sn *  sm
 
    // The rotation matrix corresponding to our quaternion is:
    //
    // |  cc+s11-s22-s33      2*s12-2*cs3       2*s13+2*cs2   |
    // |    2*s12+2*cs3     cc-s11+s22-s33      2*s23-2*cs1   |
    // |    2*s13-2*cs2       2*s23+2*cs1     cc-s11-s22+s33  |
    //
    //
    // since |q| = cc + s11 + s22 + s33 = 1, we can use substitution on the diagonal entries to get
    //
    // |  1-2*s22-2*s33      2*s12-2*cs3      2*s13+2*cs2   |
    // |    2*s12+2*cs3    1-2*s11-2*s33      2*s23-2*cs1   |
    // |    2*s13-2*cs2      2*s23+2*cs1    1-2*s11-2*s22   |
    //
    //
    //
    // r(1,1)      = 1.0 - 2*s22 - 2*s33
    // r(2,1)      =       2*s12 + 2*cs3
    // r(3,1)      =       2*s13 - 2*cs2
    //
    // r(1,2)      =       2*s12 - 2*cs3
    // r(2,2)      = 1.0 - 2*s11 - 2*s33
    // r(3,2)      =       2*s23 + 2*cs1
    //
    // r(1,3)      =       2*s13 + 2*cs2
    // r(2,3)      =       2*s23 - 2*cs1
    // r(3,3)      = 1.0 - 2*s11 - 2*s22
 
    // Using this matrix, we get the trace by summing the diagonal entries
    //    trace = (1-2*s22-2*s33) + (1-2*s11-2*s33) + (1-2*s11-2*s22)
    //          = 3 - 4*(s11 + s22 + s33)
 
    // Therefore
    //    1.0 + trace = 4 - 4*(s11 + s22 + s33)
    //                = 4*(1 - s11 - s22 - s33)       by factoring 4
    //                = 4*(|q| - s11 - s22 - s33)     since |q| = 1
    //                = 4*cc                          since |q| = cc + s11 + s22 + s33
    //
    // Solving for c, we get that
    // 4*cc = 1 + trace
    //  2*c = +/- sqrt(1 + trace)
    //    c = +/- 0.5 * sqrt( 1.0 + trace )
 
    // We also have the following where {n,m,p} = {1,2,3}
    //   1.0 + trace - 2.0*r(n,n) = 4.0 - 4.0(snn + smm + spp) - 2(1.0 - 2.0(smm + spp ))
    //                            = 4.0 - 4.0(snn + smm + spp) - 2.0 + 4.0(smm + spp )
    //                            = 2.0 - 4.0*snn
    //
    // Solving for snn, we get
    //     2.0 - 4.0*snn =  1.0 + trace - 2.0*r(n,n)
    //    -2.0 + 4.0*snn = -1.0 - trace + 2.0*r(n,n)
    //           4.0*snn =  1.0 - trace + 2.0*r(n,n) ******** we will use this value  *************
    //                                               ******** in our code to minimize *************
    //                                               ******** computations            *************
    //            2.0*sn =  +/- sqrt(1.0 - trace + 2.0*r(n,n))
    //                sn =  +/- 0.5 * sqrt(1.0 - trace + 2.0*r(n,n))    for any n in {1,2,3}
 
 
    // In addition to these observations, note that all of the product
    // pairs can easily be computed by simplifying the expressions
    // (r(n,m) - r(m,n)) / 4.0 and (r(n,m) + r(m,n)) / 4.0
    //
    // For example,
    //  (r(3,2) - r(2,3)) / 4.0 = ( (2*s23 + 2*cs1) - (2*s23 - 2*cs1) ) / 4.0
    //                          = ( 4*cs1 ) / 4.0
    //                          =     cs1
    //
    //  So we have the following
    //  cs1 = (r(3,2) - r(2,3))/4.0
    //  cs2 = (r(1,3) - r(3,1))/4.0
    //  cs3 = (r(2,1) - r(1,2))/4.0
    //  s12 = (r(2,1) + r(1,2))/4.0
    //  s13 = (r(3,1) + r(1,3))/4.0
    //  s23 = (r(2,3) + r(3,2))/4.0
 
    //but taking sums or differences of numbers that are nearly equal
    //or nearly opposite results in a loss of precision. as a result
    //we should take some care in which terms to select when computing
    //c, s1, s2, s3.  however, by simply starting with one of the
    //large quantities cc, s11, s22, or s33 we can make sure that we
    //use the best of the 6 quantities above when computing the
    //remaining components of the quaternion.
 
    double trace  = rotationMatrix(0, 0) + rotationMatrix(1, 1) + rotationMatrix(2, 2);
    double mtrace = 1.0 - trace;
 
    // cc4 = 4 * c * c where c is the constant term (first) of the quaternion
    double cc4    = 1.0 + trace;
    // snn4 = 4 * sn * sn where sn is the coefficient for the n+1 term of the quaternion vector
    double s114   = mtrace + 2.0*rotationMatrix(0, 0);
    double s224   = mtrace + 2.0*rotationMatrix(1, 1);
    double s334   = mtrace + 2.0*rotationMatrix(2, 2);
 
    // Note that cc4 + s114 + s224 + s334 = 4|q| = 4
    // Thus, at least one of the 4 terms is greater than 1.
    double normalizingFactor;
    LinearAlgebra::Vector quaternion(4);
    if ( cc4 >= 1.0 ) { // true if the trace is non-negative
 
      quaternion[0] = sqrt( cc4 * 0.25 ); // note that q0 = c = sqrt(4*c*c*0.25)
      normalizingFactor = 1.0 / ( quaternion[0] * 4.0 );
 
      // multiply the difference of the entries symmetric about the diagonal by the factor 1/4c
      // to get s1, s2, and s3
      // (see calculations for cs1, cs2, cs3 in comments above)
      quaternion[1] = ( rotationMatrix(2, 1) - rotationMatrix(1, 2) ) * normalizingFactor;
      quaternion[2] = ( rotationMatrix(0, 2) - rotationMatrix(2, 0) ) * normalizingFactor;
      quaternion[3] = ( rotationMatrix(1, 0) - rotationMatrix(0, 1) ) * normalizingFactor;
 
    }
    else if ( s114 >= 1.0 ) {
 
      quaternion[1] = sqrt( s114 * 0.25 ); // note that q1 = s1 = sqrt(4*s1*s1*0.25)
      normalizingFactor = 1.0 /( quaternion[1] * 4.0 );
 
      quaternion[0] = ( rotationMatrix(2, 1) - rotationMatrix(1, 2) ) * normalizingFactor;
      quaternion[2] = ( rotationMatrix(0, 1) + rotationMatrix(1, 0) ) * normalizingFactor;
      quaternion[3] = ( rotationMatrix(0, 2) + rotationMatrix(2, 0) ) * normalizingFactor;
 
    }
    else if ( s224 >= 1.0 ) {
 
      quaternion[2] = sqrt( s224 * 0.25 );
      normalizingFactor = 1.0 /( quaternion[2] * 4.0 );
 
      quaternion[0] = ( rotationMatrix(0, 2) - rotationMatrix(2, 0) ) * normalizingFactor;
      quaternion[1] = ( rotationMatrix(0, 1) + rotationMatrix(1, 0) ) * normalizingFactor;
      quaternion[3] = ( rotationMatrix(1, 2) + rotationMatrix(2, 1) ) * normalizingFactor;
    }
    else { // s334 >= 1.0
 
      quaternion[3]   = sqrt( s334 * 0.25 );
      normalizingFactor = 1.0 /( quaternion[3] * 4.0 );
 
      quaternion[0] = ( rotationMatrix(1, 0) - rotationMatrix(0, 1) ) * normalizingFactor;
      quaternion[1] = ( rotationMatrix(0, 2) + rotationMatrix(2, 0) ) * normalizingFactor;
      quaternion[2] = ( rotationMatrix(1, 2) + rotationMatrix(2, 1) ) * normalizingFactor;
 
    }
 
    // if the magnitude of this quaternion is not one, we polish it up a bit.
    if ( !isUnit(quaternion) ) {
       quaternion = normalize(quaternion);
    }
 
    // M2Q always returns a quaternion with scalar part greater than or equal to zero.
    if ( quaternion[0] < 0.0 ) {
       quaternion *= -1.0;
    }
 
    return quaternion;
  }
 
 
  // Derived from NAIF's axisar routine
  LinearAlgebra::Matrix LinearAlgebra::toMatrix(const Vector &axis, Angle angle) {
    if (axis.size() != 3) {
      QString msg = "Unable to convert the given vector and angle to a rotation matrix. "
                    "The given vector with size [" + toString((int) axis.size())
                    + "] is not a 3D axis vector.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // intialize the matrix with the 3x3 identity.
    LinearAlgebra::Matrix rotationMatrix = identity(3);
 
    // The matrix we want rotates every vector by angle about axis.
    // In particular, it does so to our basis vectors.  The columns
    // of the output matrix are the images of the basis vectors
    // under this rotation.
    for (unsigned int i = 0; i < axis.size(); i++) {
      // rotate the ith column of the matrix (the ith standard basis vector)
      // about the given axis using the given angle
      Vector stdBasisVector = column(rotationMatrix, i);
 
      // replace the ith column with the rotated vector
      setColumn(rotationMatrix, rotate(stdBasisVector, axis, angle), i);
    }
 
    return rotationMatrix;
  }
 
 
  LinearAlgebra::Matrix LinearAlgebra::toMatrix(const AxisAngle &axisAngle) {
    return LinearAlgebra::toMatrix(axisAngle.first, axisAngle.second);
  }
 
 
  // Derived from NAIF's eul2m routine
  LinearAlgebra::Matrix LinearAlgebra::toMatrix(const EulerAngle &angle3,
                                                const EulerAngle &angle2,
                                                const EulerAngle &angle1) {
    QSet<int> validAxes;
    validAxes << 1 << 2 << 3;
    if (!validAxes.contains(angle3.second)
        || !validAxes.contains(angle2.second)
        || !validAxes.contains(angle1.second)) {
      QString msg = "Unable to convert the given Euler angles to a matrix using the given axis "
                    "codes [" + toString(angle3.second) + ", " + toString(angle2.second) + ", "
                    + toString(angle1.second) + "]. Axis codes must be 1, 2, or 3.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    LinearAlgebra::Matrix m(3, 3);
    // implement eul2m
    // Calculate the 3x3 rotation matrix generated by a rotation
    // of a specified angle about a specified axis. This rotation
    // is thought of as rotating the coordinate system.
    //
    //ROTATE(angle1, axis1);
    double sinAngle = sin(angle1.first.radians());
    double cosAngle = cos(angle1.first.radians());
    // get the index offset based on the axis number corresponding to this angle
    double indexOffset = ((angle1.second % 3) + 3) % 3;
    QList<int> indexSet;
    indexSet << 2 << 0 << 1 << 2 << 0;
    int index1 = indexSet[indexOffset + 0];
    int index2 = indexSet[indexOffset + 1];
    int index3 = indexSet[indexOffset + 2];
    m(index1, index1) = 1.0;   m(index1, index2) =  0.0;        m(index1, index3) = 0.0;
    m(index2, index1) = 0.0;   m(index2, index2) =  cosAngle;   m(index2, index3) = sinAngle;
    m(index3, index1) = 0.0;   m(index3, index2) = -sinAngle;   m(index3, index3) = cosAngle;
 
    //
    // C     ROTMAT applies a rotation of ANGLE radians about axis IAXIS to a
    // C     matrix.  This rotation is thought of as rotating the coordinate
    // C     system.
    // C
    //CALL ROTMAT ( R,   ANGLE2,  AXIS2,  R1 )
    sinAngle = sin(angle2.first.radians());
    cosAngle = cos(angle2.first.radians());
    indexOffset = ((angle2.second % 3) + 3) % 3;
    index1 = indexSet[indexOffset + 0];
    index2 = indexSet[indexOffset + 1];
    index3 = indexSet[indexOffset + 2];
 
    LinearAlgebra::Matrix tempMatrix(3, 3);
    for (int i = 0; i < 3; i++) {
       tempMatrix(index1, i) =           m(index1, i);
       tempMatrix(index2, i) =  cosAngle*m(index2, i) + sinAngle*m(index3, i);
       tempMatrix(index3, i) = -sinAngle*m(index2, i) + cosAngle*m(index3, i);
    }
 
    //CALL ROTMAT ( tempMatrix,  ANGLE3,  AXIS3,  m  )
    sinAngle = sin(angle3.first.radians());
    cosAngle = cos(angle3.first.radians());
    indexOffset = ((angle3.second % 3) + 3) % 3;
    index1 = indexSet[indexOffset + 0];
    index2 = indexSet[indexOffset + 1];
    index3 = indexSet[indexOffset + 2];
    for (int i = 0; i < 3; i++) {
       m(index1, i) =           tempMatrix(index1, i);
       m(index2, i) =  cosAngle*tempMatrix(index2, i) + sinAngle*tempMatrix(index3, i);
       m(index3, i) = -sinAngle*tempMatrix(index2, i) + cosAngle*tempMatrix(index3, i);
    }
 
    return m;
  }
 
 
  LinearAlgebra::Matrix LinearAlgebra::toMatrix(const QList<EulerAngle> &eulerAngles) {
 
    if (eulerAngles.size() != 3) {
      QString msg = "Unable to convert the given Euler angles to a matrix. "
                    "Exactly 3 Euler angles are required. The given list has ["
                    + toString((int) eulerAngles.size()) + "] angles.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return LinearAlgebra::toMatrix(eulerAngles[0], eulerAngles[1], eulerAngles[2]);
  }
 
 
 
  // Derived from NAIF's q2m routine
  LinearAlgebra::Matrix LinearAlgebra::toMatrix(const Vector &quaternion) {
    if (quaternion.size() != 4) {
      QString msg = "Unable to convert the given vector to a rotation matrix. "
                    "The given vector with [" + toString((int) quaternion.size())
                    + "] components is not a quaternion.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    Vector q = quaternion;
    if ( !isUnit(quaternion) && !isZero(quaternion) ) {
      q = normalize(quaternion);
    }
 
    LinearAlgebra::Matrix matrix(3, 3);
    // For efficiency, we avoid duplicating calculations where possible.
    double q0q1 =  q[0] * q[1];
    double q0q2 =  q[0] * q[2];
    double q0q3 =  q[0] * q[3];
 
    double q1q1 =  q[1] * q[1];
    double q1q2 =  q[1] * q[2];
    double q1q3 =  q[1] * q[3];
 
    double q2q2 =  q[2] * q[2];
    double q2q3 =  q[2] * q[3];
 
    double q3q3 =  q[3] * q[3];
 
    matrix(0, 0) =  1.0  -  2.0 * ( q2q2 + q3q3 );
    matrix(0, 1) =          2.0 * ( q1q2 - q0q3 );
    matrix(0, 2) =          2.0 * ( q1q3 + q0q2 );
 
    matrix(1, 0) =          2.0 * ( q1q2 + q0q3 );
    matrix(1, 1) =  1.0  -  2.0 * ( q1q1 + q3q3 );
    matrix(1, 2) =          2.0 * ( q2q3 - q0q1 );
 
    matrix(2, 0) =          2.0 * ( q1q3 - q0q2 );
    matrix(2, 1) =          2.0 * ( q2q3 + q0q1 );
    matrix(2, 2) =  1.0  -  2.0 * ( q1q1 + q2q2 );
 
    return matrix;
  }
 
 
  void LinearAlgebra::setRow(Matrix &matrix, const Vector &vector, int rowIndex) {
    if ( (rowIndex+1 > (int) matrix.size1()) ) {
      QString msg = "Unable to set the matrix row to the given vector. Row index "
                    + toString(rowIndex) + " is out of bounds. The given matrix only has "
                    + toString((int) matrix.size1()) + " rows." ;
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    boost::numeric::ublas::row(matrix, rowIndex) = vector;
  }
 
 
  void LinearAlgebra::setColumn(Matrix &matrix, const Vector &vector, int columnIndex) {
    if ( (columnIndex+1 > (int) matrix.size2()) ) {
      QString msg = "Unable to set the matrix column to the given vector. Column index "
                    + toString(columnIndex) + " is out of bounds. The given matrix only has "
                    + toString((int) matrix.size1()) + " columns." ;
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    boost::numeric::ublas::column(matrix, columnIndex) = vector;
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::row(const Matrix &matrix, int rowIndex) {
    if ( (rowIndex+1 > (int) matrix.size1()) ) {
      QString msg = "Unable to get the matrix row to the given vector. Row index "
                    + toString(rowIndex) + " is out of bounds. The given matrix only has "
                    + toString((int) matrix.size1()) + " rows." ;
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return boost::numeric::ublas::row(matrix, rowIndex);
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::column(const Matrix &matrix, int columnIndex) {
    if ( (columnIndex+1 > (int) matrix.size2()) ) {
      QString msg = "Unable to get the matrix column to the given vector. Column index "
                    + toString(columnIndex) + " is out of bounds. The given matrix only has "
                    + toString((int) matrix.size1()) + " columns." ;
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return boost::numeric::ublas::column(matrix, columnIndex);
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::vector(double v0, double v1, double v2) {
    Vector v(3);
    v[0] = v0;
    v[1] = v1;
    v[2] = v2;
    return v;
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::vector(double v0, double v1, double v2, double v3) {
    Vector v(4);
    v[0] = v0;
    v[1] = v1;
    v[2] = v2;
    v[3] = v3;
    return v;
  }
 
 
  void LinearAlgebra::setVec3(Vector *v, double v0, double v1, double v2) {
    (*v)[0] = v0;
    (*v)[1] = v1;
    (*v)[2] = v2;
  }
 
 
  void LinearAlgebra::setVec4(Vector *v, double v0, double v1, double v2, double v3) {
    (*v)[0] = v0;
    (*v)[1] = v1;
    (*v)[2] = v2;
    (*v)[3] = v3;
  }
 
 
  LinearAlgebra::Vector LinearAlgebra::subVector(const Vector &v, int startIndex, int size) {
    LinearAlgebra::Vector sub(size);
    for (int i = 0; i < size; i++) {
      sub[i] = v[i + startIndex];
    }
    return sub;
  }
 
 
  QDebug operator<<(QDebug dbg, const LinearAlgebra::Vector &vector) {
    QDebugStateSaver saver(dbg);
    dbg.noquote() << toString(vector);
    return dbg;
  }
 
 
  QDebug operator<<(QDebug dbg, const LinearAlgebra::Matrix &matrix) {
    QDebugStateSaver saver(dbg);
    for (unsigned int i = 0; i < matrix.size1(); i++) {
      dbg.noquote() << "    ";
      for (unsigned int j = 0; j < matrix.size2(); j++) {
        dbg.noquote() << toString(matrix(i, j), 15) << "     ";
      }
      dbg.noquote() << endl;
    }
    return dbg;
  }
 
 
  QString toString(const LinearAlgebra::Vector &vector, int precision) {
    QString result = "( ";
    for (unsigned int i = 0; i < vector.size(); i++) {
      result += toString(vector(i), precision);
      if (i != vector.size() - 1) result += ", ";
    }
    result += " )";
    return result;
  }
}