groundFollowInFormationController.h

Go to the documentation of this file.

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




#pragma once

class DtLocalVectorIterator;
class DtVectorOutputPort;

#include "matrix/coordTransform.h"
#include "vrfmodel/vrfmodelDefines.h"
#include "vrfobjcore/constrainedAnalogIOPort.h"
#include "vrfobjcore/taskControllerComponent.h"
#include "vrfmodel/groundAutoControllerComponent.h"
#include "vrfobjcore/inputPortGroup.h"
#include "vrfobjcore/outputPortGroup.h"
#include "vrfobjcore/collisionIOPort.h"
#include "vrfobjcore/movementLocalObjectFacade.h"

class DT_DLL_vrfmodel DtGroundFollowInFormationControllerComponent : public DtGroundAutoControllerComponent
{

public:

   DtGroundFollowInFormationControllerComponent(const DtString& name,
      DtLocalObject* owner,
      DtSimulationServices* simManager,
      DtComponentDescriptor* desc = 0,
      DtReaderWriterRegistry* parentRegistry = 0);

   virtual ~DtGroundFollowInFormationControllerComponent() override;

   virtual const char* type() const override;

   virtual void tick() override;

   virtual void registerTaskMsgCallbacks() override;

   static void followInFormationCallback(DtSimMessage * msg, void * usr);
   
   static DtSimComponent * creator(const DtString& name,
      DtLocalObject* owner,
      DtSimulationServices* simManager,
      DtComponentDescriptor* desc = 0,
      DtReaderWriterRegistry* parentRegistry = 0);
protected:

   void cleanFollowInFormation();

   virtual void endFollowInFormation(bool taskSuccess = true);
   
   virtual void clearTask() override;

   virtual int adjustStartPoint(const std::vector<DtVector>& routeVertices, 
      const DtVector& entityLocation);
      
      
   virtual void doMoving();

   virtual bool checkTaskEndCondition();

   virtual bool isLeaderCapable() const;


   virtual double horizDistBetweenPoints(DtVector, DtVector);
   virtual double horizDistBetweenPoints(DtPoint loc1, DtPoint loc2);

   virtual int findClosestSegmentIndex(DtVector testPoint, std::vector<DtVector> testRoute, 
      DtPoint& closestSegmentLoc, double& distanceToSegment, int indexSegmentFloor = 0);

   virtual double findDesiredAcceleration(double targetSpeed, double targetHeading, 
      double currentHeading, double disToTarget);

   virtual bool createFollowerOffsetRoute(const std::vector<DtVector>& localPoints, 
      float rightOffset, float forwardOffset, DtUUID& routeUUID);

   virtual void processFollowInFormation(const DtTaskMessage& msg);

   virtual void updateTaskVisualizations() override;

   virtual bool publishFormationRoutes() const;

   virtual double distanceBehind(const DtVector topoRefPos,
      const DtVector leaderDir, const DtVector topoCurrentPosition);
   virtual bool isPastDestination(const int currentIndex);

private:

   DtGroundFollowInFormationControllerComponent();

   DtGroundFollowInFormationControllerComponent(
      const DtGroundFollowInFormationControllerComponent& orig);

   DtGroundFollowInFormationControllerComponent& operator=(
      const DtGroundFollowInFormationControllerComponent& orig);

protected:
   DtSimObjectReference             myRoute;

   std::vector<DtVector> myTopoOffsetRoute;
   DtCoordTransform myLocalToTopoTransform;

   bool                             myIsOnRoute;
   bool                             myHasProcessed;
   double                           myDisToTarget;
   double                           myBackwardsOffset;
   double                           myNearDistance;
   double                           myAtDistance;
   double                           myApproachSpeed;
   int                              myIndexDriven;
   DtSimObjectReference             myLeader;
   DtMovementSimObjectFacade        myLeaderMovementSimObjectFacade;
   DtSimObjectReference             myLeadersRoute;
   DtVector                         myLocalDestination;
   DtVector                         myClosestSegmentPt1;
   DtVector                         myClosestSegmentPt2;
   DtVector myLeaderLongitudinalOffsetPoint;
   DtMovementLocalObjectFacade myMovementLocalObjectFacade;
};