1 | // %flair:license{
|
---|
2 | // This file is part of the Flair framework distributed under the
|
---|
3 | // CECILL-C License, Version 1.0.
|
---|
4 | // %flair:license}
|
---|
5 | // created: 2014/02/07
|
---|
6 | // filename: SimuUsGL.cpp
|
---|
7 | //
|
---|
8 | // author: Guillaume Sanahuja
|
---|
9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
10 | //
|
---|
11 | // version: $Id: $
|
---|
12 | //
|
---|
13 | // purpose: Class for a simulation us
|
---|
14 | //
|
---|
15 | //
|
---|
16 | /*********************************************************************/
|
---|
17 | #ifdef GL
|
---|
18 |
|
---|
19 | #include "SimuLaserGL.h"
|
---|
20 | #include "Model.h"
|
---|
21 | #include "Gui.h"
|
---|
22 | #include <cvmatrix.h>
|
---|
23 | #include <SharedMem.h>
|
---|
24 | #include <TabWidget.h>
|
---|
25 | #include <Tab.h>
|
---|
26 | #include <DoubleSpinBox.h>
|
---|
27 | #include <Vector3DSpinBox.h>
|
---|
28 | #include <ISceneNode.h>
|
---|
29 | #include <ISceneCollisionManager.h>
|
---|
30 |
|
---|
31 | using namespace irr;
|
---|
32 | using namespace irr::scene;
|
---|
33 | using namespace irr::core;
|
---|
34 | using namespace flair::core;
|
---|
35 | using namespace flair::gui;
|
---|
36 | using namespace flair::simulator;
|
---|
37 |
|
---|
38 | namespace flair {
|
---|
39 | namespace sensor {
|
---|
40 | SimuLaserGL::SimuLaserGL(const Model *parent, std::string name, uint32_t modelId,uint32_t deviceId)
|
---|
41 | : SimuLaser(parent, name, modelId,deviceId), SensorGL(parent) {
|
---|
42 | Tab *setup_tab = new Tab(parent->GetTabWidget(), name);
|
---|
43 | position = new Vector3DSpinBox(setup_tab->NewRow(), "position", -2, 2, .01);
|
---|
44 | direction = new Vector3DSpinBox(setup_tab->NewRow(), "direction", -2, 2, .01);
|
---|
45 | range = new DoubleSpinBox(setup_tab->NewRow(), "range:", 0, 30, 1);
|
---|
46 | }
|
---|
47 |
|
---|
48 | SimuLaserGL::~SimuLaserGL() {}
|
---|
49 |
|
---|
50 | void SimuLaserGL::UpdateFrom(const io_data *data) {
|
---|
51 | float value[360];
|
---|
52 |
|
---|
53 | if (noGui() == false && data == NULL) {
|
---|
54 | for (int i = 0; i < 360; i++) {
|
---|
55 | line3d<f32> ray_laser; // rayon provenant de l'ultra son
|
---|
56 | vector3df intersection_laser; // point intersection us avec le sol
|
---|
57 | triangle3df hitTriangle_laser; // triangle intersection us avec le sol
|
---|
58 |
|
---|
59 | // get rotation matrix of node - Zeuss must be getRotation not
|
---|
60 | // getRelativeTransformation
|
---|
61 | matrix4 m;
|
---|
62 | matrix4 M;
|
---|
63 | m.setRotationDegrees(Node()->getRotation());
|
---|
64 |
|
---|
65 | // Matrice de rotation pour balayage du laser, angle i
|
---|
66 | M.setRotationDegrees(vector3df(0, 0, i));
|
---|
67 | // transform forward vector of us
|
---|
68 | vector3df frv = ToIrrlichtCoordinates(direction->Value());
|
---|
69 | M.transformVect(frv);
|
---|
70 | m.transformVect(frv);
|
---|
71 | frv.normalize();
|
---|
72 |
|
---|
73 | // transform pos vector of us
|
---|
74 | vector3df pos = ToIrrlichtCoordinates(position->Value());
|
---|
75 | m.transformVect(pos);
|
---|
76 |
|
---|
77 | ray_laser.start = Node()->getPosition() + pos;
|
---|
78 | ray_laser.end = ray_laser.start + ToIrrlichtScale(range->Value()) * frv;
|
---|
79 |
|
---|
80 | scene::ISceneNode *selectedSceneNode =
|
---|
81 | CollMan()->getSceneNodeAndCollisionPointFromRay(
|
---|
82 | ray_laser, intersection_laser, hitTriangle_laser);
|
---|
83 | // //////////////////////////////////////////
|
---|
84 | if (selectedSceneNode) //
|
---|
85 | {
|
---|
86 | value[i] = ToSimulatorScale(
|
---|
87 | ray_laser.start.getDistanceFrom(intersection_laser));
|
---|
88 | } else {
|
---|
89 | value[i] = -1;
|
---|
90 | }
|
---|
91 | }
|
---|
92 | shmem->Write((char *)value, 360 * sizeof(float));
|
---|
93 | }
|
---|
94 | }
|
---|
95 |
|
---|
96 | } // end namespace sensor
|
---|
97 | } // end namespace flair
|
---|
98 | #endif
|
---|