groundMoveToObstacleAvoider.h

Go to the documentation of this file.

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


#pragma once

#include <vrfobjcore/physicalWorld.h>
#include <vrfobjcore/platformLocalObjectFacade.h>

#include <vrfmodel/vrfmodelDefines.h>
#include <vrfmodel/collisionDetector.h>
#include <vrfmodel/groundMoveToDestinationControllerComponent.h>

#include <features/featureGeometry.h>

class MAKVRinTerra::DtFeatureGeometry;
class DtCompositePredicate;


class DT_DLL_vrfmodel DtGroundMoveToObstacleAvoider
{
public:
   DtGroundMoveToObstacleAvoider(const DtSimulationServices* simsvc,
      const DtLocalObject* hostEntity,
      const DtGroundMoveToDestinationControllerComponent* parentCtrlr);

   virtual ~DtGroundMoveToObstacleAvoider();

   

public:
   enum AvoiderStatusTypeValue
   {
      avoiderNotActive = 0,
      avoiderNoObstacles = 1,
      avoiderAvoiding = 2,
      avoiderStoppedForMovingObstacle = 3,
      avoiderStoppedForStaticObstacle = 4,
   };

   enum ObstacleType
   {
      obstacleStatic = 0,
      obstacleDynamic = 1
   };

   struct AvoiderStatus
   {
      enum AvoiderStatusTypeValue myStatusType;
      enum ObstacleType myObstacleType;
      DtSimObjectReference myVehicleObstacle;
      DtChord myWallObstacle;
   };

   std::string avoiderStatusString(enum AvoiderStatusTypeValue);

   virtual void steeringAndSpeedThroughObstacles(
      const DtVector& localGoalPosition,
      const double desiredImmediateHeading,
      const double desiredSpeed, const double desiredAccelLimit,
       double& desiredHeadingOut, double& desiredSpeedOut,
      AvoiderStatus& avoiderStatus,
      const bool debug = false);

   virtual void searchAreaVisualization(std::vector<DtVector>& boundary);

   virtual void staticObstacleVisualization(std::vector<std::vector<DtVector>>& walls);

   virtual void dynamicObstacleVisualization(DtExtent& extent);

   

protected:

   virtual double maxOffroadSpeed() const { return 20.0; }

   virtual double lookaheadTime() const { return 7.0; }

   

    
   virtual bool shouldYieldToRight() { return true; } 

   virtual bool shouldYieldToLeft() { return false; }



   

   struct EntityObstacleData
   {
      EntityObstacleData();
      EntityObstacleData(DtSimObjectReference obj);
      EntityObstacleData(DtSimObjectReference obj, DtVector loc, DtVector vel,
         double heading, double len, double width);

      DtSimObjectReference object; 
      DtVector bodyOffset; 
      DtVector bodyVelocity; 
      double relativeHeading; 
      double bboxLength;
      double bboxWidth;

      bool isStatic; 
      double maxX;
      double minX;
   };

   struct FeatureObstacleEdge
   {
      DtVector startLocation;
      double heading;
      bool isFacingEntity;
      FeatureObstacleEdge();
      FeatureObstacleEdge(const DtVector& loc);
   };

   struct FeatureObstacleData
   {
      FeatureObstacleData();
      ~FeatureObstacleData(); 


      std::vector<FeatureObstacleEdge> edges;
   };
   
   


protected:

   virtual void provideDynamicObstacles(std::list<struct EntityObstacleData>& vehicles);

   virtual void computeEntityObstacleData(const DtSimObjectReference& obj,
      EntityObstacleData& entityObstacleData);
   virtual void provideStaticObstacles(std::list<struct FeatureObstacleData>& walls);

   virtual void testAndSetClockwiseOrdering(FeatureObstacleData& obstacle);


   virtual void setInitialObstacleEdgeHeadings(FeatureObstacleData& obstacle) const;
   virtual void setIsFacingEntity(FeatureObstacleData& obstacle,
      const DtVector& entityLocation);

