Vehicle Sim

The class DtVehicleSim is responsible for simulating the Aircraft.

---

Header file:

/*******************************************************************************
** Copyright (c) 1992-2012 VT MAK
** All rights reserved.
*******************************************************************************/


#pragma once

#include <vl/exerciseConn.h>
#include <vl/reflectedEntityList.h>
#include <vl/entityPublisher.h>
#include <vl/emitterSystemPublisher.h>
#include <vl/topoView.h>
#include <vl/detonationInteraction.h>
#include <vl/fireInteraction.h>
#include <vl/publisherContainer.h>

class DtEmitterBeamRepository;

class DtVehicleSim
{
public:

   DtVehicleSim( double refLat, double refLon,
      DtExerciseConn* conn, DtReflectedEntityList* rel, 
      const DtEntityIdentifier& id, const DtEntityType& type,
      const char* name, const char* markings,
      const DtDeadReckonTypes alg, bool haveArtParts,
      DtObjectPublisherContainer &pubContainer,
      DtTime timeSpacing);

   virtual ~DtVehicleSim();

   virtual void init( DtConstVector initPos, double initSpeed,
      double initHeading, double initTRadius,
      double initPitch, bool autoRoll, double initRoll, int num );
   
   virtual void simulate();

   virtual void fireMunition( const DtGlobalObjectDesignator& tg );
   virtual void fireMunition();
   virtual void detonateMunition();
   virtual void munitionUpdate();

   virtual void reactToDetonation();
   virtual int checkDetonation();
   virtual void reactToDamage();
   virtual bool isDestroyed();
   virtual DtTime timeDestroyed();
   
   virtual void toggleEmissions();
   virtual void enableEmissions();
   virtual void disableEmissions();
   virtual void emissionsUpdate();

public:

   // Set the current detonation for reaction.
   static void setDetonation(const DtDetonationInteraction *theDetonation);

   static void setLethalDetonationRange( double range );
   static void setMunitionFlightTime( DtTime time );

   // set the delta time for next simTick update
   static void setSimDeltaTime(DtTime theDt) { myDt = theDt; }

protected:

   // delta time for next simTick update
   static DtTime myDt;

   // Current detonation, if any.
   static const DtDetonationInteraction *theDetonation;

protected:

   virtual void moveArtParts();
 
protected:

   DtExerciseConn*           myExConn;
   DtEntityPublisher*        myEntityPub;
   DtEmitterSystemPublisher* mySysPub;
   DtEntityStateRepository*  myESR;
   DtTopoView*               myTopoView;
   DtReflectedEntityList*    myRel;
   DtEmitterBeamRepository*  myBeam;
   DtObjectPublisherContainer &myPublishedObjects; 

   float mySpeed;
   float myTurnRadius;
   float myTurnRate;
   float myScalarAccel;
   double myBeamTime;
   
   DtTaitBryan myEuler;   
   DtVector    myPos;     
   DtVector32  myVel;     
   DtVector32  myAccel;   
   DtVector32  myRotRate; 

   float       myTurretAz;
   float       myTurretAzRate;
   float       myGunElev;
   float       myGunElevRate;

   DtFireInteraction* myFire;
   DtTime    myDetonateTime;
   DtTime    myTimeDestroyed;
   bool myIsDestroyed;

   bool myHaveArtParts;
   bool myEmissionsEnabled;

   DtTime myTimeSpacing;

protected:
   static double theLethalDetonationRange;
   static DtTime theMunitionFlightTime;
};

---

Implementation:

/*******************************************************************************
** Copyright (c) 1992-2012 VT MAK
** All rights reserved.
*******************************************************************************/


#include "vehicleSim.h"
#include <vlutil/vlUtil.h>
#include <vl/fireInteraction.h>
#include <vlpi/entityTypes.h>
#include <vlutil/vlPrint.h>
#include <vl/entityStateRepository.h>
#include <vl/reflectedEntity.h>
#include <vl/emitterBeamRepository.h>
#include <vl/emitterSystemRepository.h>
#include <vlpi/articulatedPart.h>
#include <vlpi/attachedPart.h>

#ifndef effZero
#define effZero 1.0E-30
#endif

// delta time for next simTick update
DtTime DtVehicleSim::myDt;

const DtDetonationInteraction *DtVehicleSim::theDetonation = NULL;
double DtVehicleSim::theLethalDetonationRange = 20.0;
double DtVehicleSim::theMunitionFlightTime = 3.0;

