Joint.cpp

Go to the documentation of this file.

#include "StdAfx.h"
#include "IMovableItemCallback.h"
#include "ISimGUICallback.h"
#include "AnimatBase.h"

#include "Node.h"
#include "IPhysicsMovableItem.h"
#include "IPhysicsBody.h"
#include "BoundingBox.h"
#include "MovableItem.h"
#include "BodyPart.h"
#include "Joint.h"
#include "ReceptiveField.h"
#include "ContactSensor.h"
#include "RigidBody.h"
#include "Structure.h"
#include "Organism.h"
#include "ActivatedItem.h"
#include "ActivatedItemMgr.h"
#include "DataChartMgr.h"
#include "ExternalStimuliMgr.h"
#include "KeyFrame.h"
#include "SimulationRecorder.h"
#include "OdorType.h"
#include "Odor.h"
#include "Light.h"
#include "LightManager.h"
#include "Simulator.h"

namespace AnimatSim
{
        namespace Environment
        {

Joint::Joint()
{
        m_lpChild = NULL;
        m_fltPosition = 0;
        m_fltVelocity = 0;
        m_fltForce = 0;
        m_fltSize = 0.02f;
        m_bEnableLimits = true;

    for(int iIdx=0; iIdx<6; iIdx++)
        m_aryRelaxations[iIdx] = NULL;

    m_lpFriction = NULL;
}

Joint::~Joint()
{

try
{
        //We also do not delete our references to these objects.
        m_lpParent = NULL;
        m_lpChild = NULL;

    ClearRelaxations();

    if(m_lpFriction) {delete m_lpFriction; m_lpFriction = NULL;}
}
catch(...)
{Std_TraceMsg(0, "Caught Error in desctructor of Joint\r\n", "", -1, false, true);}
}

                        
bool Joint::UsesRadians() {return true;}

float Joint::Size() {return m_fltSize;};

void Joint::Size(float fltVal, bool bUseScaling)
{
        Std_IsAboveMin((float) 0, fltVal, true, "Joint.Size");
        if(bUseScaling)
                m_fltSize = fltVal * m_lpSim->InverseDistanceUnits();
        else
                m_fltSize = fltVal;

        Resize();
}



bool Joint::EnableLimits() {return m_bEnableLimits;};

void Joint::EnableLimits(bool bVal) {m_bEnableLimits = bVal;}

ConstraintRelaxation *Joint::Relaxation1() {return m_aryRelaxations[0];}

void Joint::Relaxation1(ConstraintRelaxation *lpRelax)
{
    if(m_aryRelaxations[0])
    {
        delete m_aryRelaxations[0];
        m_aryRelaxations[0] = NULL;
    }

    m_aryRelaxations[0] = lpRelax;
}

void Joint::Relaxation1(std::string strXml)
{
        CStdXml oXml;
        oXml.Deserialize(strXml);
        oXml.FindElement("Root");
        
    Relaxation1(LoadConstraintRelaxation(oXml, "Relaxation1"));
}

ConstraintRelaxation *Joint::Relaxation2() {return m_aryRelaxations[1];}

void Joint::Relaxation2(ConstraintRelaxation *lpRelax)
{
    if(m_aryRelaxations[1])
    {
        delete m_aryRelaxations[1];
        m_aryRelaxations[1] = NULL;
    }

    m_aryRelaxations[1] = lpRelax;
}

void Joint::Relaxation2(std::string strXml)
{
        CStdXml oXml;
        oXml.Deserialize(strXml);
        oXml.FindElement("Root");
        
    Relaxation2(LoadConstraintRelaxation(oXml, "Relaxation2"));
}

ConstraintRelaxation *Joint::Relaxation3() {return m_aryRelaxations[2];}

void Joint::Relaxation3(ConstraintRelaxation *lpRelax)
{
    if(m_aryRelaxations[2])
    {
        delete m_aryRelaxations[2];
        m_aryRelaxations[2] = NULL;
    }

    m_aryRelaxations[2] = lpRelax;
}

void Joint::Relaxation3(std::string strXml)
{
        CStdXml oXml;
        oXml.Deserialize(strXml);
        oXml.FindElement("Root");
        
    Relaxation3(LoadConstraintRelaxation(oXml, "Relaxation3"));
}

ConstraintRelaxation *Joint::Relaxation4() {return m_aryRelaxations[3];}

void Joint::Relaxation4(ConstraintRelaxation *lpRelax)
{
    if(m_aryRelaxations[3])
    {
        delete m_aryRelaxations[3];
        m_aryRelaxations[3] = NULL;
    }

    m_aryRelaxations[3] = lpRelax;
}

void Joint::Relaxation4(std::string strXml)
{
        CStdXml oXml;
        oXml.Deserialize(strXml);
        oXml.FindElement("Root");
        
    Relaxation4(LoadConstraintRelaxation(oXml, "Relaxation4"));
}

ConstraintRelaxation *Joint::Relaxation5() {return m_aryRelaxations[4];}

void Joint::Relaxation5(ConstraintRelaxation *lpRelax)
{
    if(m_aryRelaxations[4])
    {
        delete m_aryRelaxations[4];
        m_aryRelaxations[4] = NULL;
    }

    m_aryRelaxations[4] = lpRelax;
}

void Joint::Relaxation5(std::string strXml)
{
        CStdXml oXml;
        oXml.Deserialize(strXml);
        oXml.FindElement("Root");
        
    Relaxation5(LoadConstraintRelaxation(oXml, "Relaxation5"));
}

ConstraintRelaxation *Joint::Relaxation6() {return m_aryRelaxations[5];}

void Joint::Relaxation6(ConstraintRelaxation *lpRelax)
{
    if(m_aryRelaxations[5])
    {
        delete m_aryRelaxations[5];
        m_aryRelaxations[5] = NULL;
    }

    m_aryRelaxations[5] = lpRelax;
}

void Joint::Relaxation6(std::string strXml)
{
        CStdXml oXml;
        oXml.Deserialize(strXml);
        oXml.FindElement("Root");
        
    Relaxation6(LoadConstraintRelaxation(oXml, "Relaxation6"));
}

ConstraintFriction *Joint::Friction() {return m_lpFriction;}

void Joint::Friction(ConstraintFriction *lpRelax)
{
    if(m_lpFriction)
    {
        delete m_lpFriction;
        m_lpFriction = NULL;
    }

    m_lpFriction = lpRelax;
}

void Joint::Friction(std::string strXml)
{
        CStdXml oXml;
        oXml.Deserialize(strXml);
        oXml.FindElement("Root");
        
    Friction(LoadConstraintFriction(oXml));
}


float Joint::GetPositionWithinLimits(float fltPos)
{return fltPos;}

float Joint::GetLimitRange()
{return -1;}

int Joint::VisualSelectionType() {return JOINT_SELECTION_MODE;}

RigidBody *Joint::Child() {return m_lpChild;}

void Joint::Child(RigidBody *lpValue) 
{
        m_lpChild = lpValue;
        if(m_lpPhysicsMovableItem)
                m_lpPhysicsMovableItem->Physics_SetChild(lpValue);
}

float Joint::JointPosition() {return m_fltPosition;}

void Joint::JointPosition(float fltPos) {m_fltPosition = fltPos;}

float Joint::JointVelocity() {return m_fltVelocity;}

void Joint::JointVelocity(float fltVel) {m_fltVelocity = fltVel;}

float Joint::JointForce() {return m_fltForce;}

void Joint::JointForce(float fltForce) {m_fltForce = fltForce;}

void Joint::WakeDynamics()
{
    if(m_lpParent)
        m_lpParent->WakeDynamics();

    if(m_lpChild)
        m_lpChild->WakeDynamics();
}

void Joint::CreateJoint()
{}

void Joint::Initialize()
{
    BodyPart::Initialize();

    for(int iIdx=0; iIdx<6; iIdx++)
        if(m_aryRelaxations[iIdx]) m_aryRelaxations[iIdx]->Initialize();

    if(m_lpFriction) m_lpFriction->Initialize();

}

void Joint::StepSimulation()
{
    BodyPart::StepSimulation();
        UpdateData();
}


float *Joint::GetDataPointer(const std::string &strDataType)
{
        std::string strType = Std_CheckString(strDataType);

        float *lpData = NULL;

        if(strType == "ENABLE")
                return &m_fltEnabled;

        if(m_lpPhysicsMovableItem)
        {
                float *lpData = NULL;
                lpData = m_lpPhysicsMovableItem->Physics_GetDataPointer(strDataType);
                if(lpData) return lpData;
        }

        return BodyPart::GetDataPointer(strDataType);
}

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

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

