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

#include <vector>

namespace AL {
  namespace Math {
    struct  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;
    }; // end struct

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

    float norm(const Transform& pT);

    void normalizeTransform(Transform& pT);


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


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



    float determinant(const Transform& pT);


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



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



    Transform transformInverse(const Transform& pT);



    Transform transformFromRotX(const float pRotX);


    Transform transformFromRotY(const float pRotY);


    Transform transformFromRotZ(const float pRotZ);


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


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

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


    void transformInvertInPlace(Transform& pT);




    Transform pinv(const Transform& pT);


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

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


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

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