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

// %flair:license{

// This file is part of the Flair framework distributed under the

// CECILL-C License, Version 1.0.

// %flair:license}

//  created:    2018/12/11

//  filename:   UavVrpnObject_impl.cpp

//

//  author:     Guillaume Sanahuja

//              Copyright Heudiasyc UMR UTC/CNRS 7253

//

//  version:    $Id: $

//

//  purpose:    uav vrpn object, can display a real vrpn object in a simulated environment

//

/*********************************************************************/

#ifdef GL



#include "UavVrpnObject_impl.h"

#include "UavVrpnObject.h"

#include <TabWidget.h>

#include <Tab.h>

#include <DoubleSpinBox.h>

#include "Simulator.h"

#include <ISceneManager.h>

#include <IVideoDriver.h>

#include "Blade.h"

#include "MeshSceneNode.h"

#include "Gui.h"

#include "FollowMeCamera.h"

#include <Matrix.h>

#include <Quaternion.h>

#include <Euler.h>



#define MOTOR_SPEED 100



using namespace irr::video;

using namespace irr::scene;

using namespace irr::core;

using namespace flair::core;

using namespace flair::simulator;

using namespace flair::gui;

  

UavVrpnObject_impl::UavVrpnObject_impl(UavVrpnObject *self,std::string name) 

  : IODevice(self,name),

    ISceneNode(getGui()->getSceneManager()->getRootSceneNode(), getGui()->getSceneManager(), -1) {

  this->self=self;

  

  // init user interface

  Tab *tab = new Tab(getSimulator()->GetTabWidget(), ObjectName());

  arm_length = new DoubleSpinBox(tab->LastRowLastCol(), "arm length (m):",0, 2, 0.1);



  // camera

  FollowMeCamera* camera = new FollowMeCamera(this,name);

  

  Draw();

}



UavVrpnObject_impl::~UavVrpnObject_impl() {

  // les objets irrlicht seront automatiquement detruits (moteurs, helices,

  // pales) par parenté

}



void UavVrpnObject_impl::UpdateFrom(const io_data *data) {

  const Matrix* input = dynamic_cast<const Matrix*>(data);

  if (!input) {

      self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());

      return;

  }

  

  Vector3Df vrpnPosition;

  Quaternion vrpnQuaternion;

  input->GetMutex();

  vrpnQuaternion.q0=input->ValueNoMutex(0, 0);

  vrpnQuaternion.q1=input->ValueNoMutex(1, 0);

  vrpnQuaternion.q2=input->ValueNoMutex(2, 0);

  vrpnQuaternion.q3=input->ValueNoMutex(3, 0);

  vrpnPosition.x=input->ValueNoMutex(4, 0);

  vrpnPosition.y=input->ValueNoMutex(5, 0);

  vrpnPosition.z=input->ValueNoMutex(6, 0);

  input->ReleaseMutex();

  

  vector3df nodePosition;

  Quaternion nodeQuaternion;

  Euler nodeEuler;



  nodePosition = ToIrrlichtCoordinates(vrpnPosition);

  setPosition(nodePosition);

  nodeQuaternion = ToIrrlichtOrientation(vrpnQuaternion);

  nodeQuaternion.ToEuler(nodeEuler);

  setRotation(Euler::ToDegree(1) * vector3df(nodeEuler.roll,nodeEuler.pitch, nodeEuler.yaw));

  

  if (arm_length->ValueChanged() == true) {

    setScale(vector3df(arm_length->Value(), arm_length->Value(), arm_length->Value()));

  }

}



void UavVrpnObject_impl::render(void) {

  IVideoDriver *driver = SceneManager->getVideoDriver();

  driver->setTransform(ETS_WORLD, AbsoluteTransformation);

}



void UavVrpnObject_impl::OnRegisterSceneNode(void) {

  if (IsVisible)

    SceneManager->registerNodeForRendering(this);



  ISceneNode::OnRegisterSceneNode();

}



void UavVrpnObject_impl::Draw(void) {

  // create unite (1m=100cm) UAV; scale will be adapted according to arm_length

  // parameter

  // note that the frame used is irrlicht one:

  // left handed, North East Up

  const IGeometryCreator *geo;

  geo = getGui()->getSceneManager()->getGeometryCreator();



  // cylinders are aligned with y axis

  IMesh *red_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 255, 0, 0));

  IMesh *black_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 128, 128, 128));

  IMesh *motor = geo->createCylinderMesh(7.5, 15, 16); //,SColor(0, 128, 128, 128));

  // geo->drop();



  ITexture *texture = getGui()->getTexture("carbone.jpg");

  MeshSceneNode *fl_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),

                             vector3df(0, 0, -135));

  MeshSceneNode *fr_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),

                             vector3df(0, 0, -45));

  MeshSceneNode *rl_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),

                             vector3df(0, 0, 135), texture);

  MeshSceneNode *rr_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),

                             vector3df(0, 0, 45), texture);



  texture = getGui()->getTexture("metal047.jpg");

  MeshSceneNode *fl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, 2.5),

                               vector3df(90, 0, 0), texture);

  MeshSceneNode *fr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, 2.5),

                               vector3df(90, 0, 0), texture);

  MeshSceneNode *rl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, 2.5),

                               vector3df(90, 0, 0), texture);

  MeshSceneNode *rr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, 2.5),

                               vector3df(90, 0, 0), texture);



  fl_blade = new Blade(this, vector3df(70.71, -70.71, 17.5));

  fr_blade = new Blade(this, vector3df(70.71, 70.71, 17.5), true);

  rl_blade = new Blade(this, vector3df(-70.71, -70.71, 17.5), true);

  rr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5));

  

  fl_blade->SetRotationSpeed(MOTOR_SPEED);

  fr_blade->SetRotationSpeed(-MOTOR_SPEED);

  rl_blade->SetRotationSpeed(-MOTOR_SPEED);

  rr_blade->SetRotationSpeed(MOTOR_SPEED);

  

  setScale(vector3df(arm_length->Value(), arm_length->Value(), arm_length->Value()));

}

#endif // GL