[286] | 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 |
|
---|
[289] | 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 |
|
---|
[286] | 89 | vector3df nodePosition;
|
---|
| 90 | Quaternion nodeQuaternion;
|
---|
| 91 | Euler nodeEuler;
|
---|
| 92 |
|
---|
[289] | 93 | //transform form earth to irrlicht
|
---|
[286] | 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));
|
---|
[339] | 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);
|
---|
[286] | 154 | rr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5));
|
---|
| 155 |
|
---|
[339] | 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));
|
---|
[286] | 160 |
|
---|
| 161 | setScale(vector3df(arm_length->Value(), arm_length->Value(), arm_length->Value()));
|
---|
| 162 | }
|
---|
| 163 | #endif // GL |
---|