DtVehicleSim::DtVehicleSim( double refLat, double refLon,
   DtExerciseConn* conn, DtReflectedEntityList* rel, 
   const DtEntityIdentifier& id, const DtEntityType& type,
   const char* name, const char* markings,
   const DtDeadReckonTypes alg, bool haveArtParts,
   DtObjectPublisherContainer &pubContainer, DtTime timeSpacing) :
   myTimeSpacing(timeSpacing),
   myPublishedObjects(pubContainer),
   mySysPub( 0 )
{
   myExConn = conn;
#if DtHLA
   myEntityPub = new DtEntityPublisher( type, conn, name );
#elif DtDIS
   myEntityPub = new DtEntityPublisher( type, conn, id );
#else
#error "Please Choose Protocol!"
#endif

   // Add object to collection for later publication (in parallel)
   myPublishedObjects.add(myEntityPub);

   myESR = myEntityPub->entityStateRep();
   myESR->setEntityType( type );
   myESR->setGuise( type );
   myESR->setMarkingText( markings );
   myESR->setForceId( DtForceFriendly );
   myESR->setAlgorithm( alg );
   myESR->setEntityId( id );
   
   myTopoView = new DtTopoView( myESR, refLat, refLon );
   myRel = rel;
   myFire = 0;
   myDetonateTime = 0.0;

   myTimeDestroyed = -1.0;
   myIsDestroyed = false;
   myEmissionsEnabled = false;
   
   myHaveArtParts = haveArtParts;
}


DtVehicleSim::~DtVehicleSim()
{
   disableEmissions();
   DtDELETE( myTopoView );
   myPublishedObjects.remove(myEntityPub);
   DtDELETE( myEntityPub );
}


void DtVehicleSim::init( DtConstVector initPos, double initSpeed,
   double initHeading, double initTRad,
   double initPitch, bool autoRoll, double initRoll, int num)
{
   myPos        = initPos;
   mySpeed      = initSpeed;
   myTurnRadius = initTRad;
   myTurnRate   = ( ( initTRad != 0.0 )  ?  ( initSpeed / initTRad ) : 0.0 );

   // hack to make roll roughly proportional to turnRate for rates
   // about 1/5 rad/sec 
   double roll = ( autoRoll )  ?
      atan( 5.0 * myTurnRate ) : DtDeg2Rad( initRoll );

   myEuler = DtTaitBryan( DtDeg2Rad( initHeading ),
                          DtDeg2Rad( initPitch ), roll );
   
   float psi = myEuler.psi();
   float cosPsi = cos( psi );
   float sinPsi = sin( psi );
   float phi = myEuler.phi();
   float cosPhi = cos( phi );
   float sinPhi = sin( phi );

   myVel = DtVector32( cosPsi * mySpeed, sinPsi * mySpeed, 0.0 );

   float timeOffset = (float)myTimeSpacing * (float)num;
   for (int i = 0; i < 3; i++)
   {
      myPos[i] += myVel[i] * timeOffset;
   }

   // force a constant rate turn
   myScalarAccel = mySpeed * myTurnRate;
   
   myAccel   = DtVector32( -myScalarAccel * sinPsi, myScalarAccel * cosPsi, 0.0 );
   myRotRate = DtVector32( 0.0, sinPhi * myTurnRate, cosPhi * myTurnRate );

   myTurretAz     = 0.0;
   myTurretAzRate = 0.0;
   myGunElev      = 0.0;

   if (myHaveArtParts)
   {
      DtArticulatedPartCollection* artPartList = myESR->artPartList();
      DtArticulatedPart& turret = artPartList->getPart(DtPrimaryTurret1);
      DtArticulatedPart& gun = artPartList->getPart(DtPrimaryGun1);
      artPartList->attachPart(&gun, &turret);
   }
}


void DtVehicleSim::simulate()
{
   // Update kinematic position, velocity, acceleration and orientation   
   double psi = DtModPerZero( ( myEuler.psi() + ( myTurnRate * myDt ) ), M_2PI );
   myEuler.setPsi( psi );

   for( int i=0; i<3; ++i )
   {
      myPos[i] += ( myVel[i] + 0.5 * myAccel[i] * myDt ) * myDt;
      myVel[i] += myAccel[i] * myDt;
   }
   myAccel[0] = -myScalarAccel * sin( psi );
   myAccel[1] =  myScalarAccel * cos( psi );
   myAccel[2] =  0.0;

   // Simulate articulated parts, if any.
   if ( myHaveArtParts )
   {
      moveArtParts();

      myESR->artPartList()->getPart(DtPrimaryTurret1).setParameter(
         DtApAzimuth, DtDeg2Rad(myTurretAz));
      myESR->artPartList()->getPart(DtPrimaryTurret1).setParameter(
         DtApAzimuthRate, DtDeg2Rad(myTurretAzRate));
      myESR->artPartList()->getPart(DtPrimaryGun1).setParameter(
         DtApElevation, DtDeg2Rad(myGunElev));
   }
   
   // Now that we're done simulating, tell the entity publisher
   // about our current state:
   myTopoView->setLocation( myPos );
   myTopoView->setVelocity( myVel );
   myTopoView->setAcceleration( myAccel );
   myTopoView->setOrientation( myEuler );
   myTopoView->setRotationalVelocity( myRotRate );

   // Simulate subsystems
   munitionUpdate();    // Simulate munitions created by this vehicle, if any.
   emissionsUpdate();   // Simulate emissions from this vehicle, if any.
}

