exampleGimbalPlugin

Example Source Files

---

DtGimbalDriver.h

/*****************************************************************************
 * Copyright (c) 2017 MAK Technologies, Inc.
 * All rights reserved.
 *****************************************************************************/


#pragma once

#include <vrvCore/DtDriver.h>

#include <vrvUtil/DtSignalConnectionManager.h>

#include <string>


namespace makVrv
{
   class DtGeoReference;
   class DtSceneObjectAgent;
   class DtSceneObject;
}

class DtGeodeticCoord;

class DtGimbalDriver : public makVrv::DtDriver
{
public:

   DtGimbalDriver( makVrv::DtAgentManager& am );

   virtual ~DtGimbalDriver();


   virtual const std::string& className() const;

 protected:
   virtual bool onStart();

   virtual bool onStop();

   virtual bool onTick();

   virtual void attachObserverToEntity( const std::string& entityName );

   virtual void slot_displayEngineAdded( const makVrv::DtDeRecord& igRecord );

   virtual bool initialize();

   virtual void slot_coordinateSystemChanged();


private:
   DtGimbalDriver();

protected:
   makVrv::DtGeoReference* myPlaneGeoRef;
   makVrv::DtGeoReference* myGimbalGeoRef;
   makVrv::DtSceneObjectAgent* myGimbalSceneObjectAgent;
   makVrv::DtSceneObject* myPlaneSceneObject;
   makVrv::DtElementID myGimbalId;
   makVrv::DtSignalConnectionManager mySignalConnections;
   DtGeodeticCoord* myTargetLocation;
};

DtGimbalDriver.cxx

/*****************************************************************************
 * Copyright (c) 2017 MAK Technologies, Inc.
 * All rights reserved.
 *****************************************************************************/


#include "DtGimbalDriver.h"

#include <vrvCore/DtAgentManager.h>
#include <vrvCore/DtAgentUpdateResolverInterface.h>
#include <vrvCore/DtDataBank.h>
#include <vrvCore/DtDe.h>
#include <vrvCore/DtDeSharedState.h>
#include <vrvCore/DtDeSharedStateSignaler.h>
#include <vrvCore/DtDriverManager.h>
#include <vrvCore/DtElementAttributes.h>
#include <vrvCore/DtElementData.h>
#include <vrvCore/DtInputDriver.h>
#include <vrvCore/DtObserver.h>
#include <vrvCore/DtScene.h>
#include <vrvCore/DtSceneObject.h>
#include <vrvCore/DtSceneObjectAgent.hpp>

#include <vrvUtil/coordUtils.h>
#include <vrvUtil/DtCoordinateSystem.h>
#include <vrvUtil/DtGeoReference.h>

#include <vlutil/vlUtil.h>

// Use the VR-Vantage namespace.
// All classes in VR-Vantage are in this namespace.
using namespace makVrv;

const double DtTargetLatitude = DtDeg2Rad(10.0327245110);
const double DtTargetLongitude = DtDeg2Rad(60.0271819335);
const double DtTargetAltitude = 29.389;

const double DtGimbalForwardOffset = 2.0;
const double DtGimbalDownOffset = 2.5;
const double DtGimbalRightOffset = 0.;

DtGimbalDriver::DtGimbalDriver( DtAgentManager& am )
   : makVrv::DtDriver( am, "DtGimbalDriver" )
   , myPlaneGeoRef( 0 )
   , myGimbalGeoRef( 0 )
   , myGimbalSceneObjectAgent( 0 )
   , myTargetLocation( 0 )
{
   myTargetLocation = new DtGeodeticCoord( DtTargetLatitude, DtTargetLongitude, DtTargetAltitude,
      new DtReferenceEllipsoid( myAgentManager.de().sharedState().coordinateSystem().datum() ) );

   mySignalConnections += DtDeSharedStateSignaler::instance( myAgentManager.de() )
      .signal_coordinateSystemChanged.connect( boost::bind(
         &DtGimbalDriver::slot_coordinateSystemChanged, this));
}

