rotations.cpp

Go to the documentation of this file.

#include <cmath>
#include <dlrCommon/functional.h>
#include <dlrNumeric/rotations.h>
#include <dlrNumeric/utilities.h>

namespace {

  const double rotationsEpsilon_ = 1.0E-10;

}

namespace dlr {

  namespace numeric {
    
    Quaternion
    angleAxisToQuaternion(double angle, const Vector3D& axis, bool isNormalized)
    {
      // Deal with the angle.
      double angleOverTwo = angle / 2.0;
      double cosineValue = std::cos(angleOverTwo);
      double sineValue = std::sin(angleOverTwo);

      // If the axis is already unit length, we're done!
      if(isNormalized) {
        return Quaternion(cosineValue, sineValue * axis.x(),
                          sineValue * axis.y(), sineValue * axis.z());
      }

      // Axis is not known to be unit length, so we have more work to do.
      Vector3D axisCopy = axis;
      double axisMagnitude = magnitude(axis);
      if(axisMagnitude != 0.0) {
        axisCopy /= axisMagnitude;
      } else {
        // Axis is too small to observe.  Pick an arbitrary axis.
        axisCopy.setValue(1.0, 0.0, 0.0);
      }
      return Quaternion(cosineValue, sineValue * axisCopy.x(),
                        sineValue * axisCopy.y(), sineValue * axisCopy.z());
    }

  
    Vector3D
    angleAxisToRollPitchYaw(double angle, const Vector3D& axis,
                            bool isNormalized)
    {
      Quaternion quaternion = angleAxisToQuaternion(angle, axis, isNormalized);
      return quaternionToRollPitchYaw(quaternion);
    }    

  
    Transform3D
    angleAxisToTransform3D(double angle, const Vector3D& axis, bool isNormalized)
    {
      Quaternion quaternion = angleAxisToQuaternion(angle, axis, isNormalized);
      return quaternionToTransform3D(quaternion);
    }

  
    std::pair<double, Vector3D>
    quaternionToAngleAxis(const Quaternion& quaternion)
    {
      // Normalize the quaternion, if necessary.
      Quaternion quaternionCopy(quaternion);
      quaternionCopy.normalize();

      // Recover the obvious quantities.
      double cosineHalfTheta = quaternionCopy.s();
      Vector3D sineTimesAxis(
        quaternionCopy.i(), quaternionCopy.j(), quaternionCopy.k());
      double sineHalfTheta = magnitude(sineTimesAxis);

      // Recover angle and axis.
      double angle = 2 * std::atan2(sineHalfTheta, cosineHalfTheta);
      if(sineHalfTheta == 0.0) {
        return std::make_pair(angle, Vector3D(1.0, 0.0, 0.0));
      }
      return std::make_pair(angle, sineTimesAxis / sineHalfTheta);
    }

  
    Vector3D
    quaternionToRollPitchYaw(const Quaternion& quaternion)
    {
      // The quaternion will be normalized inside quaternionToTransform3D().
      Transform3D transform3D = quaternionToTransform3D(quaternion);
      return transform3DToRollPitchYaw(transform3D);
    }


