Cross.cpp

#include "Cross.h"
#include <osg/Geometry>
#include <osg/Array>
#include <osgText/Text>


Cross::Cross(float cross_len) {
    geom = new osg::Geometry;
    this->addDrawable(geom);
    this->vertices = new osg::Vec3Array;
    this->vertices->push_back(osg::Vec3(-cross_len, 0.0, 0.0));
    this->vertices->push_back(osg::Vec3(cross_len, 0.0, 0.0));
    this->vertices->push_back(osg::Vec3(0.0, -cross_len, 0.0));
    this->vertices->push_back(osg::Vec3(0.0, cross_len, 0.0));
    this->vertices->push_back(osg::Vec3(0.0, 0.0, -cross_len));
    this->vertices->push_back(osg::Vec3(0.0, 0.0, cross_len));
    this->geom->setVertexArray(this->vertices);

    lines = new osg::DrawElementsUInt(osg::PrimitiveSet::LINES, 0);
    lines->push_back(5);
    lines->push_back(4);
    lines->push_back(3);
    lines->push_back(2);
    lines->push_back(1);
    lines->push_back(0);

    geom->addPrimitiveSet(lines);
}

void Cross::setVertexLabel(int id, std::string label) {
    osg::Vec3 vec = this->vertices[0][id];
    osgText::TextBase::AxisAlignment plane;
    osgText::TextBase::AlignmentType alignment;
    for(int i=0; i < 3; i++) {
        if(vec[i] == 0) {
            continue;
        }
        if(vec[i] < 0) {
            alignment = osgText::Text::RIGHT_CENTER;
        }
        else {
            alignment = osgText::Text::LEFT_CENTER;
        }
    }
    
    plane = osgText::Text::XY_PLANE;
    osg::Geode *v_geode = new osg::Geode;
    osgText::Text *v_text = new osgText::Text;
    v_geode->addDrawable(v_text);
    this->addDrawable(v_text);
    v_text->setPosition(vec);
    v_text->setText(label);
    v_text->setCharacterSize(10);
    v_text->setDrawMode(osgText::Text::TEXT |
          osgText::Text::ALIGNMENT |
          osgText::Text::BOUNDINGBOX);
    v_text->setAlignment(alignment);
    v_text->setAxisAlignment(plane);
}

Cross.h

#ifndef _CROSS_H_
#define _CROSS_H_

#include <osg/Geode>
#include <osg/Geometry>
#include <string>

class Cross : public osg::Geode {
    private:
        osg::Geometry *geom;
        osg::Vec3Array *vertices;
        osg::DrawElementsUInt *lines;
    public:
        Cross(float cross_len);
        void setVertexLabel(int id, std::string label);
};

#endif

HUD.cpp

#include "HUD.h"
#include <string>

HUD::HUD() {
    this->hud_geode = new osg::Geode;
    this->hud_text = new osgText::Text;
    this->hud_view_m = new osg::MatrixTransform();

    this->setMatrix(osg::Matrix::ortho2D(0,1024,0,768));
    this->hud_view_m->setMatrix(osg::Matrix::identity());
    this->hud_view_m->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
    this->hud_view_m->addChild(hud_geode);
    this->addChild(hud_view_m);

    this->hud_text->setCharacterSize(10);
    //this->hud_text->setFont("C:/WINDOWS/Fonts/impact.ttf");
    this->hud_text->setAxisAlignment(osgText::Text::SCREEN);
    this->hud_text->setPosition( osg::Vec3(0,750, -1) );
    this->hud_geode->addDrawable(hud_text);
}

void HUD::setStatus(std::string text) {
    this->hud_text->setText(text);
}

HUD.h

#ifndef _HUD_H
#define _HUD_H_

#include <osg/Projection>
#include <osg/MatrixTransform>
#include <osgText/Text>
#include <osg/Geode>
#include <string>

class HUD : public osg::Projection {
private:
    osg::Geode *hud_geode;
    osg::MatrixTransform *hud_view_m;
    osgText::Text *hud_text;
public:
    HUD();
    void setStatus(std::string text);
};

#endif

Makefile

SRC=Simple.cpp Cross.cpp HUD.cpp Point.cpp

CXXFLAGS=-DLINUX

INCLUDE = -I. -I/usr/include/ -I/usr/include/X11/ -I/usr/local/include/GL 
LDLIBS = -lm -ldl -lGL -lGLU -lpthread -lXext -lX11 -losg -losgViewer -losgSim -losgGA -losgText
LDFLAGS =  -L. -L/usr/lib -L/usr/X11R6/lib -L/usr/local/lib
COBJS=$(patsubst %.cpp,%.o,$(SRC))

CC=g++

EXE=simple

all: $(EXE)

$(EXE): $(COBJS)
	$(CC) -o $(EXE) $(COBJS) $(INCLUDE) $(LDFLAGS) $(LDLIBS)

$(COBJS): %.o : %.cpp
	$(CC) -c $(CXXFLAGS) -o $@ $<

.PHONY: clean

clean:
	rm -rf $(EXE) $(COBJS)

Point.cpp

#include "Point.h"
#include <osg/Geode>
#include <osg/Geometry>
#include <osg/Array>

Point::Point(osg::Vec3 pos) {

    geom = new osg::Geometry;
    this->addDrawable(geom);
    this->vertices = new osg::Vec3Array;

    vertices->push_back(pos);

    geom->setVertexArray(this->vertices);

    geom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,0,vertices->size()));
}

Point.h

#ifndef _POINT_H_
#define _POINT_H_

#include <osg/Geode>
#include <osg/Geometry>

class Point : public osg::Geode {
    private:
        osg::Geometry *geom;
        osg::Vec3Array *vertices;
    public:
        Point(osg::Vec3 pos);
};

#endif

