source: flair-src/trunk/lib/FlairSimulator/src/SimuUsGL.cpp @ 158

Last change on this file since 158 was 158, checked in by Sanahuja Guillaume, 5 years ago

corrected simu/device id for sensors

File size: 2.9 KB
RevLine 
[10]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[10]4// %flair:license}
[8]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#include "SimuUsGL.h"
18#include "Model.h"
19#include <cvmatrix.h>
20#include <SharedMem.h>
21#include <TabWidget.h>
22#include <Tab.h>
23#include <DoubleSpinBox.h>
24#include <Vector3DSpinBox.h>
25
26#ifdef GL
27#include "Gui.h"
28#include <ISceneNode.h>
29#include <ISceneCollisionManager.h>
30using namespace irr;
31using namespace irr::scene;
32using namespace irr::core;
33#endif
34using namespace flair::core;
35using namespace flair::gui;
36using namespace flair::simulator;
37
[15]38namespace flair {
39namespace sensor {
[158]40SimuUsGL::SimuUsGL(const Model *parent, std::string name, uint32_t modelId,uint32_t deviceId)
41    : SimuUs(parent, name, modelId,deviceId), SensorGL(parent) {
[15]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, 6, 1);
[8]46}
47
[15]48SimuUsGL::~SimuUsGL() {}
[8]49
[15]50void SimuUsGL::UpdateFrom(const io_data *data) {
51  float value;
[8]52#ifdef GL
[15]53  if (noGui() == true) {
[8]54#endif
[15]55    // todo: utiliser le placement de l'us dans le drone et sa portée
56    cvmatrix *input = (cvmatrix *)data;
57    value = input->Value(9, 0);
58    shmem->Write((char *)&value, sizeof(float));
[8]59#ifdef GL
[15]60  }
[8]61
[15]62  if (noGui() == false && data == NULL) {
63    line3d<f32> ray_us;         // rayon provenant de l'ultra son
64    vector3df intersection_us;  // point intersection us avec le sol
65    triangle3df hitTriangle_us; // triangle intersection us avec le sol
[8]66
[15]67    // get rotation matrix of node - Zeuss must be getRotation not
68    // getRelativeTransformation
69    matrix4 m;
70    m.setRotationDegrees(Node()->getRotation());
[8]71
[15]72    // transform forward vector of us
73    vector3df frv = ToIrrlichtCoordinates(direction->Value());
74    m.transformVect(frv);
75    frv.normalize();
[8]76
[15]77    // transform pos vector of us
78    vector3df pos = ToIrrlichtCoordinates(position->Value());
79    m.transformVect(pos);
[8]80
[15]81    ray_us.start = Node()->getPosition() + pos;
82    ray_us.end = ray_us.start + ToIrrlichtScale(range->Value()) * frv;
83
84    scene::ISceneNode *selectedSceneNode =
85        CollMan()->getSceneNodeAndCollisionPointFromRay(ray_us, intersection_us,
86                                                        hitTriangle_us);
87
88    if (selectedSceneNode) {
89      float value =
90          ToSimulatorScale(ray_us.start.getDistanceFrom(intersection_us));
91      shmem->Write((char *)&value, sizeof(float));
[8]92    }
[15]93  }
[8]94#endif
95}
96
97} // end namespace sensor
98} // end namespace flair
Note: See TracBrowser for help on using the repository browser.