InternalCable.h

 
 
#ifndef CInternalCable_H
#define CInternalCable_H
 
// Includes
#include "cable/subroutines/CableElDynStiff.h"
 
#include "sfh/constants.h"
#include "sfh/linalg.h"
#include "sfh/math/array.h"
#include "sfh/math/math.h"
#include "sfh/timers/StopWatch.h"
 
#include <fhsim/simobject/SimObject.h>
#include <fhsim_environment/EnvironmentProvider.h>
#include <string>
 
 
#define ATTACH_SPHERE
 
 
struct SCableSpec
{
   int         numElements;
   double      density;
   double      numericalDamping;
   double      diameter;
   double      dE;
   double      dampingRatio;
   double      fluidRho;
   double      maxStep;
   double      maxTension;
   double      stepSafetyFactor;
   double      dMeanTension0;
   double      eAdaptationPeriod;
   std::string materialName;
   int         cableFaces;
   std::string cableName;
   double      drawScale;
 
   SCableSpec()
   {
      Reset();
   }
   void Reset()
   {
      numElements       = 2;
      density           = 1100;
      numericalDamping  = 1;     
      diameter          = 0.01;  
      dE                = 1e9;   
      dampingRatio      = 1;     
      fluidRho          = 1025;  
      maxStep           = 1e-10; 
      maxTension        = 1e10;  
      stepSafetyFactor  = 50;    
      dMeanTension0     = 0;     
      eAdaptationPeriod = 0;     
      materialName      = "Trawl/SteelWire";
      cableFaces        = 5;
      cableName         = "";
      drawScale         = 1;
   }
   double CalcEA()
   {
      return sfh::pi * 0.25 * diameter * diameter * dE;
   }
};
 
struct SCableRenderSpec
{
   double  deltaTheta;
   double* adTheta;
   double* adSinTheta;
   double* adCosTheta;
   double  textureScale;
   double* adCirclePos;
   double* adFacePosNormal;
   double* adTextureCoordU;
   double* adTextureCoordV;
   int     numRenderEdges;
   double  drawDiam;
   int*    aiIndex;
   int     sizeIndexBuffer;
 
   SCableRenderSpec(int numFaces, int numElements, double drawDiameter)
   {
      drawDiam           = drawDiameter;
      numRenderEdges     = numFaces + 1;
      sizeIndexBuffer    = numElements * numRenderEdges * 2;
      textureScale       = 5 * (sfh::pi * drawDiam);
      aiIndex            = new int[sizeIndexBuffer];
      deltaTheta         = 2 * sfh::pi / numFaces;
      adTheta            = new double[numRenderEdges];
      adSinTheta         = new double[numRenderEdges];
      adCosTheta         = new double[numRenderEdges];
      adCirclePos        = new double[(numElements + 1) * (numRenderEdges) * 3];
      adFacePosNormal    = new double[(numElements + 1) * (numRenderEdges) * 3];
      adTextureCoordU    = new double[numRenderEdges];
      adTextureCoordV    = new double[numElements + 1];
      adTextureCoordV[0] = 0;
      for (unsigned short i = 0; i < numRenderEdges; i++) {
         adTheta[i]         = i * deltaTheta;
         adSinTheta[i]      = sin(adTheta[i]);
         adCosTheta[i]      = cos(adTheta[i]);
         adTextureCoordU[i] = adTheta[i] * (sfh::pi * drawDiam) / textureScale;
      }
      int i = 0;
      for (int element = 0; element < numElements; element++) {
         for (int edge = 0; edge < numRenderEdges; edge++) {
            aiIndex[i++] = element * numRenderEdges + edge;
            aiIndex[i++] = (element + 1) * numRenderEdges + edge;
         }
      }
   }
 
   ~SCableRenderSpec()
   {
      delete[] adTheta;
      delete[] adSinTheta;
      delete[] adCosTheta;
      delete[] adCirclePos;
      delete[] adFacePosNormal;
      delete[] adTextureCoordU;
      delete[] adTextureCoordV;
   }
 
