?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
LP-500 add hottbridgestatus uavobjct to GCS
[librepilot.git] / ground / gcs / src / libs / osgearth / utils / shapeutils.cpp
blob6cd6bfb8dad4bf7ba8d56a0ea34e0bb8397d92f0
1 #include "utils/shapeutils.h"
2
3 #include <math.h>
4
5 #include <osg/Geode>
6 #include <osg/Group>
7 #include <osg/PositionAttitudeTransform>
8 #include <osg/Geometry>
9 #include <osg/Shape>
10 #include <osg/ShapeDrawable>
11 #include <osg/Vec3>
12 #include <osg/Vec4>
13 #include <osg/ShadeModel>
14
15 namespace ShapeUtils {
16 osg::Geode *createSphere(const osg::Vec4 &color, float radius)
17 {
18 osg::TessellationHints *sphereHints = new osg::TessellationHints;
19 // sphereHints->setDetailRatio(0.1f);
20 // sphereHints->setCreateBody(true);
21 osg::Sphere *sphere = new osg::Sphere(osg::Vec3(0, 0, 0), radius);
22 osg::ShapeDrawable *sphereDrawable = new osg::ShapeDrawable(sphere, sphereHints);
23
24 sphereDrawable->setColor(color);
25
26 osg::Geode *geode = new osg::Geode;
27 geode->addDrawable(sphereDrawable);
28
29 return geode;
30 }
31
32 osg::Geode *createCube()
33 {
34 // Declare a group to act as root node of a scene:
35 // osg::Group* root = new osg::Group();
36
37 // Declare a box class (derived from shape class) instance
38 // This constructor takes an osg::Vec3 to define the center
39 // and a float to define the height, width and depth.
40 // (an overloaded constructor allows you to specify unique
41 // height, width and height values.)
42 osg::Box *unitCube = new osg::Box(osg::Vec3(0, 0, 0), 1.0f);
43
44 // Declare an instance of the shape drawable class and initialize
45 // it with the unitCube shape we created above.
46 // This class is derived from 'drawable' so instances of this
47 // class can be added to Geode instances.
48 osg::ShapeDrawable *unitCubeDrawable = new osg::ShapeDrawable(unitCube);
49
50 // Declare a instance of the geode class:
51 osg::Geode *geode = new osg::Geode();
52
53 // Add the unit cube drawable to the geode:
54 geode->addDrawable(unitCubeDrawable);
55
56 // osg::PolygonMode *pm = new osg::PolygonMode();
57 // pm->setMode( osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE );
58 //
59 // osg::StateSet *stateSet = new osg::StateSet();
60 // stateSet->setAttributeAndModes( pm,
61 // osg::StateAttribute::OVERRIDE | osg::StateAttribute::ON );
62 // geode->setStateSet(stateSet);
63
64 // Add the geode to the scene:
65 return geode;
66 }
67
68 osg::PositionAttitudeTransform *createArrow(const osg::Vec4 &color)
69 {
70 osg::ref_ptr<osg::Geode> geode = new osg::Geode;
71
72 double radius = 0.04;
73
74 osg::TessellationHints *cylinderHints = new osg::TessellationHints;
75
76 cylinderHints->setDetailRatio(0.1f);
77 cylinderHints->setCreateBody(true);
78 // cylinderHints->setCreateFrontFace(false);
79 // cylinderHints->setCreateBackFace(false);
80 osg::Cylinder *cylinder = new osg::Cylinder(osg::Vec3(0, 0, 0.4), radius, 0.8);
81 // cylinder->setRotation(quat);
82 osg::ShapeDrawable *cylinderDrawable = new osg::ShapeDrawable(cylinder, cylinderHints);
83 cylinderDrawable->setColor(color);
84 geode->addDrawable(cylinderDrawable);
85
86 osg::TessellationHints *coneHints = new osg::TessellationHints;
87 coneHints->setDetailRatio(0.5f);
88 coneHints->setCreateBottom(true);
89 osg::Cone *cone = new osg::Cone(osg::Vec3(0, 0, 0.8), radius * 2, 0.2);
90 // cone->setRotation(quat);
91 osg::ShapeDrawable *coneDrawable = new osg::ShapeDrawable(cone, coneHints);
92 coneDrawable->setColor(color);
93 geode->addDrawable(coneDrawable);
94
95 osg::PositionAttitudeTransform *transform = new osg::PositionAttitudeTransform();
96 transform->addChild(geode);
97
98 return transform;
99 }
100
101 osg::Node *create3DAxis()
102 {
103 osg::PositionAttitudeTransform *xAxis = createArrow(osg::Vec4(1, 0, 0, 1));
104
105 xAxis->setAttitude(osg::Quat(M_PI / 2.0, osg::Vec3(0, 1, 0)));
106
107 osg::PositionAttitudeTransform *yAxis = createArrow(osg::Vec4(0, 1, 0, 1));
108 yAxis->setAttitude(osg::Quat(-M_PI / 2.0, osg::Vec3(1, 0, 0)));
109
110 osg::PositionAttitudeTransform *zAxis = createArrow(osg::Vec4(0, 0, 1, 1));
111
112 osg::Node *center = createSphere(osg::Vec4(0.7, 0.7, .7, 1), 0.08);
113
114 osg::Group *group = new osg::Group();
115 group->addChild(xAxis);
116 group->addChild(yAxis);
117 group->addChild(zAxis);
118 group->addChild(center);
119 return group;
120 }
121
122 osg::Node *createOrientatedTorus(float innerRadius, float outerRadius)
123 {
124 osg::Node *node = createTorus(innerRadius, outerRadius, 64, 32);
125
126 osg::PositionAttitudeTransform *transform = new osg::PositionAttitudeTransform();
127
128 transform->addChild(node);
129
130 osg::Quat q = osg::Quat(
131 M_PI / 2, osg::Vec3d(1, 0, 0),
132 0, osg::Vec3d(0, 1, 0),
133 0, osg::Vec3d(0, 0, 1));
134 transform->setAttitude(q);
135
136 // transform->getOrCreateStateSet()->setMode(GL_NORMALIZE, osg::StateAttribute::ON);
137 // node->getOrCreateStateSet()->setMode(GL_RESCALE_NORMAL, osg::StateAttribute::ON);
138
139 return transform;
140 }
141
142 osg::Geode *createTorus(float innerRadius, float outerRadius, float sweepCuts, float sphereCuts)
143 {
144 osg::ref_ptr<osg::Geode> geode = new osg::Geode;
145
146 bool create_body = true;
147
148 if (create_body) {
149 float inner = innerRadius;
150 float outer = outerRadius;
151 float tube_radius = (outer - inner) * 0.5;
152 float avg_center = (inner + outer) * 0.5;
153
154 float start_sweep = 0; // this->getStartSweep();
155 float end_sweep = osg::DegreesToRadians(360.0); // this->getEndSweep();
156
157 int torus_sweeps = sweepCuts;
158 int sphere_sweeps = sphereCuts;
159
160 float dsweep = (end_sweep - start_sweep) / (float)torus_sweeps;
161 float dphi = osg::DegreesToRadians(360.0) / (float)sphere_sweeps;
162
163 for (int j = 0; j < sphere_sweeps; j++) {
164 osg::Vec3Array *vertices = new osg::Vec3Array;
165 osg::Vec3Array *normals = new osg::Vec3Array;
166
167 float phi = dphi * (float)j;
168 float cosPhi = cosf(phi);
169 float sinPhi = sinf(phi);
170 float next_cosPhi = cosf(phi + dphi);
171 float next_sinPhi = sinf(phi + dphi);
172
173 float z = tube_radius * sinPhi;
174 float yPrime = avg_center + tube_radius * cosPhi;
175
176 float next_z = tube_radius * next_sinPhi;
177 float next_yPrime = avg_center + tube_radius * next_cosPhi;
178
179 float old_x = yPrime * cosf(-dsweep);
180 float old_y = yPrime * sinf(-dsweep);
181 float old_z = z;
182
183 for (int i = 0; i < torus_sweeps; ++i) {
184 float sweep = start_sweep + dsweep * i;
185 float cosSweep = cosf(sweep);
186 float sinSweep = sinf(sweep);
187
188 float x = yPrime * cosSweep;
189 float y = yPrime * sinSweep;
190
191 float next_x = next_yPrime * cosSweep;
192 float next_y = next_yPrime * sinSweep;
193
194 vertices->push_back(osg::Vec3(next_x, next_y, next_z));
195 vertices->push_back(osg::Vec3(x, y, z));
196
197 // calculate normals
198 osg::Vec3 lateral(next_x - x, next_y - y, next_z - z);
199 osg::Vec3 longitudinal(x - old_x, y - old_y, z - old_z);
200 osg::Vec3 normal = longitudinal ^ lateral; // cross product
201 normal.normalize();
202
203 normals->push_back(normal);
204 normals->push_back(normal);
205
206 old_x = x;
207 old_y = y;
208 old_z = z;
209 } // end torus loop
210
211 // the last point
212 float last_sweep = start_sweep + end_sweep;
213 float cosLastSweep = cosf(last_sweep);
214 float sinLastSweep = sinf(last_sweep);
215
216 float x = yPrime * cosLastSweep;
217 float y = yPrime * sinLastSweep;
218
219 float next_x = next_yPrime * cosLastSweep;
220 float next_y = next_yPrime * sinLastSweep;
221
222 vertices->push_back(osg::Vec3(next_x, next_y, next_z));
223 vertices->push_back(osg::Vec3(x, y, z));
224
225 osg::Vec3 lateral(next_x - x, next_y - y, next_z - z);
226 osg::Vec3 longitudinal(x - old_x, y - old_y, z - old_z);
227 osg::Vec3 norm = longitudinal ^ lateral;
228 norm.normalize();
229
230 normals->push_back(norm);
231 normals->push_back(norm);
232
233 osg::ShadeModel *shademodel = new osg::ShadeModel;
234 shademodel->setMode(osg::ShadeModel::SMOOTH);
235
236 osg::StateSet *stateset = new osg::StateSet;
237 stateset->setAttribute(shademodel);
238
239 osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
240 geometry->setStateSet(stateset);
241
242 geometry->setVertexArray(vertices);
243
244 osg::Vec4Array *colors = new osg::Vec4Array;
245 colors->push_back(osg::Vec4(1.0, 1.0, 0.0, 1.0)); // this->getColor());
246 geometry->setColorArray(colors);
247 geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
248
249 geometry->setNormalArray(normals);
250 geometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX);
251
252 geometry->addPrimitiveSet(
253 new osg::DrawArrays(osg::PrimitiveSet::QUAD_STRIP, 0,
254 vertices->size()));
255 geode->addDrawable(geometry.get());
256 } // end cirle loop
257 } // endif create_body
258
259 return geode.release();
260 }
261
262 osg::Geode *createRhombicuboctahedron()
263 {
264 // Top *
265 // Bottom o
266 // Front ^
267 // Stern v
268 // Left <
269 // Right >
270 const double alpha = 1; // 0.65;
271 const osg::Vec4 colors[] = {
272 osg::Vec4(0.7, 0.5, 0.7, alpha), // TFR
273 osg::Vec4(0.9, 0.9, 0.9, alpha), // T
274 osg::Vec4(0.5, 0.7, 0.7, alpha), // TFL
275 osg::Vec4(1.0, 0.7, 0.7, alpha), // TS
276 osg::Vec4(0.7, 0.7, 1.0, alpha), // TF
277 osg::Vec4(0.5, 0.0, 0.7, alpha), // BFR
278 osg::Vec4(0.5, 0.5, 1.0, alpha), // F
279 osg::Vec4(0.1, 0.5, 0.7, alpha), // BFL
280 osg::Vec4(0.9, 0.5, 0.9, alpha), // TR
281 osg::Vec4(0.5, 0.0, 0.5, alpha), // R
282 osg::Vec4(1.0, 0.5, 0.7, alpha), // TSR
283 osg::Vec4(0.2, 0.0, 0.2, alpha), // BR
284 osg::Vec4(0.2, 0.2, 1.0, alpha), // BF
285 osg::Vec4(0.7, 0.0, 0.2, alpha), // BSR
286 osg::Vec4(0.1, 0.1, 0.1, alpha), // B
287 osg::Vec4(0.5, 0.5, 0.2, alpha), // BSL
288 osg::Vec4(0.9, 0.0, 0.4, alpha), // SR
289 osg::Vec4(1.0, 0.2, 0.2, alpha), // BS
290 osg::Vec4(1.0, 0.5, 0.5, alpha), // S
291 osg::Vec4(0.7, 0.7, 0.5, alpha), // SL
292 osg::Vec4(0.5, 0.9, 0.5, alpha), // TL
293 osg::Vec4(0.5, 0.7, 0.5, alpha), // L
294 osg::Vec4(1.0, 1.0, 0.7, alpha), // TSL
295 osg::Vec4(0.2, 0.5, 0.2, alpha), // BL
296 osg::Vec4(0.5, 0.0, 0.8, alpha), // FR
297 osg::Vec4(0.5, 0.7, 1.0, alpha) // FL
298 };
299
300 const double no = -1.0;
301 const double po = 1.0f;
302 const double ps = 1.0 + sqrt(1.0);
303 const double ns = -ps;
304 const osg::Vec3 vertices[] = {
305 osg::Vec3(po, po, ps),
306 osg::Vec3(po, no, ps),
307 osg::Vec3(no, po, ps),
308 osg::Vec3(no, no, ps),
309
310 osg::Vec3(po, ps, po),
311 osg::Vec3(po, ps, no),
312 osg::Vec3(no, ps, po),
313 osg::Vec3(no, ps, no),
314
315 osg::Vec3(ps, po, po),
316 osg::Vec3(ps, po, no),
317 osg::Vec3(ps, no, po),
318 osg::Vec3(ps, no, no),
319
320 osg::Vec3(po, po, ns),
321 osg::Vec3(po, no, ns),
322 osg::Vec3(no, po, ns),
323 osg::Vec3(no, no, ns),
324
325 osg::Vec3(po, ns, po),
326 osg::Vec3(po, ns, no),
327 osg::Vec3(no, ns, po),
328 osg::Vec3(no, ns, no),
329
330 osg::Vec3(ns, po, po),
331 osg::Vec3(ns, po, no),
332 osg::Vec3(ns, no, po),
333 osg::Vec3(ns, no, no),
334
335 // bonus vertices to map unique colors
336 // we have only 24 regular vertices, but 26 faces
337 osg::Vec3(ps, po, no), // copy from vertex 9
338 osg::Vec3(no, ps, no) // copy from vertex 7
339 };
340
341 const unsigned int indices[] = {
342 // PITCH ROLL YAW
343 8, 4, 0, // TFR 45 -45
344
345 0, 2, 1, // T1 0 0
346 3, 2, 1, // T2
347
348 6, 20, 2, // TFL 45 45
349
350 1, 16, 3, // TS1 -45 0
351 18, 16, 3, // TS2
352
353 0, 2, 4, // TF1 45 0
354 6, 2, 4, // TF2
355
356 12, 9, 5, // BFR 45 -135
357
358 4, 5, 6, // F1 90 X
359 7, 5, 6, // F2
360
361 14, 21, 7, // BFL 45 135
362
363 0, 1, 8, // TR1 0 -45
364 10, 1, 8, // TR2
365
366 8, 10, 9, // R1 0 -90
367 11, 10, 9, // R2
368
369 1, 16, 10, // TSR -45 -45
370 // PITCH ROLL YAW
371 13, 12, 11, // BR1 0 -135
372 9, 12, 11, // BR2
373
374 5, 7, 12, // BF1 45 +-180
375 14, 7, 12, // BF2
376
377 11, 17, 13, // BSR -45 -135
378
379 12, 13, 14, // B1 0 +-180
380 15, 13, 14, // B2
381
382 19, 23, 15, // BSL -45 135
383
384 11, 17, 16, // SR1 -45 -90
385 11, 10, 16, // SR2
386
387 13, 15, 17, // BS1 -45 +-180
388 19, 15, 17, // BS2
389
390 16, 17, 18, // S1 -90 X
391 19, 17, 18, // S2
392
393 18, 22, 19, // SL -45 90
394 22, 23, 19, // SL
395 // PITCH ROLL YAW
396 2, 3, 20, // TL1 0 45
397 22, 3, 20, // TL2
398
399 20, 22, 21, // L1 0 90
400 23, 22, 21, // L2
401
402 3, 18, 22, // TSL -45 45
403
404 14, 15, 23, // BL1 0 135
405 14, 21, 23, // BL2
406
407 // last vertex is equal to vertex 9 to map a unique color
408 4, 5, 24, // FR 45 -90
409 4, 8, 24, // FR
410
411 // last vertex is equal to vertex 7 to map a unique color
412 6, 20, 25, // FL 45 90
413 21, 20, 25 // FL
414 };
415
416 osg::ShadeModel *shademodel = new osg::ShadeModel;
417
418 shademodel->setMode(osg::ShadeModel::FLAT);
419
420 osg::StateSet *stateset = new osg::StateSet;
421 stateset->setAttribute(shademodel);
422
423 osg::ref_ptr<osg::Geometry> geom = new osg::Geometry;
424
425 geom->setStateSet(stateset);
426
427 unsigned int vertexCount = sizeof(vertices) / sizeof(vertices[0]);
428 osg::Vec3Array *vertexArray = new osg::Vec3Array(vertexCount, const_cast<osg::Vec3 *>(&vertices[0]));
429 geom->setVertexArray(vertexArray);
430
431 unsigned int indexCount = sizeof(indices) / sizeof(indices[0]);
432 geom->addPrimitiveSet(new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES, indexCount, indices));
433
434 unsigned int colorCount = sizeof(colors) / sizeof(colors[0]);
435 osg::Vec4Array *colorArray = new osg::Vec4Array(colorCount, const_cast<osg::Vec4 *>(&colors[0]));
436 geom->setColorArray(colorArray);
437 geom->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
438
439 // geometry->setNormalArray(normals);
440 // geometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX);
441
442 osg::ref_ptr<osg::Geode> geode = new osg::Geode;
443 geode->addDrawable(geom.get());
444
445 return geode.release();
446 }
447 }