altransform.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_TYPES_ALTRANSFORM_H_
#define _LIBALMATH_ALMATH_TYPES_ALTRANSFORM_H_

#include <vector>
#include <almath/api.h>

namespace AL {
  namespace Math {
    struct ALMATH_API Transform {

      float r1_c1, r1_c2, r1_c3, r1_c4;
      float r2_c1, r2_c2, r2_c3, r2_c4;
      float r3_c1, r3_c2, r3_c3, r3_c4;

      Transform();


      explicit Transform(const std::vector<float>& pFloats);


      Transform(
        const float& pPosX,
        const float& pPosY,
        const float& pPosZ);

      Transform& operator*= (const Transform& pT2);

      Transform operator* (const Transform& pT2) const;

      bool operator==(const Transform& pT2) const;

      bool operator!=(const Transform& pT2) const;

      bool isNear(
        const Transform& pT2,
        const float&     pEpsilon=0.0001f) const;

      bool isTransform(
          const float& pEpsilon=0.0001f) const;

      void normalizeTransform(void);

      float norm() const;


      float determinant() const;



      Transform inverse() const;


      static Transform fromRotX(const float pRotX);


      static Transform fromRotY(const float pRotY);


      static Transform fromRotZ(const float pRotZ);


      static Transform from3DRotation(
        const float& pWX,
        const float& pWY,
        const float& pWZ);



      static Transform fromPosition(
        const float pX,
        const float pY,
        const float pZ);

      static Transform fromPosition(
        const float& pX,
        const float& pY,
        const float& pZ,
        const float& pWX,
        const float& pWY,
        const float& pWZ);

      Transform diff(const Transform& pT2) const;


      float distanceSquared(const Transform& pT2) const;


      float distance(const Transform& pT2) const;


      void toVector(std::vector<float>& pReturnVector) const;
      std::vector<float> toVector(void) const;


      void writeToVector(std::vector<float>::iterator& pIt) const;
    }; // end struct

    ALMATH_API void transformPreMultiply(
      const Transform& pT,
      Transform&       pTOut);

    ALMATH_API float norm(const Transform& pT);

    ALMATH_API void normalizeTransform(Transform& pT);


    ALMATH_API void transformToFloatVector(
      const Transform&    pT,
      std::vector<float>& pTOut);


    ALMATH_API std::vector<float> transformToFloatVector(
      const Transform& pT);



    ALMATH_API float determinant(const Transform& pT);


    ALMATH_API float determinant(const std::vector<float>& pFloats);



    ALMATH_API void transformInverse(
      const Transform& pT,
      Transform&       pTOut);



    ALMATH_API Transform transformInverse(const Transform& pT);



    ALMATH_API Transform transformFromRotX(const float pRotX);


    ALMATH_API Transform transformFromRotY(const float pRotY);


    ALMATH_API Transform transformFromRotZ(const float pRotZ);


    ALMATH_API Transform transformFrom3DRotation(
      const float& pWX,
      const float& pWY,
      const float& pWZ);


    ALMATH_API Transform transformFromPosition(
      const float& pX,
      const float& pY,
      const float& pZ);

    ALMATH_API Transform transformFromPosition(
      const float& pX,
      const float& pY,
      const float& pZ,
      const float& pWX,
      const float& pWY,
      const float& pWZ);


    ALMATH_API void transformInvertInPlace(Transform& pT);




    ALMATH_API Transform pinv(const Transform& pT);


    ALMATH_API Transform transformDiff(
      const Transform& pT1,
      const Transform& pT2);

    ALMATH_API float transformDistanceSquared(
      const Transform& pT1,
      const Transform& pT2);


    ALMATH_API float transformDistance(
      const Transform& pT1,
      const Transform& pT2);

  } // end namespace Math
} // end namespace AL
#endif  // _LIBALMATH_ALMATH_TYPES_ALTRANSFORM_H_