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
{
    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