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

Last change on this file since 299 was 289, checked in by Sanahuja Guillaume, 6 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.