Vehicle Sim

The class DtVehicleSim is responsible for simulating the Aircraft.

---

Header file:

/*******************************************************************************
** Copyright (c) 1992-2010 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>

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 );

   virtual ~DtVehicleSim();

   virtual void init( DtConstVector initPos, double initSpeed,
      double initHeading, double initTRadius,
      double initPitch, bool autoRoll, double initRoll );
   
   virtual void simTick( DtTime dt );
   
   virtual void fireMunition( const DtGlobalObjectDesignator& tg );
   virtual void fireMunition();
   virtual void detonateMunition();
   virtual void munitionTick();

   virtual void processDetonation( DtDetonationInteraction* inter );
   virtual int checkDetonation( DtDetonationInteraction* inter );
   virtual void reactToDamage();
   virtual bool isDestroyed();
   virtual DtTime timeDestroyed();
   
   virtual void toggleEmissions();
   virtual void enableEmissions();
   virtual void disableEmissions();
   virtual void emissionsTick( double dt );

public:

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

protected:

   virtual void moveArtParts( DtTime dt );
 
protected:

   static void theDetonationCb( DtDetonationInteraction*, void* );

protected:
   DtExerciseConn*           myExConn;
   DtEntityPublisher*        myEntityPub;
   DtEmitterSystemPublisher* mySysPub;
   DtEntityStateRepository*  myESR;
   DtTopoView*               myTopoView;
   DtReflectedEntityList*    myRel;
   DtEmitterBeamRepository*  myBeam;

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

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

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

   bool myHaveArtParts;
   bool myEmissionsEnabled;

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

---

Implementation:

/*******************************************************************************
** Copyright (c) 1992-2010 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


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 ) :
   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

   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;

   DtDetonationInteraction::addCallback( myExConn, theDetonationCb, this );
   DtInfo <<  "EntityType: " <<  type.string()  << '\n';
}


DtVehicleSim::~DtVehicleSim()
{
   disableEmissions();
   DtDetonationInteraction::removeCallback( myExConn, theDetonationCb, this );
   DtDELETE( myTopoView );
   DtDELETE( myEntityPub );
   DtDELETE( mySysPub );
}


void DtVehicleSim::init( DtConstVector initPos, double initSpeed,
   double initHeading, double initTRad,
   double initPitch, bool autoRoll, double initRoll )
{
   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 );

   double psi = myEuler.psi();
   double cosPsi = cos( psi );
   double sinPsi = sin( psi );
   double phi = myEuler.phi();
   double cosPhi = cos( phi );
   double sinPhi = sin( phi );

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

   // force a constant rate turn
   myScalarAccel = mySpeed * myTurnRate;

   myAccel   = DtVector( -myScalarAccel * sinPsi, myScalarAccel * cosPsi, 0.0 );
   myRotRate = DtVector( 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::simTick( DtTime dt )
{
   munitionTick();

   double psi =
      DtModPerZero( ( myEuler.psi() + ( myTurnRate * dt ) ), M_2PI );
   myEuler.setPsi( psi );

   for( int i=0; i<3; ++i )
   {
      myPos[i] += ( myVel[i] + 0.5 * myAccel[i] * dt ) * dt;
      myVel[i] += myAccel[i] * dt;
   }

   myAccel[0] = -myScalarAccel * sin( psi );
   myAccel[1] =  myScalarAccel * cos( psi );
   myAccel[2] =  0.0;

   if ( myHaveArtParts )
   {
      moveArtParts( dt );

      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 );

   myEntityPub->tick();

   emissionsTick( dt );
}

void DtVehicleSim::moveArtParts( DtTime dt )
{
   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) dt;
   myGunElev = myGunElev + myGunElevRate * (float) dt;
}


void DtVehicleSim::processDetonation( DtDetonationInteraction* detInter )
{
   std::cout << "Received Detonation!" << std::endl;
   if ( checkDetonation( detInter ) )
   {
      reactToDamage();
   }
}


int DtVehicleSim::checkDetonation( DtDetonationInteraction* detInter )
{
   double distance;
   switch( detInter->result() )
   {
      case DtDetResEntityImpact:
      case DtDetResEntityProximate:
      {
         if ( myEntityPub->globalId() == detInter->targetId() )
         {
            DtInfo <<  "Direct hit!  Going down!" << std::endl;
            return 1;
         }
      }
      // fall through
      case DtDetResGroundImpact:
      case DtDetResGroundProximate:
      case DtDetResDetonation:
      {
         distance = DtDistance( detInter->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     = DtVector( 0.0, 0.0, 0.0 );
   myAccel   = DtVector( 0.0, 0.0, 0.0 );
   myRotRate = DtVector( 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 );
      mySysPub->esr()->setEntityId( myESR->entityId() );
      mySysPub->esr()->setHostId( myEntityPub->globalId() );
      mySysPub->esr()->setEventId( myExConn->nextEventId() );

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

      // Create beam
      myBeam = mySysPub->esr()->addBeam( 1 );
      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();
      DtDELETE( mySysPub );
   }
}


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


void DtVehicleSim::emissionsTick( double dt )
{
   if ( myEmissionsEnabled )
   {
      // Update beam
      myBeamTime += dt;
      float beamAngle = (float)cos( myBeamTime );
      myBeam->setAzimuthCenter( beamAngle );
      mySysPub->tick();
   }
}


// 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( DtVector( 0.0, 0, 0 ) );
   det.setEntityLocation( DtVector( 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
      DtVector 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(DtVector(0.0, 0.0, 1000.0 / 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::munitionTick()
{
   if ( myFire && myExConn->clock()->simTime() > myDetonateTime )
   {
      detonateMunition();
   }
}


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


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


void DtVehicleSim::theDetonationCb(
   DtDetonationInteraction* inter, void* vehicle)
{
   ( (DtVehicleSim*)vehicle )->processDetonation( inter );
}


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


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