Simple.cpp

#include <osgViewer/Viewer>
#include <osgGA/TrackballManipulator>
#include <osg/Point>
#include <osg/Math>
#include <iostream>
#include <iomanip>
#include <sstream>

#include "Cross.h"
#include "HUD.h"
#include "Point.h"

//#define S std::setw(4)
#define S '\t'

#define DEBUG_MATD(matd, s) s.fill(' '); s \
      << S << matd(0,0) << S << ' ' << S << matd(0, 1) << S << ' ' << S << matd(0, 2) << S << ' ' << S << matd(0, 3) << '\n' \
      << S << matd(1,0) << S << ' ' << S << matd(1, 1) << S << ' ' << S << matd(1, 2) << S << ' ' << S << matd(1, 3) << '\n' \
      << S << matd(2,0) << S << ' ' << S << matd(2, 1) << S << ' ' << S << matd(2, 2) << S << ' ' << S << matd(2, 3) << '\n' \
      << S << matd(3,0) << S << ' ' << S << matd(3, 1) << S << ' ' << S << matd(3, 2) << S << ' ' << S << matd(3, 3) << '\n' 

std::string getMatrixRepresentation(osg::Matrixd &m) {
    std::stringstream s;
    s.fill(' ');
    s.setf(std::ios::right, std::ios::adjustfield);
    for(int i=0; i < 4; i++) {
        for(int j=0; j < 4; j++) {
            s << std::setw(10) << m(i, j) << ", ";
        }
        s << '\n';
    }
    return s.str();
}
std::string getMatrixRepresentation(osg::Vec3d v) {
    std::stringstream s;
    s.fill(' ');
    s.setf(std::ios::right, std::ios::adjustfield);
    s << "vec(";
    for(int i=0; i < 3; i++) {
        s << std::setw(10) << v[i];
        if(i < 2) {
            s << ',';
        }
    }
    s << ")\n";
    return s.str();
}
std::string getMatrixRepresentation(const osg::Quat q) {
    std::stringstream s;
    s.fill(' ');
    s.setf(std::ios::right, std::ios::adjustfield);
    s << "quat(" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << ")\n";
    return s.str();
}

osg::Vec3d getHPRfromQuat(osg::Quat quat){
// From: http://guardian.curtin.edu.au/cga/faq/angles.html
// Except OSG exchanges pitch & roll from what is listed on that page
    double qx = quat.x();
    double qy = quat.y();
    double qz = quat.z();
    double qw = quat.w();

    double sqx = qx * qx;
    double sqy = qy * qy;
    double sqz = qz * qz;
    double sqw = qw * qw;

    double term1 = 2*(qx*qy+qw*qz);
    double term2 = sqw+sqx-sqy-sqz;
    double term3 = -2*(qx*qz-qw*qy);
    double term4 = 2*(qw*qx+qy*qz);
    double term5 = sqw - sqx - sqy + sqz;

    double heading = osg::RadiansToDegrees(atan2(term1, term2));
    double pitch = osg::RadiansToDegrees(atan2(term4, term5));
    double roll = osg::RadiansToDegrees(asin(term3));

    return osg::Vec3d( heading, pitch, roll );
} 

int main(void) {
    osgViewer::Viewer viewer;

    //otherwise we wouldn't see individual vertices due to culling (bounding
    //box 0)
    viewer.getCamera()->setCullingMode(
    viewer.getCamera()->getCullingMode() &
    ~osg::CullSettings::SMALL_FEATURE_CULLING);

    osg::Group *root = new osg::Group();
    viewer.setSceneData(root);
    osgGA::TrackballManipulator *cameraManipulator = new osgGA::TrackballManipulator();
    viewer.setCameraManipulator(cameraManipulator);
    viewer.realize();

    cameraManipulator->setByMatrix(osg::Matrixd::identity());

    Cross *crossGeode = new Cross(100.0);
    crossGeode->setVertexLabel(0, "-X");
    crossGeode->setVertexLabel(1, "X");
    crossGeode->setVertexLabel(2, "-Y");
    crossGeode->setVertexLabel(3, "Y");
    crossGeode->setVertexLabel(4, "-Z");
    crossGeode->setVertexLabel(5, "Z");

    root->addChild(crossGeode);

    HUD *hudGeode = new HUD();
    root->addChild(hudGeode);
    
    osg::Point *point=new osg::Point;
    point->setSize(20);
    root->getOrCreateStateSet()->setAttribute(point);
    Point *pointGeode = new Point(osg::Vec3(0.0, 0.0, -10.0));
    root->addChild(pointGeode);

#define DEG_DELTA 1.0
#define DEG 40.0

    osg::Matrixd camM;
    std::stringstream oss;
    osg::Quat x_rot_q(osg::DegreesToRadians(-DEG), osg::Vec3d(1.0, 0.0, 0.0));
    osg::Quat y_rot_q(osg::DegreesToRadians(DEG), osg::Vec3d(0.0, 1.0, 0.0));
    camM = cameraManipulator->getMatrix();
    camM.makeRotate(x_rot_q * y_rot_q);
    camM.setTrans(250.0, 300.0, 250.0);
    cameraManipulator->setByMatrix(camM);

    osg::Quat y_delta_trans(osg::DegreesToRadians(DEG_DELTA), osg::Vec3d(0.0, 1.0, 0.0));

    while(!viewer.done()) {
        oss.str(std::string());
        oss.clear();
        camM = cameraManipulator->getMatrix();
        camM.makeRotate(camM.getRotate() * y_delta_trans);
        camM.setTrans(250.0, 300.0, 250.0);
        cameraManipulator->setByMatrix(camM);
        oss << getMatrixRepresentation(camM);
        hudGeode->setStatus(oss.str());
        viewer.frame();
    }
    return 0;
}