roadMovementUpdater.h

Go to the documentation of this file.

#pragma once

//#include "railPSR.h"
#include <vector>
#include <matrix/vlVector.h>
#include <matrix/vlTaitBryan.h>
#include <matrix/coordTransform.h>

#include "vrfmodel/railSplineCalculator.h"
#include "vrfobjcore/railMovementControllerPSR.h"
#include "vrfobjcore/platformLocalObjectFacade.h"
#include "vrfmodel/roadSegment.h"

class DtLocalObject;
class DtRoadMovementController;
class DtRailSegment;
class DtAttachedTerrain;
class DtUpdateRepositoryOutputPortGroup;
class DtVrfMovingObjectStateRepository;
class DtRoadMovementDescriptor;
class DtConstrainedAnalogOutputPort;
class DtConstrainedIntegerOutputPort;
class DtBooleanOutputPort;
class DtOutputPortGroup; 


#define DT_ROAD_MOVE_COAST_DECEL_FRACTION 0.5

#define DT_ROAD_MOVE_DESTROYED_DECELERATION -5.0

#include "vrfmodel/vrfmodelDefines.h"
class DT_DLL_vrfmodel DtRoadMovementUpdater
{

public:

   DtRoadMovementUpdater(DtRoadMovementController * parentController, 
      bool useHighGroundClamp,const DtAttachedTerrain* attachedTerrain);

   virtual ~DtRoadMovementUpdater();


   virtual bool init();

   virtual void clear();

   virtual void setPSR(DtRailMovementControllerPSR* psr);
   virtual void movementComplete();

   

   virtual void updateRailAndWorldPosition(DtRoadSegment* currentSeg, double newAccel,
      double dT, double simTime = 0.0);

   virtual void updateNoMovement();

   virtual double offLaneDistanceThreshold() const;

   virtual DtVector steeringTargetPosition() const;

   virtual DtVector closestPositionInLane() const;

   virtual void updateFromPSR(const DtVector localPos, DtRailSegment * currentSeg);

  
protected:
   virtual void updateLaneAcquisitionStatus(const DtRoadSegment* segment);

   virtual double fuelUsed(double speed) const;

   virtual void kinematicUpdate(const double oldSpeed, const double accel,
      const double dt, double& deltaPosition, double& newSpeed,
      double &actualAccel);

   virtual void doKinematicUpdateAlongRail(
      const double dt,
      DtRoadSegment* currentSeg,
      const int roadLaneNum,
      const double oldPositionInLane,
      const double oldSpeed,
      double & accel,
      double & newPositionInLane,
      double & newLateralOffset,
      double & laneChangeHeadingOffset,
      double & newSpeed,
      double & newRotationRate);

   virtual void computePositionInDownstreamSegment(
      DtRoadSegment* & currentSeg,
      int & roadLaneNum, 
      double & newPositionInLane, 
      const double newLateralOffset,
      double &segmentCurveHeadingOffset,
      DtVector& localPosition,
      int & segmentIndex);

   virtual void updateLateralOffset(const double currentOffset,
      const double distanceBetweenLanes, const double speed,
      const double xMove, 
      double & newLateralOffset, double& newHeadingOffset,
      double &headingChangeRate);

   virtual void updatePsrWithNewState(
      const int roadLaneNum,
      const bool isSwitchingLanes,
      const int roadSegmentIndex,
      const double positionInLane,
      double lateralOffset,
      const double heading,
      const double newSpeed,
      const double newAccel,
      DtRoadSegment* currentSegment);

   virtual void computeBodyMotionVectors(const double speed, 
      const double accel, const double turnRate, const double dt,
      DtVector & bodyVelocity, DtVector & bodyAcceleration);

   virtual void computeRepositoryUpdateVariables(
      const DtVector & newLocalPosition, 
      const DtVector & newBodyVelocity,
      const DtVector & newBodyAcceleration,
      const DtDcm & newLocalOrientation,
      const double newRotationRate,
      DtVector & newRelativeVelocity,
      DtVector & newRelativeAcceleration,
      DtTaitBryan & newLocalOrientationTb,
      DtVector & newBodyRotationalVelocity
      );

   virtual void useFuel(double fuelUsed);

   virtual bool isEntityImmobilized();

   virtual double fuelLeft() const;

   virtual void updateRepository(const DtVector & localPosition, 
      const DtVector & localVelocity, const DtVector & localAcceleration, 
      const DtTaitBryan & localOrientation, const DtVector& rotationalVelocity,
      double newHeading);

   virtual void calculateOrientationAndPosition(
      const DtVector& newLocalProjectedLocation, const double newHeading, 
      DtDcm& newLocalOrientation, DtVector& newLocalPosition);
   
   virtual void calculateOriginLocation(
      const DtVector & turnCenterLocation,
      const double heading,
      DtVector & originLocation);

   bool groundClamp(const double heading, const DtVector& location, 
      DtAttachedTerrain& terrain, DtVector& newNormal);

   void groundClampHighFidelity(const double heading, const DtVector& location, 
      DtAttachedTerrain& terrain, DtVector& newNormal);

   void groundClampLowFidelity(const double heading, const DtVector& location, 
      DtAttachedTerrain& terrain, DtVector& newNormal);

   DtLocalObject* entity() const;
   DtVrfMovingObjectStateRepository* movingSR();


   virtual void steerTowardLane(DtRoadSegment* segment, const double desiredAcceleration);

   //virtual double distanceOfActualPositionBehindLanePosition(

   virtual double lookAheadDistance() const;

   virtual bool createAutoActuatorPorts();

   virtual bool createThrottlePort();
   virtual bool createSteeringPort();
   virtual bool createBrakePort();
   virtual bool createGearPort();
   virtual bool createParkingBrakePort();

   virtual DtConstrainedAnalogOutputPort* throttlePort();
   virtual DtConstrainedAnalogOutputPort* steeringPort();
   virtual DtConstrainedAnalogOutputPort* brakePort();
   virtual DtConstrainedIntegerOutputPort* gearPort();
   virtual DtBooleanOutputPort* parkingBrakePort();


private:
   DtRoadMovementUpdater();

   DtRoadMovementUpdater(const DtRoadMovementUpdater & orig);

   const DtRoadMovementUpdater & operator=(const 
      DtRoadMovementUpdater & orig);

protected:
   DtRoadMovementController * myController;
   DtRailMovementControllerPSR *myRailMovementPSR;

   const DtAttachedTerrain* myAttachedTerrain;
   bool     myUseHighFidelityGroundClamping;
   bool     myDrivingOnLeft;

   
   DtUpdateRepositoryOutputPortGroup* myUpdateRepositoryOutputPortGroup;

   bool           myWarnedAboutImmobilizedOnce;

   DtVector myLocalNewTargetPosition;
   DtVector myLocalEquivalentInLanePosition;

   DtPlatformLocalObjectFacade myPlatformLocalObjectFacade;
   const DtRoadMovementDescriptor* myRoadMovementDescriptor = nullptr;


   DtConstrainedAnalogOutputPort* myThrottlePort;

   DtConstrainedAnalogOutputPort* mySteeringPort;

   DtConstrainedAnalogOutputPort* myBrakePort;

   DtConstrainedIntegerOutputPort* myGearPort;

   DtBooleanOutputPort* myParkingBrakePort;

   DtOutputPortGroup* myAutomotiveControlPortGroup;


};