   void Update(int element, const double* adElPosA, const double* adElPosB, const double* adDirection, double dL0)
   {
      // Creating a vector not parallel to the direction of the element.
      sfh::math::FindNotParallel(adDirection, 3, adNotParallel);
 
      // Creating two vectors normal to each other and the direction of the element.
      sfh::linalg::Cross(adDirection, adNotParallel, adNormal1);
      sfh::linalg::Cross(adDirection, adNormal1, adNormal2);
      sfh::math::Normalize3(adNormal1);
      sfh::math::Normalize3(adNormal2);
      sfh::math::ArrayMultiply(adNormal1, drawDiam / 2.0, 3);
      sfh::math::ArrayMultiply(adNormal2, drawDiam / 2.0, 3);
      adTextureCoordV[element + 1] = adTextureCoordV[element] + dL0 / textureScale * 3;
 
      for (unsigned short face = 0; face < numRenderEdges; face++) {
         for (unsigned short ind = 0; ind < 3; ind++) {
            if (element == 0) {
               adFacePosNormal[element * ((numRenderEdges) * 3) + 3 * face + ind] = adSinTheta[face] * adNormal1[ind] + adCosTheta[face] * adNormal2[ind];
               adCirclePos[element * ((numRenderEdges) * 3) + 3 * face + ind]     = adElPosA[ind] + adFacePosNormal[element * ((numRenderEdges) * 3) + 3 * face + ind];
            }
            adFacePosNormal[(element + 1) * ((numRenderEdges) * 3) + 3 * face + ind] = adSinTheta[face] * adNormal1[ind] + adCosTheta[face] * adNormal2[ind];
            adCirclePos[(element + 1) * ((numRenderEdges) * 3) + 3 * face + ind]     = adElPosB[ind] + adFacePosNormal[(element + 1) * ((numRenderEdges) * 3) + 3 * face + ind];
         }
      }
   }
 
private:
   // Varaiables to avoid excessive allocations
   double adNotParallel[3];
   double adNormal1[3];
   double adNormal2[3];
};
 
// Class definition
class InternalCable
{
public:
   InternalCable(ISimObjectCreator* creator, const SCableSpec& CableSpec);
 
   ~InternalCable();
 
   // Member functions
   virtual void FinalSetup(const double dT, const double* const adX, ISimObjectCreator* const creator);
 
   virtual void OdeFcn(const double* const adPosA,
         const double* const               adVelA,
         const double* const               adPosB,
         const double* const               adVelB,
         const double* const               adX,
         double* const                     adXDot,
         double                            dT) const;
 
   void ComputeStructuralJacobian(
         const double* posA, const double* velA,
         const double* posB, const double* velB,
         const double* X, double* J, int nStates, int stateOffset) const;
 
   void AddEndpointJacobianContributions(
         const double* posA, const double* velA,
         const double* posB, const double* velB,
         int globalPosIdxA, int globalVelIdxA,
         int globalPosIdxB, int globalVelIdxB,
         double endInertiaA, double endInertiaB,
         const double* X, double* J, int nStates, int stateOffset) const;
 
   virtual bool InitializeStates(const double* const adPosA,
         const double* const                         adVelA,
         const double* const                         adPosB,
         const double* const                         adVelB,
         double* const                               adX);
 
   void UpdateCable(double newLength, double dT);
 
   void AddEndForcesAndInertia(
         double* const       adEndForceA,
         double* const       adEndForceB,
         double* const       pdEndInertiaA,
         double* const       pdEndInertiaB,
         const double* const adPosA,
         const double* const adVelA,
         const double* const adPosB,
         const double* const adVelB,
         const double* const adX,
         double              dT);
 
   double GetLength();
 
#ifdef FH_VISUALIZATION
   void RenderInit(Ogre::Root* ogreRoot);
 
   void RenderUpdate(const double* const adX, const double* adPosA, const double* adPosB);
#endif
 
   unsigned long m_numElements; 
   unsigned long m_numNodes;    
protected:
   // Deletes all node forces.
   void ResetAllNodeForces() const;
 
   // Adds the forces from each cable element to the array of node forces.
   void AddAllElementForces(const double* const adPosA,
         const double* const                    adVelA,
         const double* const                    adPosB,
         const double* const                    adVelB,
         const double* const                    adX,
         double                                 dT) const;
 
   // Calculates the derivatives of the states of the cable.
   void CalcDerivatives(const double* const adX,
         double* const                      adXDot) const;
 
   // Adds the forces from one element to the array of node forces.
   virtual void AddElementForce(int cableElement,
         const double               adPosA[3],
         const double               adVelA[3],
         const double               adPosB[3],
         const double               adVelB[3],
         double                     adForceA[3],
         double                     adForceB[3],
         double                     dT) const;
 
   void UpdateInertia();
 
   unsigned long* m_iStatePos; 
   unsigned long* m_iStateVel; 
 
   // Member variables
   SCableSpec             m_CableSpec; 
   double                 m_eA;        
   sfh::timers::StopWatch m_StopWatch;
 
   CableElDynStiff** m_cableElements; 
   double*           m_inertiaA;      
   double*           m_inertiaB;      
   double**          m_aadForceA;     
   double**          m_aadForceB;     
 
   environment::EnvironmentProvider* m_environment;
   double                            m_elVelWater[3];
   double                            m_elMeanPos[3];
   double                            m_elMeanVel[3];
 
#ifdef FH_VISUALIZATION
   SCableRenderSpec*   m_renderSpec;
   Ogre::Root*         m_ogreRoot;      
   Ogre::SceneManager* m_sceneMgr;      
   Ogre::Entity*       m_renderElement; 
   Ogre::SceneNode*    m_renderNode;    
   Ogre::ManualObject* m_cableRenderObj;
   double              m_drawDiam; 
#   ifdef ATTACH_SPHERE
   Ogre::Entity*    m_renderSphere[2];
   Ogre::SceneNode* m_renderNodeSphere[2];
#   endif
#endif
};
 
 
#endif