source: flair-src/trunk/lib/FlairSimulator/src/UavVrpnObject_impl.cpp @ 289

Last change on this file since 289 was 289, checked in by Sanahuja Guillaume, 2 years ago

use yaw rotation of vrpn

File size: 5.8 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:    2018/12/11
6//  filename:   UavVrpnObject_impl.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    uav vrpn object, can display a real vrpn object in a simulated environment
14//
15/*********************************************************************/
16#ifdef GL
17
18#include "UavVrpnObject_impl.h"
19#include "UavVrpnObject.h"
20#include <TabWidget.h>
21#include <Tab.h>
22#include <DoubleSpinBox.h>
23#include "Simulator.h"
24#include <ISceneManager.h>
25#include <IVideoDriver.h>
26#include "Blade.h"
27#include "MeshSceneNode.h"
28#include "Gui.h"
29#include "FollowMeCamera.h"
30#include <Matrix.h>
31#include <Quaternion.h>
32#include <Euler.h>
33
34#define MOTOR_SPEED 100
35
36using namespace irr::video;
37using namespace irr::scene;
38using namespace irr::core;
39using namespace flair::core;
40using namespace flair::simulator;
41using namespace flair::gui;
42 
43UavVrpnObject_impl::UavVrpnObject_impl(UavVrpnObject *self,std::string name) 
44  : IODevice(self,name),
45    ISceneNode(getGui()->getSceneManager()->getRootSceneNode(), getGui()->getSceneManager(), -1) {
46  this->self=self;
47 
48  // init user interface
49  Tab *tab = new Tab(getSimulator()->GetTabWidget(), ObjectName());
50  arm_length = new DoubleSpinBox(tab->LastRowLastCol(), "arm length (m):",0, 2, 0.1);
51
52  // camera
53  FollowMeCamera* camera = new FollowMeCamera(this,name);
54 
55  Draw();
56}
57
58UavVrpnObject_impl::~UavVrpnObject_impl() {
59  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
60  // pales) par parenté
61}
62
63void UavVrpnObject_impl::UpdateFrom(const io_data *data) {
64  const Matrix* input = dynamic_cast<const Matrix*>(data);
65  if (!input) {
66      self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
67      return;
68  }
69 
70  Vector3Df vrpnPosition;
71  Quaternion vrpnQuaternion;
72  input->GetMutex();
73  vrpnQuaternion.q0=input->ValueNoMutex(0, 0);
74  vrpnQuaternion.q1=input->ValueNoMutex(1, 0);
75  vrpnQuaternion.q2=input->ValueNoMutex(2, 0);
76  vrpnQuaternion.q3=input->ValueNoMutex(3, 0);
77  vrpnPosition.x=input->ValueNoMutex(4, 0);
78  vrpnPosition.y=input->ValueNoMutex(5, 0);
79  vrpnPosition.z=input->ValueNoMutex(6, 0);
80  input->ReleaseMutex();
81 
82  //use yaw rotation from earth to vrpn
83  Quaternion yaw_rot_quat;
84  Euler yaw_rot_euler(0, 0, getSimulator()->Yaw()); // yaw_rad is vrpn rotation in earth reference
85  yaw_rot_euler.ToQuaternion(yaw_rot_quat);
86  vrpnPosition.Rotate(yaw_rot_quat);
87  vrpnQuaternion= yaw_rot_quat * vrpnQuaternion;
88 
89  vector3df nodePosition;
90  Quaternion nodeQuaternion;
91  Euler nodeEuler;
92
93  //transform form earth to irrlicht
94  nodePosition = ToIrrlichtCoordinates(vrpnPosition);
95  setPosition(nodePosition);
96  nodeQuaternion = ToIrrlichtOrientation(vrpnQuaternion);
97  nodeQuaternion.ToEuler(nodeEuler);
98  setRotation(Euler::ToDegree(1) * vector3df(nodeEuler.roll,nodeEuler.pitch, nodeEuler.yaw));
99 
100  if (arm_length->ValueChanged() == true) {
101    setScale(vector3df(arm_length->Value(), arm_length->Value(), arm_length->Value()));
102  }
103}
104
105void UavVrpnObject_impl::render(void) {
106  IVideoDriver *driver = SceneManager->getVideoDriver();
107  driver->setTransform(ETS_WORLD, AbsoluteTransformation);
108}
109
110void UavVrpnObject_impl::OnRegisterSceneNode(void) {
111  if (IsVisible)
112    SceneManager->registerNodeForRendering(this);
113
114  ISceneNode::OnRegisterSceneNode();
115}
116
117void UavVrpnObject_impl::Draw(void) {
118  // create unite (1m=100cm) UAV; scale will be adapted according to arm_length
119  // parameter
120  // note that the frame used is irrlicht one:
121  // left handed, North East Up
122  const IGeometryCreator *geo;
123  geo = getGui()->getSceneManager()->getGeometryCreator();
124
125  // cylinders are aligned with y axis
126  IMesh *red_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 255, 0, 0));
127  IMesh *black_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 128, 128, 128));
128  IMesh *motor = geo->createCylinderMesh(7.5, 15, 16); //,SColor(0, 128, 128, 128));
129  // geo->drop();
130
131  ITexture *texture = getGui()->getTexture("carbone.jpg");
132  MeshSceneNode *fl_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
133                             vector3df(0, 0, -135));
134  MeshSceneNode *fr_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
135                             vector3df(0, 0, -45));
136  MeshSceneNode *rl_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
137                             vector3df(0, 0, 135), texture);
138  MeshSceneNode *rr_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
139                             vector3df(0, 0, 45), texture);
140
141  texture = getGui()->getTexture("metal047.jpg");
142  MeshSceneNode *fl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, 2.5),
143                               vector3df(90, 0, 0), texture);
144  MeshSceneNode *fr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, 2.5),
145                               vector3df(90, 0, 0), texture);
146  MeshSceneNode *rl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, 2.5),
147                               vector3df(90, 0, 0), texture);
148  MeshSceneNode *rr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, 2.5),
149                               vector3df(90, 0, 0), texture);
150
151  fl_blade = new Blade(this, vector3df(70.71, -70.71, 17.5));
152  fr_blade = new Blade(this, vector3df(70.71, 70.71, 17.5), true);
153  rl_blade = new Blade(this, vector3df(-70.71, -70.71, 17.5), true);
154  rr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5));
155 
156  fl_blade->SetRotationSpeed(MOTOR_SPEED);
157  fr_blade->SetRotationSpeed(-MOTOR_SPEED);
158  rl_blade->SetRotationSpeed(-MOTOR_SPEED);
159  rr_blade->SetRotationSpeed(MOTOR_SPEED);
160 
161  setScale(vector3df(arm_length->Value(), arm_length->Value(), arm_length->Value()));
162}
163#endif // GL
Note: See TracBrowser for help on using the repository browser.