Program Listing for File Arrow.hpp

↰ Return to documentation for file (include/lvr2/display/Arrow.hpp)

 /*
 * PointCloud.h
 *
 *  Created on: 02.09.2008
 *      Author: twiemann
 */

#ifndef ARROW_H_
#define ARROW_H_

#include "lvr2/display/Renderable.hpp"

#include "lvr2/geometry/BaseVector.hpp"

#include <vector>
#include <string>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>

namespace lvr2
{

class Arrow : public Renderable{
public:
    Arrow(string filename);
    Arrow(int color);
    virtual ~Arrow();
    virtual inline void render();

    void setPosition(double x, double y, double z, double roll, double pitch, double yaw);

    inline void setColor(int color) {
        this->color = color;
    };

private:
    double roll, pitch, yaw;
    int color;
    Matrix4<BaseVector<float>> rotation;
};




inline void EulerToMatrix(const double *rPos, const double *rPosTheta, float *alignxf)
{
  double sx = sin(rPosTheta[0]);
  double cx = cos(rPosTheta[0]);
  double sy = sin(rPosTheta[1]);
  double cy = cos(rPosTheta[1]);
  double sz = sin(rPosTheta[2]);
  double cz = cos(rPosTheta[2]);

  alignxf[0]  = cy*cz;
  alignxf[1]  = sx*sy*cz + cx*sz;
  alignxf[2]  = -cx*sy*cz + sx*sz;
  alignxf[3]  = 0.0;
  alignxf[4]  = -cy*sz;
  alignxf[5]  = -sx*sy*sz + cx*cz;
  alignxf[6]  = cx*sy*sz + sx*cz;
  alignxf[7]  = 0.0;
  alignxf[8]  = sy;
  alignxf[9]  = -sx*cy;
  alignxf[10] = cx*cy;

  alignxf[11] = 0.0;

  alignxf[12] = rPos[0];
  alignxf[13] = rPos[1];
  alignxf[14] = rPos[2];
  alignxf[15] = 1;
};

} // namespace lvr2

// @TODO Why do we need this?
void render();

#endif /* Arrow_H_ */