void DtVehicleSim::moveArtParts()
{
   DtClock* clock = myExConn->clock();
   
   if ( clock->simTime() >= 2.0)// && clock->simTime() <= 10.0 )
   { 
      myGunElevRate = 5.0;
   }
   else 
   {
      myGunElevRate = 0.0;
   }

   if ( clock->simTime() >= 2.0)// && clock->simTime() <= 20.0 )
   {
      myTurretAzRate = 9.0;
   }
   else 
   {
      myTurretAzRate = 0.0;
   }

   myTurretAz = myTurretAz + myTurretAzRate * (float) myDt;
   myGunElev = myGunElev + myGunElevRate * (float) myDt;
}


void DtVehicleSim::setDetonation(const DtDetonationInteraction *detonation)
{
   theDetonation = detonation;
}


void DtVehicleSim::reactToDetonation()
{
   if ( checkDetonation() )
   {
      reactToDamage();
   }
}


int DtVehicleSim::checkDetonation()
{
   double distance;
   switch( theDetonation->result() )
   {
      case DtDetResEntityImpact:
      case DtDetResEntityProximate:
      {
         if ( myEntityPub->globalId() == theDetonation->targetId() )
         {
            DtInfo <<  "Direct hit!  Going down!" << std::endl;
            return 1;
         }
      }
      // fall through
      case DtDetResGroundImpact:
      case DtDetResGroundProximate:
      case DtDetResDetonation: 
      {
         distance = DtDistance( theDetonation->worldLocation(), myPos );
         if ( distance < theLethalDetonationRange )
         {
            DtInfo <<  "Indirect hit only " << distance << " m away!  Going down!"
               << std::endl;
            return 1;
         }
         else
         {
            return 0;
         }
      }
      default:
      {  // ignore it
         return 0;
      }
   }
}


void DtVehicleSim::reactToDamage()
{
   myEntityPub->entityStateRep()->setDamageState( DtDamageDestroyed );
   myEntityPub->entityStateRep()->setSmokePlumePresent( true );
   myEntityPub->entityStateRep()->setFlamesPresent( true );

   myVel     = DtVector32( 0.0, 0.0, 0.0 );
   myAccel   = DtVector32( 0.0, 0.0, 0.0 );
   myRotRate = DtVector32( 0.0, 0.0, 0.0 );
   myTurnRate= 0.0;
   mySpeed   = 0.0;

   myIsDestroyed = true;
   myTimeDestroyed = myExConn->clock()->simTime();
}

void DtVehicleSim::enableEmissions()
{
   if ( !myEmissionsEnabled )
   {
      DtInfo <<  "Emissions enabled." << std::endl;
      myEmissionsEnabled = true;

      // Create emitter system
      mySysPub = new DtEmitterSystemPublisher( myExConn );

      // Add it to publications
      myPublishedObjects.add(mySysPub);

      mySysPub->esr()->setEntityId( myESR->entityId() );
      mySysPub->esr()->setHostId( myEntityPub->globalId() );
      mySysPub->esr()->setEventId( myExConn->nextEventId() );

      // Attach emitter system to entity
      DtVector32 vec( 6.4f, 0.0, 0.0 );
      mySysPub->esr()->setRelativePosition( vec );

      // Create beam
      myBeam = mySysPub->esr()->addBeam( 1 );
#if DtHLA
      // HLA Only - In DIS a beam is not a separate object and
      // also an emittingSystemID would be just an integer...
      myBeam->setEmittingSystemId( mySysPub->globalId() );
#endif
      myBeam->setFrequency( 8999999488.000000 );
      myBeam->setFrequencyRange( 5000 );
      myBeam->setERP( 70 );
      myBeam->setAzimuthCenter( 0.0 );
      myBeam->setAzimuthSweep( 0.0523599f );
      myBeam->setElevationCenter( 0.349066f );
      myBeam->setElevationSweep( 0.523599f );
      myBeamTime = 0.0;
   }
}


void DtVehicleSim::disableEmissions()
{
   if (myEmissionsEnabled)
   {
      DtInfo <<  "Emissions disabled." << std::endl;
      myEmissionsEnabled = false;
      mySysPub->esr()->removeBeam( 1 );
      mySysPub->tick();

      // Remove it from publications
      myPublishedObjects.remove(mySysPub);

      DtDELETE( mySysPub );
   }
}