    Transform3D
    quaternionToTransform3D(const Quaternion& quaternion)
    {
      // Normalize the quaternion, if necessary.
      Quaternion quaternionCopy(quaternion);
      quaternionCopy.normalize();

      // Some convenience variables.
      double ii = 2.0 * quaternionCopy.i() * quaternionCopy.i();
      double jj = 2.0 * quaternionCopy.j() * quaternionCopy.j();
      double kk = 2.0 * quaternionCopy.k() * quaternionCopy.k();
      double si = 2.0 * quaternionCopy.s() * quaternionCopy.i();
      double sj = 2.0 * quaternionCopy.s() * quaternionCopy.j();
      double sk = 2.0 * quaternionCopy.s() * quaternionCopy.k();
      double ij = 2.0 * quaternionCopy.i() * quaternionCopy.j();
      double ik = 2.0 * quaternionCopy.i() * quaternionCopy.k();
      double jk = 2.0 * quaternionCopy.j() * quaternionCopy.k();

      return Transform3D(1 - jj - kk, ij - sk, ik + sj, 0.0,
                         ij + sk, 1 - ii - kk, jk - si, 0.0,
                         ik - sj, jk + si, 1 - ii - jj, 0.0,
                         0.0, 0.0, 0.0, 1);
    }

  
    std::pair<double, Vector3D>
    rollPitchYawToAngleAxis(const Vector3D& rollPitchYaw)
    {
      Quaternion quaternion = rollPitchYawToQuaternion(rollPitchYaw);
      return quaternionToAngleAxis(quaternion);
    }

  
    Quaternion
    rollPitchYawToQuaternion(const Vector3D& rollPitchYaw)
    {
      Transform3D transform3D = rollPitchYawToTransform3D(rollPitchYaw);
      return transform3DToQuaternion(transform3D);
    }
  

