DtRk4MotionModel.h

Go to the documentation of this file.

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


#pragma once

#include <vrvTriton/vrvTriton.h>

#include <vrvUtil/DtSignalConnectionManager.h>

#include <matrix/vlVector.h>
#include <matrix/vlTaitBryan.h>

namespace makVrv
{
   class DtTritonManager;
   class Dt3DLineModelInstanceSegment;
   class DtDe;
   class DtCoordinateConverter;

   class DtRk4MotionModel : protected DtSignalConnectionManager
   {
   public:
      DtRk4MotionModel(makVrv::DtDe& de);

      virtual ~DtRk4MotionModel();

      void tick(double dt);

      DtVector velocity() const;

      DtVector position() const;

      DtTaitBryan orientation() const;

      void setPosition(const DtVector& position);

      void setOrientation(const DtTaitBryan& orientation);

      void setTargetOrientation(const DtTaitBryan& orientation);

      void setSpeed(double speed);

      void sink();

      void unsink();

      bool isSinking();

      void setSinkingSpeedScale( double scale );

      double sinkingSpeedScale() const;

      void setRotationalInertia(double pitchInertia, double rollInertia);

      void applyForce(const DtVector& offset, const DtVector& force);

      void applyHeadingTorque(double magnitude);

      void applyRollTorque(double magnitude);

      struct BuoyancySample
      {
         BuoyancySample(DtDe& de, DtVector offset, double area, double extent, double density, double waterline);

         double computeForce(const DtVector& pos, const DtVector& vel, const DtDcm& ori);

         double computeFlatForce(const DtVector& pos, const DtVector& vel, const DtTaitBryan& ori);

         BuoyancySample& operator=(const BuoyancySample& obj);

         double myExtent;

         double myDensity;

         double myArea;

         double myWaterline;

         DtVector myOffset;
         DtDe& myDe;
         DtRk4MotionModel* myParent;

         double myWaterAltitude;

         bool myIsGeocentric;
#if 0
         enum ImpactState
         {
            AboveWater,
            BelowWater,
            Impacted
         };
         ImpactState myImpactState; // whether sample went from above to below water surface last frame
#endif
      };

      void addBuoyancySample(BuoyancySample* bs);

      typedef std::vector<BuoyancySample*> BuoyancySampleVec;

      BuoyancySampleVec& buoyancySamples();

      void setOrigin(const DtVector& pos);

      DtVector origin() const;

      void moveOrigin(const DtVector& pos);
      
      static const double theTimeStep;

      static const double theGeocentricWaterLineNudge;
      
   private:
      friend struct BuoyancySample;

      DtVector worldPos(const DtVector& bodyOffset) const;

      DtTaitBryan worldOri(const DtTaitBryan& bodyOffset) const;

      DtVector localPos(const DtVector& pos) const;

      DtTaitBryan localOri(const DtTaitBryan& ori) const;

      virtual void slot_coordinateSystemChanged();

   private:
      DtCoordinateConverter* myCoordinateConverter;

      struct AppliedForce
      {
         DtVector offset;
         DtVector force;
      };

      struct State
      {
         DtVector position;
         DtVector velocity;
         DtTaitBryan orientation; // heading, roll, pitch
         DtTaitBryan rotationalVelocity;
      };

      struct Derivative
      {
         Derivative();
         double dz;
         double dv_z;
         double dy;
         double dv_y;
         double dx;
         double dv_x;
         double dpitch;
         double dv_pitch;
         double droll;
         double dv_roll;
         double dheading;
         double dv_heading;
      };

      struct Acceleration
      {
         double a_z;
         double a_y;
         double a_x;
         double a_pitch;
         double a_roll;
         double a_heading;
      };

      Derivative evaluate(const State& initial, double dt, const Derivative& d);

      Acceleration acceleration(const State& initial, double dt);

      AppliedForce myAppliedForce;
      Dt3DLineModelInstanceSegment* myDebugLine[8];
      State myState;
      bool mySinking;
      double myHeadingTorque;
      double myRollTorque;
      double myPitchTorque;
      double myForwardSpeed;
      double mySinkingDensity;
      bool mySinkingFloat[6];
      double mySinkingSpeedScale;

      double myRollInertia;
      double myPitchInertia;

      DtVector myOriginPos;

      DtDe& myDe;

      BuoyancySampleVec myBuoyancySamples;
   };
}