   virtual DtExtent makeDynamicObstacleSearchArea(const DtVector& localGoal);

   virtual MAKVRinTerra::DtFeatureGeometry::Area makeStaticObstacleSearchArea(
      const DtVector& localGoal);




protected:

   virtual bool isNotSelf(const DtSimObject* obj);
   virtual bool isNotBehind(const DtSimObject* obj);
   virtual bool isNotEmbarkedOnDifferentParent(const DtSimObject* obj);

   virtual bool doesNotOverlapInAltitude(const DtSimObject* obj);
   static bool ignoreSelfFilterFcn(const DtSimObject* obj, void* usr);
   static bool ignoreBehindFilterFcn(const DtSimObject* obj, void* usr);

   static bool ignoreEmbarkedOnDifferentParentFcn(const DtSimObject* obj, void* usr);

   static bool ignoreDifferentAltitudeFcn(const DtSimObject* obj, void* usr);

   virtual DtCompositePredicate dynamicObstaclePredicate();

   virtual void putFeatureInCache(const MAKVRinTerra::DtFeature& f);




public:
   struct SpeedInterval
   {
      double lowSpeed;
      double highSpeed;
      SpeedInterval(const double a, const double b)
      {
         lowSpeed = a;
         highSpeed = b;
      }
   };

   typedef std::list<SpeedInterval> IntervalList;

protected:

   class SpeedConstraint
   {
   public:
      SpeedConstraint();
      SpeedConstraint(const SpeedConstraint& other);
      ~SpeedConstraint();
      SpeedConstraint& operator=(const SpeedConstraint& other);

      double myHeading;
      IntervalList myIntervals;
      double myInitialClearDistance;

      
      bool myIsWall;
      DtChord myWallChord;
      DtSimObjectReference myblockingVehicle;
      bool myIsVehicleStopped;
      bool myIsContactRight;
      bool myIsContactLeft;

      bool myShouldYieldTo;
   
      void setUnconstrained();

      void removeInterval(const double start, const double end);

      void merge(const SpeedConstraint& other);

      IntervalList intersectWithIntervalList(
         const IntervalList& otherIntervals) const;

      double highestSpeedAllowed() const;
   };




   virtual void computeFinalAction(
      const std::list<struct EntityObstacleData>& vehicles,
      const std::list<struct FeatureObstacleData>& walls,
      const double desiredHeading, const double desiredSpeed,
      double& outHeading, double& outSpeed, AvoiderStatus& status);

   virtual double computeSpeedAndScoreForPathHeading(const double pathHeading,
      double refHeading, SpeedConstraint& refConstraint, const bool maybeYieldRow,
      const std::list<struct EntityObstacleData>& vehicles,
      const std::list<struct FeatureObstacleData>& walls, const double desiredSpeed, const double desiredHeading,
      double& outSpeed, SpeedConstraint& resultConstraint);

   virtual double computeSpeedAndScoreForStraightAhead(
      const std::list<struct EntityObstacleData>& vehicles,
      const std::list<struct FeatureObstacleData>& walls, 
      const double desiredSpeed,
      const double desiredHeading,
      SpeedConstraint& outConstraint,
      double& outSpeed);

   virtual SpeedConstraint computeSpeedConstraintFromDynamicObstacle(
      const double originOffset,
      const double ownHalfWidth, const double ownHalfLength,
      const EntityObstacleData& obstacle, 
      bool leftTurn, bool rightTurn, const double cornerBufferWidth);

   virtual SpeedConstraint computeSpeedConstraintFromStaticObstacle(
      const double originOffset,
      const double ownHalfWidth, const double ownHalfLength,
      const FeatureObstacleData& obstacle,
      bool leftTurn, bool rightTurn, const double cornerBufferWidth);




