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/alposition2d.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>
#include <almath/types/aldisplacement.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);

    void position2DFromTransformInPlace(
        const Transform&     pT,
        Position2D&          pPos);

    Position2D position2DFromTransform(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 Position2D& pPos);

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

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

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


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

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

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

    Transform orthogonalSpace(const Position3D& pPos);

    Transform transformFromQuaternion(
      const Quaternion& pQua);

    Quaternion quaternionFromTransform(
      const Transform& pT);

    Transform transformFromDisplacement(const Displacement& pDisp);

    Displacement displacementFromTransform(const Transform& pTrans);

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