Utilities.cpp

#include "Utilities.h"

using json = nlohmann::json;

// Calculates a rotation matrix from Euler angles
// in - euler[3]
// out - rotationMatrix[9]
void calculateRotationMatrixFromEuler(
    double euler[],
    double rotationMatrix[])
{
  double cos_a = cos(euler[0]);
  double sin_a = sin(euler[0]);
  double cos_b = cos(euler[1]);
  double sin_b = sin(euler[1]);
  double cos_c = cos(euler[2]);
  double sin_c = sin(euler[2]);

  rotationMatrix[0] = cos_b * cos_c;
  rotationMatrix[1] = -cos_a * sin_c + sin_a * sin_b * cos_c;
  rotationMatrix[2] = sin_a * sin_c + cos_a * sin_b * cos_c;
  rotationMatrix[3] = cos_b * sin_c;
  rotationMatrix[4] = cos_a * cos_c + sin_a * sin_b * sin_c;
  rotationMatrix[5] = -sin_a * cos_c + cos_a * sin_b * sin_c;
  rotationMatrix[6] = -sin_b;
  rotationMatrix[7] = sin_a * cos_b;
  rotationMatrix[8] = cos_a * cos_b;
}


// uses a quaternion to calclate a rotation matrix.
// in - q[4]
// out - rotationMatrix[9]
void calculateRotationMatrixFromQuaternions(
    double q[4],
    double rotationMatrix[9])
{
  double norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
  q[0] /= norm;
  q[1] /= norm;
  q[2] /= norm;
  q[3] /= norm;

  rotationMatrix[0] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
  rotationMatrix[1] = 2 * (q[0] * q[1] - q[2] * q[3]);
  rotationMatrix[2] = 2 * (q[0] * q[2] + q[1] * q[3]);
  rotationMatrix[3] = 2 * (q[0] * q[1] + q[2] * q[3]);
  rotationMatrix[4] = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
  rotationMatrix[5] = 2 * (q[1] * q[2] - q[0] * q[3]);
  rotationMatrix[6] = 2 * (q[0] * q[2] - q[1] * q[3]);
  rotationMatrix[7] = 2 * (q[1] * q[2] + q[0] * q[3]);
  rotationMatrix[8] = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
}

// Compue the distorted focal plane coordinate for a given image pixel
// in - line
// in - sample
// in - sampleOrigin - the origin of the ccd coordinate system relative to the top left of the ccd
// in - lineOrigin - the origin of the ccd coordinate system relative to the top left of the ccd
// in - sampleSumming
// in - startingSample - first ccd sample for the image
// in - iTransS[3] - the transformation from focal plane to ccd samples
// in - iTransL[3] - the transformation from focal plane to ccd lines
// out - natFocalPlane
void computeDistortedFocalPlaneCoordinates(
    const double& line,
    const double& sample,
    const double& sampleOrigin,
    const double& lineOrigin,
    const double& sampleSumming,