    Transform3D
    rollPitchYawToTransform3D(const Vector3D& rollPitchYaw)
    {
      // First compute some convenience values.
      double cosineRoll = std::cos(rollPitchYaw.x());
      double cosinePitch = std::cos(rollPitchYaw.y());
      double cosineYaw = std::cos(rollPitchYaw.z());

      double sineRoll = std::sin(rollPitchYaw.x());
      double sinePitch = std::sin(rollPitchYaw.y());
      double sineYaw = std::sin(rollPitchYaw.z());

      // Each of roll, pitch, yaw, correspond to a rotation about one
      // axis.
      Transform3D rollTransform(1.0, 0.0, 0.0, 0.0,
                                0.0, cosineRoll, -sineRoll, 0.0,
                                0.0, sineRoll, cosineRoll, 0.0,
                                0.0, 0.0, 0.0, 1.0);
      Transform3D pitchTransform(cosinePitch, 0.0, sinePitch, 0.0,
                                 0.0, 1.0, 0.0, 0.0,
                                 -sinePitch, 0.0, cosinePitch, 0.0,
                                 0.0, 0.0, 0.0, 1.0);
      Transform3D yawTransform(cosineYaw, -sineYaw, 0.0, 0.0,
                               sineYaw, cosineYaw, 0.0, 0.0,
                               0.0, 0.0, 1.0, 0.0, 
                               0.0, 0.0, 0.0, 1.0);

      // Compose the three rotations to get the result.
      return rollTransform * (pitchTransform * yawTransform);
    }

  
    std::pair<double, Vector3D>
    transform3DToAngleAxis(const Transform3D& transform3D)
    {
      Quaternion quaternion = transform3DToQuaternion(transform3D);
      return quaternionToAngleAxis(quaternion);
    }

  
    // This routine draws from _On Homogeneous Transforms, Quaternions,
    // and Computation Efficiency_, by Funda, Taylor, and Paul, IEEE R&A,
    // June 1990.
    Quaternion
    transform3DToQuaternion(const Transform3D& transform3D)
    {
      // For convenience, we make temporary variables representing the
      // elements of transform3D.
      double t00 = transform3D.value<0, 0>();
      double t01 = transform3D.value<0, 1>();
      double t02 = transform3D.value<0, 2>();
      double t10 = transform3D.value<1, 0>();
      double t11 = transform3D.value<1, 1>();
      double t12 = transform3D.value<1, 2>();
      double t20 = transform3D.value<2, 0>();
      double t21 = transform3D.value<2, 1>();
      double t22 = transform3D.value<2, 2>();
    
      // First compute s.
      double sSquaredTimesFour = (1.0 + t00 + t11 + t22);
      // Allow for numerical errors.
      if(sSquaredTimesFour < 0.0) {
        sSquaredTimesFour = 0.0;
      }
      double sValue = std::sqrt(sSquaredTimesFour) / 2.0;
      // Allow for numerical errors.
      if(sValue > 1.0) {
        sValue = 1.0;
      }

      // This vector points in the direction of the axis of rotation, but
      // unfortunately goes to zero for theta approaching 0 degrees and
      // 180 degrees.
      Vector3D axis0(t21 - t12, t02 - t20, t10 - t01);
    
      // We need to find another parallel vector using the elements of
      // transform3D. Start by noting which axis dominates.
      int axisOfLargestRotation = 0;
      if(t00 > t11) {
        if(t00 > t22) {
          axisOfLargestRotation = 0;
        } else {
          axisOfLargestRotation = 2;
        }
      } else {
        if(t11 > t22) {
          axisOfLargestRotation = 1;
        } else {
          axisOfLargestRotation = 2;
        }
      }

      // Now compute the parallel vector and add it to axis0.
      if(axisOfLargestRotation == 0) {
        Vector3D axis1(1.0 + t00 - t11 - t22, t10 + t01, t20 + t02);
        if(axis0.x() >= 0) {
          axis0 += axis1;
        } else {
          axis0 -= axis1;
        }
      } else if(axisOfLargestRotation == 1) {
        Vector3D axis1(t10 + t01, 1.0 + t11 - t00 - t22, t21 + t12);
        if(axis0.y() >= 0) {
          axis0 += axis1;
        } else {
          axis0 -= axis1;
        }
      } else if(axisOfLargestRotation == 2) {
        Vector3D axis1(t20 + t02, t21 + t12, 1.0 + t22 - t00 - t11);
        if(axis0.z() >= 0) {
          axis0 += axis1;
        } else {
          axis0 -= axis1;
        }
      }

      // Now see about normalizing the unit quaternion.
      double axisMagnitudeSquared = dot(axis0, axis0);
      if(approximatelyEqual(axisMagnitudeSquared, 0.0, rotationsEpsilon_)) {
        // Hmm, we still have a very small axis.  Assume this means that
        // the input rotation is nearly zero.
        if(sValue >= 0) {
          return Quaternion(1.0, 0.0, 0.0, 0.0);
        } else {
          return Quaternion(-1.0, 0.0, 0.0, 0.0);
        }
      }
      axis0 *= std::sqrt((1 - (sValue * sValue)) / axisMagnitudeSquared);

      // Done.
      return Quaternion( sValue, axis0.x(), axis0.y(), axis0.z());
    }

  
    Vector3D
    transform3DToRollPitchYaw(const Transform3D& transform3D)
    {
      // There must be a better way to get this value.
      const double piOverTwo = 1.57079632679;    

      // Start by recovering pitch.
      double sinePitch = transform3D.value<0, 2>();
      double cosinePitch = std::sqrt(
        transform3D.value<0, 0>() * transform3D.value<0, 0>()
        + transform3D.value<0, 1>() * transform3D.value<0, 1>());
      double pitch = std::atan2(sinePitch, cosinePitch);

      // Now recover roll and yaw.
      double roll;
      double yaw;
      if((!approximatelyEqual(pitch, piOverTwo, rotationsEpsilon_))
         && (!approximatelyEqual(pitch, -piOverTwo, rotationsEpsilon_))) {
        double cosinePitch = std::cos(pitch);
        roll = std::atan2(-(transform3D.value<1, 2>() / cosinePitch),
                          (transform3D.value<2, 2>() / cosinePitch));
        yaw = std::atan2(-(transform3D.value<0, 1>() / cosinePitch),
                         (transform3D.value<0, 0>() / cosinePitch));
      } else {
        roll = std::atan2(transform3D.value<2, 1>(),
                          transform3D.value<1, 1>());
        yaw = 0.0;
      }
      return Vector3D(roll, pitch, yaw);
    }

  
  } // namespace numeric

} // namespace dlr

---