Arrow.hpp

Go to the documentation of this file.

 
 /*
 * 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>
 
 
using namespace std;
 
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_ */