almath.h

Go to the documentation of this file.

/*
 * 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 {

  ALMATH_API void modulo2PIInPlace(float& pAngle);

  ALMATH_API float modulo2PI(float pAngle);

  ALMATH_API float meanAngle(const std::vector<float>& pAngles);

  ALMATH_API float weightedMeanAngle(const std::vector<float>& pAngles,
                          const std::vector<float>& pWeights);

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

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

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


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

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


    ALMATH_API Position6D position6DFromVelocity6D(const Velocity6D& pVel);

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


    ALMATH_API Position2D position2DFromPose2D(const Pose2D& pPose2D);


    ALMATH_API Position3D position3DFromPosition6D(const Position6D& pPosition6D);


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

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



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


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


    ALMATH_API Velocity3D operator*(
      const Rotation&   pRot,
      const Velocity3D& pVel);

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


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

    ALMATH_API Position6D position6DFromPose2D(const Pose2D& pPose2D);

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

    ALMATH_API Position6D position6DFromPosition3D(const Position3D& pPosition3D);

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

    ALMATH_API Pose2D pose2DFromPosition6D(const Position6D& pPosition6D);

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

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


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

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

    ALMATH_API Quaternion quaternionFromRotation3D(
        const Rotation3D& pRot3D);

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

    ALMATH_API Rotation rotationFromQuaternion(
        const Quaternion& pQua);

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

    ALMATH_API Rotation3D rotation3DFromQuaternion(
        const Quaternion& pQuaternion);

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



    ALMATH_API void pointMassRotationalInertia(
      float pMass,
      const Position3D& pPos,
      std::vector<float>& pInertia);
  } // namespace Math
} // namespace AL
#endif  // _LIBALMATH_ALMATH_TOOLS_ALMATH_H_