DtGimbalDriver::~DtGimbalDriver()
{
   DtGimbalDriver::onStop();
   if ( myGimbalSceneObjectAgent )
   {
      reportElementDestroyed( myGimbalId );
   }

   DtDELETE( myPlaneGeoRef );
   DtDELETE( myGimbalGeoRef );
   DtDELETE( myGimbalSceneObjectAgent );
   DtDELETE( myTargetLocation );
}

void DtGimbalDriver::slot_coordinateSystemChanged()
{
   DtDELETE( myTargetLocation );

   myTargetLocation = new DtGeodeticCoord( DtTargetLatitude, DtTargetLongitude, DtTargetAltitude,
      new DtReferenceEllipsoid( myAgentManager.de().sharedState().coordinateSystem().datum() ) );
}

const std::string& DtGimbalDriver::className() const
{
   static std::string name = "DtGimbalDriver";
   return name;
}

bool DtGimbalDriver::initialize()
{
   // look up the plane from exampleDriver (or exampleDriverPlugin)
   // It has display name "Entity From Facade"
   bool found = false;

   // Iterate through all the elements
   DtElementData& elemData = myAgentManager.de().dataBank().elementData();
   DtElementData::ElementEntryMap::const_iterator curIter = 
      elemData.elements().begin();
   DtElementData::ElementEntryMap::const_iterator endIter = 
      elemData.elements().end();
   for( ; curIter != endIter; ++curIter )
   {
      DtElementEntry* entry = curIter->second;
      const DtElementAttributeDisplayName* name =
         dynamic_cast<const DtElementAttributeDisplayName*>(
         entry->findAttribute(DtElementAttributes::DisplayName));

      if ( name && name->value() == "Entity From Facade" )
      {
         // Found it.  Look up the scene object id used to visualize this element 
         // in the 3D model set.  We can use that scene object to get the position
         // and orientation of the plane
         DtElementID planeElementId = entry->elementID();
         DtElementData::SceneObjectIdList sceneObjectIds;
         elemData.getSceneObjectIds(planeElementId,
            sceneObjectIds, DtSceneObject::ModelSet_3D );

         // getSceneObjectIds() can return multiple scene object ids if multiple model sets
         // are specified.  Since we only asked for the 3D model set scene object, 
         // sceneObjectIds.size() should be 1.
         if ( sceneObjectIds.size() == 1 )
         {
            DtAgentUpdateResolverInterface* updater = agentManager().findUpdater( sceneObjectIds.front() );
            if(updater)
            {
               myPlaneSceneObject = updater->castObjectTypeFromUpdater<DtSceneObject>("DtSceneObject");
               found = true;
            }
         }
      }
   }

   // If we didn't find it, assume it hasn't been created yet. Return false, and we'll
   // try again next tick
   if (! found )
   {
      return false;
   }

   // We have the plane's scene object.  Now we can create GeoReferences for the 
   // plane and the Gimbal
   myPlaneGeoRef = new DtGeoReference( myAgentManager.de().sharedState().coordinateSystem() );
   myGimbalGeoRef = new DtGeoReference( myAgentManager.de().sharedState().coordinateSystem() );

   // We create a scene object (and associate it with an element) for the Gimbal so that 
   // our observer can be attached to it.  When an observer is attached to an element in
   // Mimic mode, it effectively uses that element's coordinate system; so attaching to
   // this Gimbal gives us an observer in Gimbal coordinates.  Note that no models are 
   // added to the scene object, so this element will be invisible (but if you had a 3D 
   // gimbal model you could create and add it here
   myGimbalSceneObjectAgent = DtSceneObjectAgent::create( myAgentManager );
   myGimbalSceneObjectAgent->setModelSet( 0 );

   // Set up the element
   myGimbalId = myAgentManager.createNewUniqueId();
   myGimbalSceneObjectAgent->setElementID( myGimbalId );
   reportElementCreated( myGimbalId, 0, DtElementEntry::Entity );

   // Give the element a name that we can use to attach to it
   DtElementAttributeDisplayName* displayName = new DtElementAttributeDisplayName( "Gimbal" );
   reportElementAttribute( myGimbalId, displayName);

   return true;
}

bool DtGimbalDriver::onStart()
{
   return true;
}

bool DtGimbalDriver::onStop()
{
   return true;
}