   //-------------------------------
   
   
   virtual double actionScore(const SpeedConstraint& constrainedAction,
      const double desiredHeading, const double desiredSpeed,
      double& speedForBestScore);

   virtual void updateBestScoreAndSpeed(double& bestScore, double& bestSpeed,
      const double speed, const double heading,
      const double desiredSpeed, const double desiredHeading,
      const bool applyYield = false, const bool isStatic = false);

   virtual double combinedScore(const double speed, const double heading,
      const double desiredSpeed, const double desiredHeading,
      const bool yieldTo = false, const bool isStatic = false);

   virtual double speedGreaterThanDesiredScore(const double speed, const double heading,
      const double desiredSpeed, const double desiredHeading);
   virtual double speedLessThanDesiredScore(const double speed, const double heading,
      const double desiredSpeed, const double desiredHeading);
   virtual double speedInDesiredDirectionScore(const double speed, const double heading,
      const double desiredSpeed, const double desiredHeading);
   virtual double speedLessThanFeasibleScore(const double speed, const double heading,
      const double desiredSpeed, const double desiredHeading);
   virtual double speedGreaterThanFeasibleScore(const double speed, const double heading,
      const double desiredSpeed, const double desiredHeading);
   virtual double headingInDesiredDirectionScore(const double speed, const double heading,
      const double desiredSpeed, const double desiredHeading);
   virtual double yieldToScore(const double speed, const double heading,
      const double desiredSpeed, const double desiredHeading);




   virtual void computeBodyTransform();

   virtual void computeHorizontalRotation(const double angle,
      DtDcm& rot);

   virtual void markVehiclesWithRightOfWay(const double desiredHeading,
      const std::list<struct EntityObstacleData>& vehicles);

   virtual double maxSpeedForPassBehind(const double timeLimit,
      const double distance);

   virtual double minSpeedForPassAhead(const double timeLimit,
      const double distance);

   virtual double minTurnRadiusForCurrentSpeed() const;

   virtual double maxAccelerationForSpeed();

   virtual double maxTractionForConditions();

   
   virtual double highestFeasibleSpeed() const;
   virtual void setHighestFeasibleSpeed(const double s);

   virtual double lowestFeasibleSpeed() const;
   virtual void setLowestFeasibleSpeed(const double s);

   


protected:
   const DtSimulationServices* mySimulationServices;
   const DtLocalObject* myEntity;
   const DtGroundMoveToDestinationControllerComponent* myParentController;
   const DtGroundVehicleStateRepository* myVehicleState;
   DtVector myLocalDestination;
   bool myIsGoingBackwards;
   

   
   double myLateralAccelerationLimit;

   double myAccelerationLimit;

   double myBrakingLimit; 

   double myHighestFeasibleSpeedNow;

   double myLowestFeasibleSpeedNow;

   double myMaxSpeedForMinTurnRadius;

   double myCurrentAltitude;

   double mySpeed;

   double myHeading;


   std::set<DtSimObjectReference> myVehiclesToYieldTo;

   std::list<DtSimObjectReference> myCachedEntityObstacles;
   double myLastEntityQueryTime;

   bool myWasEmbarked;

   std::shared_ptr<MAKVRinTerra::DtFeatureSet> myObstacleFeatureSet;

   DtVector myLastObstacleSearchLocation;
   double myLastObstacleSearchHeading;

   bool myHaveAskedForFeatureObstacles;
   bool myHaveLoadedFeatureObstacles;

   std::list<FeatureObstacleData> myCachedFeatureObstacles;

   DtCoordTransform myLocalToHorizontalBody;

   bool myDebug;
   DtSimObjectReference myKeyVehicleObstacle;
   bool myIsObstacleWall;
   DtChord myKeyWallObstacle;

   DtExtent myDynamicObstacleSearchArea;
   MAKVRinTerra::DtFeatureGeometry::PointVector myStaticObstacleSearchArea;
};