value_setter.h

Go to the documentation of this file.

#ifndef __INC_VALUE_SETTER
#define __INC_VALUE_SETTER

#include <vector>  
#include <frame.h>
#include <vecmath.h>
#include <utils.h>

namespace robotLibPbD
{

    class ValueSetter
    {
    public:
      CFrame *frame;
      int x,y,z;
      int a, b, g;
      int qx,qy,qz,qw;

      ValueSetter()
      {
        a = b = g = x = y = z = qx = qw = qy = qz = -1;
      }

      bool set(std::vector<double> &values)
      {
        if (frame == NULL)
          return false;

        CMatrix pose;
        if (a != -1 || b != -1 || g != -1)
        {
            /*double alpha, beta, gamma = 0.0;
            if (a >= 0)
                alpha = values[a];
            if (b >= 0)
                beta = values[b];
            if (g >= 0)
                gamma = values[g];*/
        }
        else
        {
            CVec quater;

            if (qw >= 0)
              quater.w = values[qw];
            if (qx >= 0)
              quater.x = values[qx];
            if (qy >= 0)
              quater.y = values[qy];
            if (qz >= 0)
              quater.z = values[qz];

            CMathLib::matrixFromQuaternion(quater, pose);
        }

        if (x >= 0)
          pose.a[12] = values[x];
        if (y >= 0)
          pose.a[13] = values[y];
        if (z >= 0)
          pose.a[14] = values[z];

        frame->setPose(pose);
        return true;
      }
    };
}

#endif