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 | |
---|
36 | using namespace irr::video; |
---|
37 | using namespace irr::scene; |
---|
38 | using namespace irr::core; |
---|
39 | using namespace flair::core; |
---|
40 | using namespace flair::simulator; |
---|
41 | using namespace flair::gui; |
---|
42 | |
---|
43 | UavVrpnObject_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 | |
---|
58 | UavVrpnObject_impl::~UavVrpnObject_impl() { |
---|
59 | // les objets irrlicht seront automatiquement detruits (moteurs, helices, |
---|
60 | // pales) par parenté |
---|
61 | } |
---|
62 | |
---|
63 | void 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 | |
---|
105 | void UavVrpnObject_impl::render(void) { |
---|
106 | IVideoDriver *driver = SceneManager->getVideoDriver(); |
---|
107 | driver->setTransform(ETS_WORLD, AbsoluteTransformation); |
---|
108 | } |
---|
109 | |
---|
110 | void UavVrpnObject_impl::OnRegisterSceneNode(void) { |
---|
111 | if (IsVisible) |
---|
112 | SceneManager->registerNodeForRendering(this); |
---|
113 | |
---|
114 | ISceneNode::OnRegisterSceneNode(); |
---|
115 | } |
---|
116 | |
---|
117 | void 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), vector3df(0, 0, 0),true); |
---|
153 | rl_blade = new Blade(this, vector3df(-70.71, -70.71, 17.5), vector3df(0, 0, 0),true); |
---|
154 | rr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5)); |
---|
155 | |
---|
156 | fl_blade->SetRotationSpeed(vector3df(0, 0,MOTOR_SPEED)); |
---|
157 | fr_blade->SetRotationSpeed(vector3df(0, 0,-MOTOR_SPEED)); |
---|
158 | rl_blade->SetRotationSpeed(vector3df(0, 0,-MOTOR_SPEED)); |
---|
159 | rr_blade->SetRotationSpeed(vector3df(0, 0,MOTOR_SPEED)); |
---|
160 | |
---|
161 | setScale(vector3df(arm_length->Value(), arm_length->Value(), arm_length->Value())); |
---|
162 | } |
---|
163 | #endif // GL |
---|