BlConstraintRelaxation.cpp

// BlConstraintRelaxation.cpp: implementation of the BlConstraintRelaxation class.
//

#include "StdAfx.h"
#include "BlConstraintRelaxation.h"
#include "BlJoint.h"
#include "BlMotorizedJoint.h"
#include "BlRigidBody.h"
#include "BlSimulator.h"

namespace BulletAnimatSim
{
        namespace Environment
        {

// Construction/Destruction

BlConstraintRelaxation::BlConstraintRelaxation()
{
    m_lpBlJoint = NULL;
    m_lpConstraint = NULL;
    m_fltMaxLimit = 0; 
    m_fltMinLimit = 0;
    m_fltEqPos = 0;
    m_fltInvDamping = 1/m_fltDamping;
    m_bDisallowSpringEnable = false;
}

BlConstraintRelaxation::~BlConstraintRelaxation()
{
}

float BlConstraintRelaxation::MinLimit() {return m_fltMinLimit;}

void BlConstraintRelaxation::MinLimit(float fltVal, bool bUseScaling) 
{
        if(bUseScaling)
                fltVal = fltVal * m_lpSim->InverseDistanceUnits();
        else
                fltVal = fltVal;

        m_fltMinLimit = fltVal;
        SetRelaxationProperties();
}

float BlConstraintRelaxation::MaxLimit() {return m_fltMaxLimit;}

void BlConstraintRelaxation::MaxLimit(float fltVal, bool bUseScaling) 
{
        if(bUseScaling)
                fltVal = fltVal * m_lpSim->InverseDistanceUnits();
        else
                fltVal = fltVal;

        m_fltMaxLimit = fltVal;
        SetRelaxationProperties();
}

float BlConstraintRelaxation::EqPos() {return m_fltEqPos;}

void BlConstraintRelaxation::EqPos(float fltVal, bool bUseScaling) 
{
        Std_IsAboveMin((float) 0, fltVal, true, "EqPos", true);

        if(bUseScaling)
                fltVal = fltVal * m_lpSim->InverseDistanceUnits();
        else
                fltVal = fltVal;

        m_fltEqPos = fltVal;
        SetRelaxationProperties();
}
void BlConstraintRelaxation::Damping(float fltVal, bool bUseScaling) 
{
        Std_IsAboveMin((float) 0, fltVal, true, "Damping", true);

        if(bUseScaling)
                fltVal = fltVal/m_lpSim->DisplayMassUnits();

        m_fltDamping = fltVal;
    
    if(m_fltDamping <= 0)
        m_fltInvDamping = 1;
    else
        m_fltInvDamping = 1/m_fltDamping;

        SetRelaxationProperties();
}

bool BlConstraintRelaxation::DisallowSpringEnable()
{
    return m_bDisallowSpringEnable;
}

void BlConstraintRelaxation::DisallowSpringEnable(bool bVal)
{
    m_bDisallowSpringEnable = bVal;
}

void BlConstraintRelaxation::Initialize()
{
        ConstraintRelaxation::Initialize();

    if(m_lpSim && m_lpNode)
    {
        m_lpBlJoint = dynamic_cast<BlJoint *>(m_lpNode);
        if(m_lpBlJoint)
            m_lpConstraint = dynamic_cast<btAnimatGeneric6DofConstraint *>(m_lpBlJoint->Constraint());
    }

        SetRelaxationProperties();
}

void BlConstraintRelaxation::CreateDefaultUnits()
{
    ConstraintRelaxation::CreateDefaultUnits();

    if(m_fltDamping <= 0)
        m_fltInvDamping = 1;
    else
        m_fltInvDamping = 1/m_fltDamping;

    m_fltMinLimit *= m_lpSim->InverseDistanceUnits();
    m_fltMaxLimit *= m_lpSim->InverseDistanceUnits();
    m_fltEqPos *= m_lpSim->InverseDistanceUnits();
}

bool BlConstraintRelaxation::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
{
        std::string strType = Std_CheckString(strDataType);

        if(ConstraintRelaxation::SetData(strType, strValue, false))
                return true;

        if(strType == "MINLIMIT")
        {
                MinLimit((float) atof(strValue.c_str()));
                return true;
        }

    if(strType == "MAXLIMIT")
        {
                MaxLimit((float) atof(strValue.c_str()));
                return true;
        }

    if(strType == "EQPOS")
        {
                EqPos((float) atof(strValue.c_str()));
                return true;
        }
        
        //If it was not one of those above then we have a problem.
        if(bThrowError)
                THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);

        return false;
}

void BlConstraintRelaxation::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
{
        ConstraintRelaxation::QueryProperties(aryProperties);

        aryProperties.Add(new TypeProperty("MinLimit", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
        aryProperties.Add(new TypeProperty("MaxLimit", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
        aryProperties.Add(new TypeProperty("EqPos", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
}

void BlConstraintRelaxation::Load(CStdXml &oXml)
{
        ConstraintRelaxation::Load(oXml);

        oXml.IntoElem();  //Into ConstraintRelaxation Element

        MinLimit(oXml.GetChildFloat("MinLimit", m_fltMinLimit));
        MaxLimit(oXml.GetChildFloat("MaxLimit", m_fltMaxLimit));
        EqPos(oXml.GetChildFloat("EqPos", m_fltEqPos));

    oXml.OutOfElem(); //OutOf ConstraintRelaxation Element

}

void BlConstraintRelaxation::SetRelaxationProperties()
{
    if(m_lpSim && m_lpNode && m_lpConstraint && m_lpBlJoint)
    {
        if(m_bDisallowSpringEnable)
            m_lpBlJoint->AxisConstraintSpringEnableChanged(m_bEnabled);
        else
            m_lpConstraint->enableSpring(m_iCoordinateID, m_bEnabled);

        m_lpConstraint->setEquilibriumPoint(m_iCoordinateID, m_fltEqPos);
        m_lpConstraint->setStiffness(m_iCoordinateID, m_fltStiffness);
        m_lpConstraint->setDamping(m_iCoordinateID, m_fltInvDamping);

        m_lpBlJoint->SetLimitValues();
    }
}

        }                       // Visualization
}                               //BulletAnimatSim