        if(strType == "ENABLELIMITS")
        {
                EnableLimits(Std_ToBool(strValue));
                return true;
        }

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

    if(strType == "RELAXATION1")
    {
        Relaxation1(strValue);
    }

    if(strType == "RELAXATION2")
    {
        Relaxation2(strValue);
    }

    if(strType == "RELAXATION3")
    {
        Relaxation3(strValue);
    }

    if(strType == "RELAXATION4")
    {
        Relaxation4(strValue);
    }

    if(strType == "RELAXATION5")
    {
        Relaxation5(strValue);
    }

    if(strType == "RELAXATION6")
    {
        Relaxation6(strValue);
    }

        if(strType == "FRICTION")
    {
        Friction(strValue);
    }

        //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 Joint::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
{
        BodyPart::QueryProperties(aryProperties);

        aryProperties.Add(new TypeProperty("Enable", AnimatPropertyType::Boolean, AnimatPropertyDirection::Get));

        aryProperties.Add(new TypeProperty("EnableLimits", AnimatPropertyType::Boolean, AnimatPropertyDirection::Set));
        aryProperties.Add(new TypeProperty("Size", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
        aryProperties.Add(new TypeProperty("Relaxation1", AnimatPropertyType::Xml, AnimatPropertyDirection::Set));
        aryProperties.Add(new TypeProperty("Relaxation2", AnimatPropertyType::Xml, AnimatPropertyDirection::Set));
        aryProperties.Add(new TypeProperty("Relaxation3", AnimatPropertyType::Xml, AnimatPropertyDirection::Set));
        aryProperties.Add(new TypeProperty("Relaxation4", AnimatPropertyType::Xml, AnimatPropertyDirection::Set));
        aryProperties.Add(new TypeProperty("Relaxation5", AnimatPropertyType::Xml, AnimatPropertyDirection::Set));
        aryProperties.Add(new TypeProperty("Relaxation6", AnimatPropertyType::Xml, AnimatPropertyDirection::Set));
        aryProperties.Add(new TypeProperty("Friction", AnimatPropertyType::Xml, AnimatPropertyDirection::Set));
}

void Joint::AddExternalNodeInput(int iTargetDataType, float fltInput) {}

void Joint::ResetSimulation()
{
        BodyPart::ResetSimulation();

        if(m_lpPhysicsMovableItem)
                m_lpPhysicsMovableItem->Physics_ResetSimulation();

        JointPosition(0);
        JointVelocity(0);
        JointForce(0);
}

void Joint::AfterResetSimulation()
{
        if(m_lpPhysicsMovableItem)
                m_lpPhysicsMovableItem->Physics_AfterResetSimulation();
}

void Joint::UpdatePhysicsPosFromGraphics()
{
        UpdateAbsolutePosition();
}

void Joint::ClearRelaxations()
{
    for(int iIdx=0; iIdx<6; iIdx++)
    if(m_aryRelaxations[iIdx]) 
    {
        delete m_aryRelaxations[iIdx]; 
        m_aryRelaxations[iIdx] = NULL;
    }
}

void Joint::Load(CStdXml &oXml)
{
        BodyPart::Load(oXml);

        oXml.IntoElem();  //Into Joint Element

        if(!m_lpParent)
                THROW_PARAM_ERROR(Al_Err_lParentNotDefined, Al_Err_strParentNotDefined, "JointID", m_strName);

        if(!m_lpChild)
                THROW_PARAM_ERROR(Al_Err_lChildNotDefined, Al_Err_strChildNotDefined, "JointID", m_strName);

        //Reset the absolute position differently for a joint. It is derived from the child object, not the parent.
        //AbsolutePosition(m_lpChild->AbsolutePosition() + m_oPosition);

        EnableLimits(oXml.GetChildBool("EnableLimits", m_bEnableLimits));

        Size(oXml.GetChildFloat("Size", m_fltSize));

    ClearRelaxations();

    if(m_lpFriction) {delete m_lpFriction; m_lpFriction = NULL;} 

    if(oXml.FindChildElement("Relaxation1", false))
        m_aryRelaxations[0] = LoadConstraintRelaxation(oXml, "Relaxation1");

    if(oXml.FindChildElement("Relaxation2", false))
        m_aryRelaxations[1] = LoadConstraintRelaxation(oXml, "Relaxation2");

    if(oXml.FindChildElement("Relaxation3", false))
        m_aryRelaxations[2] = LoadConstraintRelaxation(oXml, "Relaxation3");

    if(oXml.FindChildElement("Relaxation4", false))
        m_aryRelaxations[3] = LoadConstraintRelaxation(oXml, "Relaxation4");

    if(oXml.FindChildElement("Relaxation5", false))
        m_aryRelaxations[4] = LoadConstraintRelaxation(oXml, "Relaxation5");

    if(oXml.FindChildElement("Relaxation6", false))
        m_aryRelaxations[5] = LoadConstraintRelaxation(oXml, "Relaxation6");

    if(oXml.FindChildElement("Friction", false))
        m_lpFriction = LoadConstraintFriction(oXml);    

        oXml.OutOfElem(); //OutOf Joint Element
}

ConstraintRelaxation *Joint::LoadConstraintRelaxation(CStdXml &oXml, std::string strName)
{
        ConstraintRelaxation *lpConstraintRelaxation = NULL;
        std::string strModule;
        std::string strType;

try
{
    oXml.FindChildElement(strName);
        oXml.IntoElem(); //Into Child Element
        strModule = oXml.GetChildString("ModuleName", "");
        strType = oXml.GetChildString("Type");
        oXml.OutOfElem(); //OutOf Child Element

        lpConstraintRelaxation = dynamic_cast<ConstraintRelaxation *>(m_lpSim->CreateObject(strModule, "ConstraintRelaxation", strType));
        if(!lpConstraintRelaxation)
                THROW_TEXT_ERROR(Al_Err_lConvertingClassToType, Al_Err_strConvertingClassToType, "ConstraintRelaxation");

        lpConstraintRelaxation->SetSystemPointers(m_lpSim, m_lpStructure, m_lpModule, this, true);
        lpConstraintRelaxation->Load(oXml);

        return lpConstraintRelaxation;
}
catch(CStdErrorInfo oError)
{
        if(lpConstraintRelaxation) delete lpConstraintRelaxation;
        RELAY_ERROR(oError);
        return NULL;
}
catch(...)
{
        if(lpConstraintRelaxation) delete lpConstraintRelaxation;
        THROW_ERROR(Std_Err_lUnspecifiedError, Std_Err_strUnspecifiedError);
        return NULL;
}
}


ConstraintFriction *Joint::LoadConstraintFriction(CStdXml &oXml)
{
        ConstraintFriction *lpConstraintFriction = NULL;
        std::string strModule;
        std::string strType;

try
{
    oXml.FindChildElement("Friction");
        oXml.IntoElem(); //Into Child Element
        strModule = oXml.GetChildString("ModuleName", "");
        strType = oXml.GetChildString("Type");
        oXml.OutOfElem(); //OutOf Child Element

        lpConstraintFriction = dynamic_cast<ConstraintFriction *>(m_lpSim->CreateObject(strModule, "ConstraintFriction", strType));
        if(!lpConstraintFriction)
                THROW_TEXT_ERROR(Al_Err_lConvertingClassToType, Al_Err_strConvertingClassToType, "ConstraintFriction");

        lpConstraintFriction->SetSystemPointers(m_lpSim, m_lpStructure, m_lpModule, this, true);
        lpConstraintFriction->Load(oXml);

        return lpConstraintFriction;
}
catch(CStdErrorInfo oError)
{
        if(lpConstraintFriction) delete lpConstraintFriction;
        RELAY_ERROR(oError);
        return NULL;
}
catch(...)
{
        if(lpConstraintFriction) delete lpConstraintFriction;
        THROW_ERROR(Std_Err_lUnspecifiedError, Std_Err_strUnspecifiedError);
        return NULL;
}
}


        }                       //Environment
}                               //AnimatSim