update to vizkit3d
[rock-gui:point_cloud.git] / viz / PointCloudVisualization.cpp
1 #include "PointCloudVisualization.hpp"
2 #include <iostream>
3 #include <osgUtil/Optimizer>
4 #include <osg/PolygonMode>
5 #include <osg/Point>
6 #include <base/Logging.hpp>
7
8 namespace vizkit3d
9 {
10
11 PointCloudVisualization::PointCloudVisualization()
12 {
13     reduction_ = 1.0;
14     cloud_ = new osg::Vec3Array();
15     point_size_ = 1.f;
16
17     color_index_ = 0;
18     colors_.push_back(osg::Vec4d(0.8,0.8,0.8,1));
19     colors_.push_back(osg::Vec4d(1,1,1,1));
20     colors_.push_back(osg::Vec4d(0.2,0.2,0.2,1));
21     colors_.push_back(osg::Vec4d(0,0,0,1));
22     colors_.push_back(osg::Vec4d(1,0,0,1));
23     colors_.push_back(osg::Vec4d(0,1,0,1));
24     colors_.push_back(osg::Vec4d(0,0,1,1));
25     colors_.push_back(osg::Vec4d(0,1,1,1));
26     colors_.push_back(osg::Vec4d(1,0,1,1));
27     colors_.push_back(osg::Vec4d(1,1,0,1));
28
29     color_ = colors_[color_index_];
30 }
31
32
33 void analysePrimSet(osg::PrimitiveSet*prset, const osg::Vec3Array *verts) {
34     unsigned int ic;
35     unsigned int i2;
36     unsigned int nprim=0;
37     osg::notify(osg::WARN) << "Prim set type "<< prset->getMode() << std::endl;
38     for (ic=0; ic<prset->getNumIndices(); ic++) { // NB the vertices are held in the drawable -
39         osg::notify(osg::WARN) << "vertex "<< ic << " is index "<<prset->index(ic) << " at " <<
40                                   (* verts)[prset->index(ic)].x() << "," <<
41                                   (* verts)[prset->index(ic)].y() << "," <<
42                                   (* verts)[prset->index(ic)].z() << std::endl;
43     }
44     // you might want to handle each type of primset differently: such as:
45     switch (prset->getMode()) {
46     case osg::PrimitiveSet::TRIANGLES: // get vertices of triangle
47         osg::notify(osg::WARN) << "Triangles "<< nprim << " is index "<<prset->index(ic) << std::endl;
48         for (i2=0; i2<prset->getNumIndices()-2; i2+=3) {
49         }
50         break;
51     case osg::PrimitiveSet::TRIANGLE_STRIP: // look up how tristrips are coded
52         break;
53         // etc for all the primitive types you expect. EG quads, quadstrips lines line loops....
54     }
55 }
56
57 void PointCloudVisualization::setModelFile(QString modelFile)
58 {
59     cloud_->clear();
60
61     osg::ref_ptr<osg::Node> loadedModel = osgDB::readNodeFile(std::string(modelFile.toAscii().data()));
62     osgUtil::Optimizer optimizer;
63     optimizer.optimize(loadedModel.get());
64
65     osg::Geode* geode = loadedModel.get()->asGeode();
66     for (unsigned int i=0; i<geode->getNumDrawables(); i++) {
67         osg::Drawable *drawable=geode->getDrawable(i);
68         osg::Geometry *geom=dynamic_cast<osg::Geometry *> (drawable);
69         for (unsigned int ipr=0; ipr<geom->getNumPrimitiveSets(); ipr++) {
70             //osg::PrimitiveSet* prset=geom->getPrimitiveSet(ipr);
71             osg::notify(osg::WARN) << "Primitive Set "<< ipr << std::endl;
72
73             const osg::Vec3Array* vertices = dynamic_cast<const osg::Vec3Array*>(geom->getVertexArray());
74             cloud_->insert(cloud_->end(), vertices->begin(), vertices->end());
75         }
76     }
77
78     modelFile_ = modelFile;
79
80     emit childrenChanged();
81     setDirty();
82 }
83
84 QString PointCloudVisualization::modelFile() const{
85     return modelFile_;
86 }
87
88 osg::ref_ptr< osg::Node > PointCloudVisualization::createMainNode()
89 {
90     osg::Group* group = new osg::Group;
91
92     return group;
93 }
94
95 void PointCloudVisualization::setCloudColor(int color)
96 {
97     this->color_index_ = color%colors_.size();
98     this->color_ = colors_[color_index_];
99     setDirty();
100 }
101
102 int PointCloudVisualization::cloudColor()
103 {
104     return color_index_;
105 }
106
107 float PointCloudVisualization::pointSize()
108 {
109     return point_size_;
110 }
111
112 void PointCloudVisualization::setPointSize(float point_size)
113 {
114     point_size_ = point_size;
115 }
116
117 void PointCloudVisualization::setReduction(double reduction)
118 {
119     if(reduction > 1. || reduction <= 0.){
120         LOG_ERROR("Reduction factor must be larger than 0.0 and smaller or equal to 1.0. Setting to 1.0");
121         reduction = 1.0;
122     }
123     reduction_ = reduction;
124     setDirty();
125 }
126
127 void PointCloudVisualization::updateDataIntern ( const base::samples::Pointcloud& data )
128 {
129     cloud_->clear();
130     int step = data.points.size()/(data.points.size()*reduction_);
131     for(uint i=0; i<data.points.size(); i=i+step){
132         cloud_->push_back(to_osg(data.points[i]));
133     }
134 }
135
136 void PointCloudVisualization::updateMainNode( osg::Node* node )
137 {
138     if(node->asGroup()->getNumChildren() > 0){
139         node->asGroup()->removeChildren(0, 1);
140     }
141
142     osg::Geode* geode = new osg::Geode;
143
144     osg::Geometry* osg_points_ = new osg::Geometry;
145     osg_points_->setVertexArray( cloud_ );
146     osg_points_->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,0,cloud_->size()));
147
148
149     osg::Vec4Array* colors = new osg::Vec4Array;
150     colors->push_back(color_);
151
152     osg_points_->setColorArray(colors);
153     osg_points_->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE_SET);
154
155
156     osg::StateSet* stateset = node->getOrCreateStateSet();
157     //stateset->setMode(GL_VERTEX_PROGRAM_POINT_SIZE, osg::StateAttribute::ON);
158     stateset->setAttribute( new osg::Point( point_size_ ), osg::StateAttribute::ON);
159     ///////////////////////////////////////////////////////////////////
160     // vertex shader using just Vec4 coefficients
161     char vertexShaderSource[] =
162             "void main(void) \n"
163             "{ \n"
164             "\n"
165             "    gl_FrontColor = gl_Color;\n"
166             "    gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;\n"
167             "}\n";
168
169
170
171     osg::Program* program = new osg::Program;
172     stateset->setAttribute(program);
173
174     osg::Shader* vertex_shader = new osg::Shader(osg::Shader::VERTEX, vertexShaderSource);
175     program->addShader(vertex_shader);
176     //std::cout << cloud_->size() << std::endl;
177     geode->addDrawable(osg_points_);
178     node->asGroup()->addChild(geode);
179 }
180 //Macro that makes this plugin loadable in ruby, this is optional.
181 VizkitQtPlugin(PointCloudVisualization)
182 }
183