bool DtGimbalDriver::onTick()
{
   if ( myGimbalSceneObjectAgent == NULL )
   {
      if ( ! initialize() )
      {
         return true;
      }

      // The plane was found. Attach to it.
      attachObserverToEntity( "Gimbal" );
   }

   // Set up a GeoRef for the plane.  We use this to find the Gimbal position
   // and orientation for the Gimbal scene object (and Gimbal GeoRef)
   DtVector planePosition;
   DtTaitBryan planeOrientation;
   myPlaneSceneObject->getPosition( 0, planePosition );
   myPlaneSceneObject->getTopographicOrientation( 0, planeOrientation );
   myPlaneGeoRef->setLocationAndOrientation( DtLocalToGeodetic( planePosition,
      myAgentManager.de().sharedState().coordinateSystem() ), planeOrientation );

   // Find the position and orientation of the gimbal in geodetic coordinates, using
   // the GeoRef placed above;
   DtGeodeticCoord gimbalGeod;
   DtTaitBryan gimbalOri;
   myPlaneGeoRef->topoToGeodetic( 
      DtVector( DtGimbalForwardOffset, DtGimbalRightOffset, DtGimbalDownOffset), 
      DtTaitBryan(0., 0., 0.), 
      gimbalGeod,  gimbalOri );

   // Set up a GeoRef for our Gimbal
   myGimbalGeoRef->setLocationAndOrientation( gimbalGeod, gimbalOri );

   // Position our gimbal object
   myGimbalSceneObjectAgent->setPosition( 0, DtGeodeticToLocal( gimbalGeod, 
      myAgentManager.de().sharedState().coordinateSystem() ) );
   myGimbalSceneObjectAgent->setTopographicOrientation( 0, gimbalOri );

   // Get tower location in gimbal's topocentric system. (We don't need the tower's orientation)
   DtVector targetTopo;
   DtTaitBryan unused( 0., 0., 0. );
   myGimbalGeoRef->geodeticToTopo( *myTargetLocation, unused, targetTopo, unused );

   // find azimuth and elevation angles to our target
   double azimuthAngle = -atan2( targetTopo[1], targetTopo[0] );
   double xyPlaneDistance = sqrt( targetTopo[0] * targetTopo[0] + targetTopo[1] * targetTopo[1] );
   double elevationAngle = DtDeg2Rad( 90. ) - atan2( xyPlaneDistance, -targetTopo[2] );

   // Get the main observer
   DtObserver* observer = myAgentManager.de().driverManager().inputDriver().findObserverByName( "Observer 1" );
   if ( observer )
   {
      // Set the observer's azimuth and elevation angle to keep the tower in view
      observer->setAttachedOrientation( azimuthAngle, elevationAngle, 0. ); 
   }     

   return true;
}

void DtGimbalDriver::attachObserverToEntity( const std::string& entityName )
{
   DtObserver* observer = myAgentManager.de().
      driverManager().inputDriver().findObserverByName( "Observer 1" );
   if ( observer )
   {
      // Start by getting the existing observer state record for this observer.
      // We can modify it and then set it back.  We use the observer record
      // mechanism because it will allow the observer to attach even if the 
      // plane hasn't been created yet (the observer will retry attaching when
      // an observer record is used until the attach succeeds
      makArchives::DtObserverStateRecord observerState;
      observer->getRecord( observerState );

      // Attach to the plane
      std::set<std::string> displayNames;
      displayNames.insert( entityName );
      observerState.setAttachedToDisplayNames( displayNames );

      // We want no offset
      observerState.setObserverOffsetVector( 0., 0., 0. );

      // Get the observer settings record associated with this observer state record
      // so we can force the attach type to mimic
      makArchives::DtObserverSettingsRecord settingsRec = observerState.observerSettingsRecord();
      settingsRec.setAttachType( makArchives::DtObserverSettingsRecord::AttachTypeMimic );

      // Add the modified settings record back to the observer state record
      observerState.setObserverSettingsRecord( settingsRec );

      // Set a view magnification
      observerState.setViewMagnification( 100. );

      // ... and then add the observer state to the observer
      observer->setFromRecord( observerState );
   }
}

void DtGimbalDriver::slot_displayEngineAdded( const DtDeRecord& igRecord )
{
   stop();
   start();
}