almath.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_ALMATH_H_
#define _LIBALMATH_ALMATH_TOOLS_ALMATH_H_

#include <almath/types/alvelocity3d.h>
#include <almath/types/alvelocity6d.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/alquaternion.h>
#include <almath/types/alpose2d.h>

namespace AL {
  namespace Math {

  void modulo2PIInPlace(float& pAngle);

  float modulo2PI(float pAngle);

  bool clipData(
    const float& pMin,
    const float& pMax,
    float&       pData);

  bool clipData(
    const float& pMin,
    const float& pMax,
    std::vector<float>& pData);

  bool clipData(
    const float& pMin,
    const float& pMax,
    std::vector<std::vector<float> >& pData);


  void changeReferencePose2D(
      const float&  pTheta,
      const Pose2D& pPosIn,
      Pose2D&       pPosOut);

  void changeReferencePose2DInPlace(
      const float& pTheta,
      Pose2D&      pPosOut);


    Position6D position6DFromVelocity6D(const Velocity6D& pVel);

    void position2DFromPose2DInPlace(
        const Pose2D& pPose2D,
        Position2D&   pPosition2D);


    Position2D position2DFromPose2D(const Pose2D& pPose2D);


    Position3D position3DFromPosition6D(const Position6D& pPosition6D);


    Position3D operator*(
      const Rotation&   pRot,
      const Position3D& pPos);

    Position3D operator*(
        const Quaternion& pQuat,
        const Position3D& pPos);



    Velocity6D operator*(
      const float       pVal,
      const Position6D& pPos);


    Velocity3D operator*(
      const float       pVal,
      const Position3D& pPos);


    Rotation rotationFromAngleDirection(
      const float&      pTheta,
      const Position3D& pPos);


    void position6DFromPose2DInPlace(
        const Pose2D& pPose2D,
        Position6D&   pPosition6D);

    Position6D position6DFromPose2D(const Pose2D& pPose2D);

    void position6DFromPosition3DInPlace(
        const Position3D& pPosition3D,
        Position6D&       pPosition6D);

    Position6D position6DFromPosition3D(const Position3D& pPosition3D);

    void pose2DFromPosition6DInPlace(
        const Position6D& pPosition6D,
        Pose2D&           pPose2D);

    Pose2D pose2DFromPosition6D(const Position6D& pPosition6D);

    void pose2DFromPosition2DInPlace(
        const Position2D& pPosition2D,
        const float pAngle,
        Pose2D& pPose2D);

    Pose2D pose2DFromPosition2D(const Position2D& pPosition2D,
                                const float       pAngle=0.0f);


    Position2D operator*(
      const Pose2D& pVal,
      const Position2D& pPos);

    void quaternionFromRotation3D(
        const Rotation3D& pRot3D,
        Quaternion& pQuaternion);

    Quaternion quaternionFromRotation3D(
        const Rotation3D& pRot3D);

    void rotationFromQuaternion(
        const Quaternion& pQua,
        Rotation& pRot);

    Rotation rotationFromQuaternion(
        const Quaternion& pQua);

    void rotation3DFromQuaternion(
        const Quaternion& pQuaterion,
        Rotation3D& pRot3D);

    Rotation3D rotation3DFromQuaternion(
        const Quaternion& pQuaternion);

    void quaternionPosition3DFromPosition6D(
        const Position6D& pPos6D,
        Quaternion& pQua,
        Position3D& pPos3D);

  } // namespace Math
} // namespace AL
#endif  // _LIBALMATH_ALMATH_TOOLS_ALMATH_H_