CInternalCable.h

 
 
#ifndef CInternalCable_H
#define CInternalCable_H
 
// Includes
#include <string>
#include <SimObject.h>
#include "ICurrentVel.h"
#include "sfh/timers/StopWatch.h"
#include "cable/subroutines/CCableElDynStiff.h"
#include "sfh/constants.h"
#include "sfh/math/math.h"
#include "sfh/linalg.h"
#include "sfh/math/array.h"
 
 
#define ATTACH_SPHERE
 
 
struct SCableSpec
{
    int         iNumElements;
    double      dDensity;
    double      dNumericalDamping;
    double      dDiameter;
    double      dE;
    double      dDampingRatio;
    double      dFluidRho;
    double      dMaxStep;
    double      dMaxTension;
    double      dMaxAcceleration;
    double      dStepSafetyFactor;
    double      dMeanTension0;
    double      dEAdaptationPeriod;
    std::string sMaterialName;
    int         iCableFaces;
    std::string sCableName;
    double      dDrawScale;
 
    SCableSpec()
    {
        Reset();
    }
    void Reset()
    {
        iNumElements        = 2;
        dDensity            = 1100;
        dNumericalDamping   = 1;                    
        dDiameter           = 0.01;                 
        dE                  = 1e9;                  
        dDampingRatio       = 1;                    
        dFluidRho           = 1025;                 
        dMaxStep            = 1e-10;                
        dMaxTension         = 1e10;                 
        dMaxAcceleration    = 1e10;                 
        dStepSafetyFactor   = 50;                   
        dMeanTension0       = 0;                    
        dEAdaptationPeriod  = 0;                    
        sMaterialName       = "Trawl/SteelWire";
        iCableFaces         = 5;
        sCableName          = "";
        dDrawScale          = 1;
    }
    double CalcEA()
    {
        return sfh::pi * 0.25 * dDiameter * dDiameter* dE;
    }
};
 
struct SCableRenderSpec
{
    double  dDeltaTheta;
    double* adTheta;
    double* adSinTheta;
    double* adCosTheta;
    double  dTextureScale;
    double* adCirclePos;
    double* adFacePosNormal;
    double* adTextureCoordU;
    double* adTextureCoordV;
    int     iNumRenderEdges;
    double  dDrawDiam;
    int*    aiIndex;
    int     iSizeIndexBuffer;
 
    SCableRenderSpec(int iNumFaces, int iNumElements, double dDrawDiameter)
    {
        dDrawDiam = dDrawDiameter;
        iNumRenderEdges = iNumFaces + 1;
        iSizeIndexBuffer = iNumElements * iNumRenderEdges * 2;
        dTextureScale   = 5 * (sfh::pi  * dDrawDiam);
        aiIndex         = new int [iSizeIndexBuffer];
        dDeltaTheta     = 2 * sfh::pi / iNumFaces;
        adTheta         = new double [iNumRenderEdges];
        adSinTheta      = new double [iNumRenderEdges];
        adCosTheta      = new double [iNumRenderEdges];
        adCirclePos     = new double [ (iNumElements + 1) * (iNumRenderEdges) * 3 ];
        adFacePosNormal = new double [ (iNumElements + 1) * (iNumRenderEdges) * 3 ];
        adTextureCoordU = new double [ iNumRenderEdges ];
        adTextureCoordV = new double [ iNumElements + 1 ];
        adTextureCoordV[0] = 0;
        for (unsigned short i = 0; i < iNumRenderEdges; i++)
        {
            adTheta[i]          = i * dDeltaTheta;
            adSinTheta[i]       = sin(adTheta[i]);
            adCosTheta[i]       = cos(adTheta[i]);
            adTextureCoordU[i]  = adTheta[i] * (sfh::pi  * dDrawDiam) / dTextureScale;
        }
        int i = 0;
        for (int iElement = 0; iElement < iNumElements; iElement++)
        {
            for (int iEdge = 0; iEdge < iNumRenderEdges; iEdge++)
            {
                aiIndex[i++] =  iElement      * iNumRenderEdges + iEdge;
                aiIndex[i++] = (iElement + 1) * iNumRenderEdges + iEdge;
            }
        }
 
    }
 
