source: flair-src/trunk/lib/FlairSimulator/src/SimuLaserGL.cpp @ 15

Last change on this file since 15 was 15, checked in by Bayard Gildas, 5 years ago

sources reformatted with flair-format-dir script

File size: 3.0 KB
Line 
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
31using namespace irr;
32using namespace irr::scene;
33using namespace irr::core;
34using namespace flair::core;
35using namespace flair::gui;
36using namespace flair::simulator;
37
38namespace flair {
39namespace sensor {
40SimuLaserGL::SimuLaserGL(const Model *parent, std::string name, int dev_id)
41    : SimuLaser(parent, name, dev_id), 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
48SimuLaserGL::~SimuLaserGL() {}
49
50void 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
Note: See TracBrowser for help on using the repository browser.