void DtVehicleSim::toggleEmissions()
{
   if ( myEmissionsEnabled )
   {
      disableEmissions();
   }
   else
   {
      enableEmissions();
   }
}


void DtVehicleSim::emissionsUpdate()
{
   if ( myEmissionsEnabled )
   {
      // Update beam
      myBeamTime += myDt;
      float beamAngle = (float)cos( myBeamTime );
      myBeam->setAzimuthCenter( beamAngle );
   }
}


// return the vehicle that is nearest to ours
static DtReflectedEntity* nearestEntity( const DtObjectId& me,
   DtConstVector geocPos, DtReflectedEntityList* rel )
{
   DtReflectedEntity* nearest = 0;
   double minDistSqr = 0.0;

   // Loop through the entities in the rel
   for( DtReflectedEntity* entity = rel->first();
        entity;
        entity = entity->next() )
   {
      if ( entity->id() == me )  continue;
      double distSqr = DtDistSqr(
         entity->entityStateRep()->location(), geocPos );
      if ( distSqr < minDistSqr || !nearest )
      {
         nearest = entity;
         minDistSqr = distSqr;
      }
   }
   return nearest;
}
  

void DtVehicleSim::fireMunition()
{
   DtReflectedEntity* nearest = nearestEntity(
      myEntityPub->id(), myEntityPub->entityStateRep()->location(), myRel );
   if ( nearest )
   {
      fireMunition( nearest->globalId() );
   }
}


void DtVehicleSim::fireMunition( const DtGlobalObjectDesignator& tg )
{        
   if ( myFire )
   {
      return;
   }

   myFire = new DtFireInteraction;
   myFire->setAttackerId( myEntityPub->globalId() );
   myFire->setTargetId( tg );
   myFire->setMunitionId( DtGlobalObjectDesignator() );
   myFire->setRange( 1000.0 );
   myFire->setFuseType( DtFuzeOther );
   myFire->setQuantity( 1 );
   myFire->setRate( 0 );
   myFire->setWarheadType( DtWarheadOther );
#if DtHLA
   myExConn->setObjectIdForEventId( myEntityPub->globalId() );
#endif
   myFire->setEventId( myExConn->nextEventId() );
   myFire->setLocation( myESR->location() );
   myFire->setVelocity( myESR->velocity() );
   myFire->setMunitionType( DtEntityType(
      DtMunition, DtAntiAir, DtUnitedStates, DtMunitionGuided,
      DtSidewinder, 0, 0 ) );
   myExConn->sendStamped( *myFire );

   DtInfo <<  "Sending fire interaction" << std::endl;
   myDetonateTime = myExConn->clock()->simTime() + theMunitionFlightTime;
}


void DtVehicleSim::detonateMunition()
{        
   if ( !myFire )
   {
      return;
   }

   DtDetonationInteraction det( *myFire );

   // Sidewinder Missile
   DtEntityType sidewinder(2,1,255,1,1,3,0);
   det.setMunitionType(sidewinder);
   det.setTargetId(myFire->targetId());
   det.setVelocity( DtVector32( 0.0, 0, 0 ) );
   det.setEntityLocation( DtVector32( 0.0, 0.0, 0.0 ) );

   DtReflectedEntity* ent = myRel->lookup( myFire->targetId() );
   if ( ent )
   {
      // Get vector from fire location to detonation location
      DtVector difference = myFire->location() - ent->esr()->location();

      // Velocity = Distance / Time
      DtVector32 velocity;
      velocity.setX(difference.x() / theMunitionFlightTime);
      velocity.setY(difference.y() / theMunitionFlightTime);
      velocity.setZ(difference.z() / theMunitionFlightTime);
      det.setVelocity(velocity);
      det.setWorldLocation( ent->entityStateRep()->location() );
      det.setResult( DtDetResEntityImpact );
   }
   else
   {
      // just detonate 1000 meters under us.
      det.setVelocity(DtVector32(0.0, 0.0, 1000.0f / theMunitionFlightTime));
      det.setWorldLocation( DtVector( myPos[0], myPos[1], myPos[2] + 1000.0 ) );
      det.setResult( DtDetResDetonation );
   }

   myExConn->sendStamped( det );
   DtInfo << "Sending detonation interaction" << std::endl;
   DtDELETE( myFire );
}   


void DtVehicleSim::munitionUpdate()
{
   if ( myFire && myExConn->clock()->simTime() > myDetonateTime )
   {
      detonateMunition();
   }
}


bool DtVehicleSim::isDestroyed()
{
   return myIsDestroyed;
}


DtTime DtVehicleSim::timeDestroyed()
{
   return myTimeDestroyed;
}


void DtVehicleSim::setLethalDetonationRange( double range )
{
   theLethalDetonationRange = range;
}


void DtVehicleSim::setMunitionFlightTime( DtTime time )
{
   theMunitionFlightTime = time;
}