CLinearSpring.cpp

This source file shows how to implement a linear spring. It has no states, and most "work" is done in the outputport functions. See also the class documentation.

 
// Includes
#include "CLinearSpring.h"
#include <cmath>
 
//==================================================================================================
// FUNCTION NAME:           CLinearSpring constructor
//==================================================================================================
CLinearSpring::CLinearSpring(std::string sSimObjectName, ISimObjectCreator* pCreator)
:SimObject(sSimObjectName)
{
#ifdef FH_VISUALIZATION
    m_iNumPoints = 2; // The number of points for the visualization.
#endif
    //Input ports
    pCreator->AddInport("PosA", 3, &m_pInPosA);
    pCreator->AddInport("PosB", 3, &m_pInPosB);
 
    // Register common computation function
    pCreator->RegisterCommonCalculation(COMMON_COMPUTATION_FUNCTION(CLinearSpring::CalcOutput), &m_CalcOutputs);
 
    // Output ports
    pCreator->AddOutport("ForceA", 3, PORT_FUNCTION(CLinearSpring::ForceA));
    pCreator->AddOutport("ForceB", 3, PORT_FUNCTION(CLinearSpring::ForceB));
 
 
    // Parameters
    pCreator->GetDoubleParam("Stiffness", &m_dStiffness);
    pCreator->GetDoubleParam("RelaxedLength", &m_dRelaxedLength);
}
 
//==================================================================================================
// FUNCTION NAME:           CLinearSpring destructor
//==================================================================================================
//
CLinearSpring::~CLinearSpring()
{
#ifdef FH_VISUALIZATION
    //delete m_lines;
#endif
 
}
 
 
 
 
void CLinearSpring::CalcOutput(const double dT, const double* const adX)
{
    const double* adPosA = m_pInPosA->GetPortValue(dT,adX);
    const double* adPosB = m_pInPosB->GetPortValue(dT,adX);
    double adDeltaPos[3];
    double dL = 0;
    for(int i = 0;i < 3;i++)
    {
        adDeltaPos[i] = adPosA[i]-adPosB[i];
        dL+=adDeltaPos[i]*adDeltaPos[i];
    }
    if (dL <= 0.0)
    {
        for (int i = 0; i < 3; i++)
        {
            m_adOutForceA[i] = 0.0;
            m_adOutForceB[i] = 0.0;
        }
    }
    else
    {
        dL = sqrt(dL);
        double dDeltaL = dL - m_dRelaxedLength;
 
        for (int i = 0; i < 3; i++)
        {
            m_adOutForceA[i] = -m_dStiffness * dDeltaL*adDeltaPos[i] / dL;
            m_adOutForceB[i] = -m_adOutForceA[i];
        }
    }
}
 
const double* CLinearSpring::ForceA( const double dT, const double* const adX )
{
    m_CalcOutputs->ComputeFunction(dT, adX);
    return m_adOutForceA;
}
 
const double* CLinearSpring::ForceB( const double dT, const double* const adX )
{
    m_CalcOutputs->ComputeFunction(dT, adX);
    return m_adOutForceB;
}
 
 
 
 
#ifdef FH_VISUALIZATION
 
void CLinearSpring::RenderInit(Ogre::Root* const pOgreRoot, ISimObjectCreator* const pCreator)
{
 
    auto scenemgr = pOgreRoot->getSceneManager("main");
    m_pLines = new C3DLine(scenemgr, Ogre::RenderOperation::OT_LINE_LIST,2);
    //m_RenderNode->setCustomParameter(fhsim::kIndexEnableAsCameraTarget,Ogre::Vector4(fhsim::kDisableAsCameraTarget,0,0,0));
 
}
 
//==================================================================================================
// FUNCTION NAME:           RenderUpdate
//==================================================================================================
//
void CLinearSpring::RenderUpdate(const double dT, const double *const adX)
{
 
    const double* const adPos1In = m_pInPosA->GetPortValue(dT, adX);
    const double* const adPos2In = m_pInPosB->GetPortValue(dT, adX);
 
    // Assumes just values have changed, use 'setPoint' instead of 'addPoint'
    m_pLines->SetPoint(0,adPos1In[0],adPos1In[1],adPos1In[2]);
    m_pLines->SetPoint(1,adPos2In[0],adPos2In[1],adPos2In[2]);
 
    m_pLines->Update();
 
}
#endif