    ~SCableRenderSpec()
    {
        delete [] adTheta;
        delete [] adSinTheta;
        delete [] adCosTheta;
        delete [] adCirclePos;
        delete [] adFacePosNormal;
        delete [] adTextureCoordU;
        delete [] adTextureCoordV;
    }
 
    void Update(int iElement, 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, dDrawDiam/2.0, 3);
        sfh::math::ArrayMultiply(adNormal2, dDrawDiam/2.0, 3);
        adTextureCoordV[iElement + 1] = adTextureCoordV[iElement] + dL0 / dTextureScale * 3;
 
        for (unsigned short iFace = 0; iFace < iNumRenderEdges; iFace++)
        {
            for(unsigned short iInd = 0; iInd < 3; iInd++)
            {
                if(iElement == 0)
                {
                    adFacePosNormal[iElement * ((iNumRenderEdges) * 3) + 3 * iFace + iInd]  = adSinTheta[iFace]*adNormal1[iInd] + adCosTheta[iFace]*adNormal2[iInd];
                    adCirclePos[iElement     * ((iNumRenderEdges) * 3) + 3 * iFace + iInd]  = adElPosA[iInd] + adFacePosNormal[iElement * ((iNumRenderEdges)* 3) + 3 * iFace + iInd];
                }
                adFacePosNormal[(iElement + 1) * ((iNumRenderEdges) * 3) + 3 * iFace + iInd]    = adSinTheta[iFace]*adNormal1[iInd] + adCosTheta[iFace]*adNormal2[iInd];
                adCirclePos[(iElement + 1)     * ((iNumRenderEdges) * 3) + 3 * iFace + iInd]    = adElPosB[iInd] + adFacePosNormal[(iElement + 1) * ((iNumRenderEdges) * 3) + 3 * iFace + iInd];
            }
        }
    }
 
private:
    // Varaiables to avoid excessive allocations
    double adNotParallel[3];
    double adNormal1[3];
    double adNormal2[3];
};
 
//Class definition
class CInternalCable
{
public:
    CInternalCable(ISimObjectCreator* pCreator, const SCableSpec& CableSpec);
 
    ~CInternalCable();
 
    // Member functions
    virtual void FinalSetup(const double dT, const double *const adX, ISimObjectCreator* const pCreator);
 
    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);
 
    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 dNewLength, 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* pOgreRoot);
 
    void            RenderUpdate( const double* const adX, const double* adPosA, const double* adPosB);
#endif
 
    unsigned long           m_iNumElements;     
    unsigned long           m_iNumNodes;        
protected:
    // Deletes all node forces.
    void ResetAllNodeForces();
 
    // 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);
 
    // Calculates the derivatives of the states of the cable.
    void CalcDerivatives(const double* const adX,
        double* const       adXDot);
 
    // Adds the forces from one element to the array of node forces.
    virtual void AddElementForce(int iCableElement,
        const double adPosA[3],
        const double adVelA[3],
        const double adPosB[3],
        const double adVelB[3],
        double adForceA[3],
        double adForceB[3],
        double dT);
 
    void UpdateInertia();
 
    unsigned long*      m_aIStatePos;           
    unsigned long*      m_aIStateVel;           
 
    // Member variables
    SCableSpec          m_CableSpec;        
    double              m_dEA;              
    sfh::timers::StopWatch  m_StopWatch;
 
    CCableElDynStiff**  m_apCableElements;  
    double*             m_adInertiaA;       
    double*             m_adInertiaB;       
    double**            m_aadForceA;        
    double**            m_aadForceB;        
 
    ICurrentVel**       m_ppCurrentVel;
    double              m_adElVelWater[3];
    double              m_adElMeanPos[3];
    double              m_adElMeanVel[3];
 
#  ifdef FH_VISUALIZATION
    SCableRenderSpec*       m_pRenderSpec;
    Ogre::Root*             m_pOgreRoot;            
    Ogre::SceneManager*     m_pSceneMgr;            
    Ogre::Entity*           m_pRenderElement;   
    Ogre::SceneNode*        m_pRenderNode;      
    Ogre::ManualObject*     m_pCableRenderObj;
    double                  m_dDrawDiam;        
#    ifdef ATTACH_SPHERE
    Ogre::Entity*           m_aRenderSphere[2];
    Ogre::SceneNode*        m_aRenderNodeSphere[2];
#    endif
#  endif
};
 
 
#endif