almath/tools/altransformhelpers.h

/*
 * Copyright (c) 2012 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */


#pragma once
#ifndef _LIBALMATH_ALMATH_TOOLS_ALTRANSFORMHELPERS_H_
#define _LIBALMATH_ALMATH_TOOLS_ALTRANSFORMHELPERS_H_

#include <almath/types/alposition3d.h>
#include <almath/types/alposition6d.h>
#include <almath/types/alrotation.h>
#include <almath/types/alrotation3d.h>
#include <almath/types/alvelocity6d.h>
#include <almath/types/altransform.h>
#include <almath/types/alaxismask.h>
#include <almath/types/alpose2d.h>
#include <almath/types/alquaternion.h>

namespace AL {
  namespace Math {










  void transformLogarithmInPlace(
    const Transform& pT,
    Velocity6D&      pVel);









    Velocity6D transformLogarithm(const Transform& pT);









    Transform velocityExponential(const Velocity6D& pVel);

    // TODO: Add to doc or set private.
    void velocityExponentialInPlace(
      const Velocity6D& pVel,
      Transform&        pT);










    void changeReferenceVelocity6D(
      const Transform&  pT,
      const Velocity6D& pVelIn,
      Velocity6D&       pVelOut);










    void changeReferencePosition6D(
      const Transform&  pT,
      const Position6D& pPosIn,
      Position6D&       pPosOut);

    // TODO: rename argument.
    void changeReferencePosition3DInPlace(
      const Transform& pT,
      Position3D&      pPosOut);

    // TODO: rename argument.
    void changeReferenceTransposePosition3DInPlace(
      const Transform& pT,
      Position3D&      pPosOut);










    void changeReferencePosition3D(
      const Transform&  pT,
      const Position3D& pPosIn,
      Position3D&       pPosOut);









    void changeReferenceTransposePosition3D(
      const Transform&  pT,
      const Position3D& pPosIn,
      Position3D&       pPosOut);









    void changeReferenceTransform(
      const Transform& pT,
      const Transform& pTIn,
      Transform&       pTOut);









    void changeReferenceTransposeTransform(
      const Transform& pT,
      const Transform& pTIn,
      Transform&       pTOut);











    void changeReferenceTransposeVelocity6D(
      const Transform&  pT,
      const Velocity6D& pVelIn,
      Velocity6D&       pVelOut);










    void changeReferenceTransposePosition6D(
      const Transform&  pT,
      const Position6D& pPosIn,
      Position6D&       pPosOut);

    void transformMeanInPlace(
      const Transform& pTIn1,
      const Transform& pTIn2,
      const float&     pVal,
      Transform&       pTOut);

    Transform transformMean(
      const Transform& pTIn1,
      const Transform& pTIn2,
      const float&     pVal = 0.5f);











    Transform transformFromRotationPosition3D(
      const Rotation& pRot,
      const float&    pX,
      const float&    pY,
      const float&    pZ);









    Transform transformFromRotationPosition3D(
      const Rotation&   pRot,
      const Position3D& pPos);






    void transformFromPosition3DInPlace(
      const Position3D& pPosition,
      Transform&        pTransform);









    Transform transformFromPosition3D(const Position3D& pPosition);






    void transformFromRotationInPlace(
      const Rotation& pRotation,
      Transform&      pTransform);








    Transform transformFromRotation(const Rotation& pRotation);

    void rotationFromTransformInPlace(
      const Transform& pTransform,
      Rotation&        pRotation);

    Rotation rotationFromTransform(const Transform& pTransform);

    Rotation3D rotation3DFromRotation(const Rotation& pRotation);

    void position6DFromTransformInPlace(
        const Transform& pT,
        Position6D&      pPos);

    Position6D position6DFromTransform(const Transform& pT);

    void transformFromPose2DInPlace(
        const Pose2D& pPose,
        Transform&    pT);

    Transform transformFromPose2D(const Pose2D& pPose);

    void pose2DFromTransformInPlace(
        const Transform& pT,
        Pose2D&          pPos);

    Pose2D pose2DFromTransform(const Transform& pT);

    Transform transformFromRotation3D(const Rotation3D& pRotation);

    Transform transformFromPosition6D(const Position6D& pPosition6D);

    void position6DFromTransformDiffInPlace(
      const Transform& pCurrent,
      const Transform& pTarget,
      Position6D&      result);

    Position6D position6DFromTransformDiff(
      const Transform& pCurrent,
      const Transform& pTarget);






    void position3DFromTransformInPlace(
      const Transform& pT,
      Position3D&      pPos);






    Position3D position3DFromTransform(const Transform& pT);


    Rotation3D rotation3DFromTransform(const Transform& pT);

    void transformFromRotVecInPlace(
      const int         pAxis,
      const float       pTheta,
      const Position3D& pPos,
      Transform&        pT);

    Transform transformFromRotVec(
      const int         pAxis,
      const float       pTheta,
      const Position3D& pPos);


    void transformFromRotVecInPlace(
      const Position3D& pPos,
      Transform&        pT);

    Transform transformFromRotVec(const Position3D& pPos);


    Transform transformFromRotVec(
      const int&   pAxis,
      const float& pTheta);

    Position3D operator*(
      const Transform&  pT,
      const Position3D& pPos);

    Transform axisRotationProjection(
      const Position3D& pPos,
      const Transform&  pT);

    Rotation axisRotationProjection(
      const Position3D& pAxis,
      const Rotation&   pRot);


    void axisRotationProjectionInPlace(
      const Position3D& pPos,
      Transform&        pT);

    void axisRotationProjectionInPlace(
      const Position3D& pPos,
      Rotation&         pRot);


    void orthogonalSpace(
      const Position3D& pPos,
      Transform&        pTOut);

    Transform orthogonalSpace(const Position3D& pPos);

    Transform transformFromQuaternion(
      const Quaternion& pQua);

    Quaternion quaternionFromTransform(
      const Transform& pT);

  } // namespace Math
} // namespace AL
#endif  // _LIBALMATH_ALMATH_TOOLS_ALTRANSFORMHELPERS_H_