Structure.cpp

Go to the documentation of this file.

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

#include "Node.h"
#include "Link.h"
#include "IPhysicsMovableItem.h"
#include "IPhysicsBody.h"
#include "BoundingBox.h"
#include "MovableItem.h"
#include "BodyPart.h"
#include "Joint.h"
#include "MotorizedJoint.h"
#include "ReceptiveField.h"
#include "ContactSensor.h"
#include "RigidBody.h"
#include "Structure.h"
#include "NeuralModule.h"
#include "Adapter.h"
#include "NervousSystem.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
        {

Structure::Structure()
{
        m_lpSim = NULL;
        m_lpBody = NULL;
        m_lpPhysicsMovableItem = NULL;
        m_fltSize = 0.02f;
        m_lpScript = NULL;
}

Structure::~Structure()
{

try
{
        m_aryRigidBodies.RemoveAll();
        m_aryJoints.RemoveAll();
        m_aryExcludeCollisionList.RemoveAll();
        if(m_lpBody) delete m_lpBody;
        if(m_lpScript) delete m_lpScript;
}
catch(...)
{Std_TraceMsg(0, "Caught Error in desctructor of Structure\r\n", "", -1, false, true);}
}

void Structure::Sim(Simulator *lpSim) 
{m_lpSim = lpSim;}

void Structure::Body(RigidBody *lpBody) {m_lpBody = lpBody;}

RigidBody *Structure::Body() 
{return m_lpBody;}

CStdFPoint Structure::Position() {return MovableItem::Position();}

void Structure::Position(CStdFPoint &oPoint, bool bUseScaling, bool bFireChangeEvent, bool bUpdateMatrix)
{
        MovableItem::Position(oPoint, bUseScaling, bFireChangeEvent, bUpdateMatrix);

        if(m_lpBody && m_lpBody->PhysicsMovableItem())
    {
            if(bUpdateMatrix)
                    m_lpBody->PhysicsMovableItem()->Physics_PositionChanged();
        else if(bFireChangeEvent)
            m_lpBody->Callback()->PositionChanged();
    }
}

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

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

void Structure::Selected(bool bValue, bool bSelectMultiple)
{
        AnimatBase::Selected(bValue, bSelectMultiple);
        MovableItem::Selected(bValue, bSelectMultiple);
}

//The structure is not manipulated directly through the editor window. Instead, the root rigid body is moved/rotated.
bool Structure::AllowTranslateDragX() {return false;}

bool Structure::AllowTranslateDragY() {return false;}

bool Structure::AllowTranslateDragZ() {return false;}

bool Structure::AllowRotateDragX() {return false;}

bool Structure::AllowRotateDragY() {return false;}

bool Structure::AllowRotateDragZ() {return false;}

void Structure::Script(ScriptProcessor *lpScript) 
{
        if(m_lpScript)
        {
                delete m_lpScript;
                m_lpScript = NULL;
        }

        m_lpScript = lpScript;
}

ScriptProcessor *Structure::Script() {return m_lpScript;}

CStdPtrArray<CollisionPair> Structure::ExclusionList() 
{return m_aryExcludeCollisionList;}

void Structure::UpdateData()
{
        if(m_lpPhysicsMovableItem)
                m_lpPhysicsMovableItem->Physics_CollectData();
}

void Structure::UpdatePhysicsPosFromGraphics()
{
        if(m_lpBody)
                m_lpBody->UpdatePhysicsPosFromGraphics();
}

void Structure::Create()
{
        if(m_lpBody)
        {
                //First create all of the model objects.
                m_lpBody->CreateParts();

                //Then create all of the joints between the models.
                m_lpBody->CreateJoints();
        }

        m_lpSim->DisableCollisions(this, m_aryExcludeCollisionList);
}

void Structure::ResetSimulation()
{
        if(m_lpPhysicsMovableItem)
                m_lpPhysicsMovableItem->Physics_ResetSimulation();

        if(m_lpBody)
        {
                m_lpBody->ResetSimulation();
                m_lpBody->UpdatePhysicsPosFromGraphics();

                UpdateData();

                //We have to call this after method because some objects (ie: muscles and spindles, etc.) depend on other items
                //already being reset to their original positions. So they must be done first and then these items get reset.
                m_lpBody->AfterResetSimulation();
        }

        if(m_lpScript)
                m_lpScript->ResetSimulation();
}

void Structure::MinTimeStep(float &fltMin) {}


void Structure::StepPhysicsEngine()
{
        if(m_lpScript)
                m_lpScript->BeforeStepPhysicsEngine();

        if(m_lpBody)
        {
                m_lpBody->StepSimulation();
                UpdateData();
        }

        if(m_lpScript)
                m_lpScript->AfterStepPhysicsEngine();
}

void Structure::Initialize()
{
        AnimatBase::Initialize();
        if(m_lpScript)
                m_lpScript->Initialize();
}

void Structure::Kill(bool bState)
{
        if(m_lpScript)
                m_lpScript->Kill(bState);
}

void Structure::SimStarting()
{
}

void Structure::SimPausing()
{
}

void Structure::SimStopping()
{
}

void Structure::AddJoint(Joint *lpJoint)
{
        if(!lpJoint)
                THROW_PARAM_ERROR(Al_Err_lJointNotDefined, Al_Err_strJointNotDefined, "StructureID", m_strName);

        try
        {
                m_aryJoints.Add(lpJoint->ID(), lpJoint);
        }
        catch(CStdErrorInfo oError)
        {
                oError.m_strError += " Duplicate joint Key: " + lpJoint->Name() + " in Structure: " + m_strName; 
                THROW_ERROR(oError.m_lError, oError.m_strError);
        }
}

void Structure::RemoveJoint(std::string strID)
{
        try
        {
                m_aryJoints.Remove(strID);
        }
        catch(CStdErrorInfo oError)
        {
                oError.m_strError += " Joint not found Key: " + strID + " in Structure: " + m_strName;
                THROW_ERROR(oError.m_lError, oError.m_strError);
        }
}

void Structure::AddRigidBody(RigidBody *lpBody)
{
        if(!lpBody)
                THROW_PARAM_ERROR(Al_Err_lBodyNotDefined, Al_Err_strBodyNotDefined, "StructureID", m_strName);

        try
        {
                m_aryRigidBodies.Add(lpBody->ID(), lpBody);
        }
        catch(CStdErrorInfo oError)
        {
                oError.m_strError += " Duplicate rigid body Key: " + lpBody->Name() + " in Structure: " + m_strName; 
                THROW_ERROR(oError.m_lError, oError.m_strError);
        }
}


void Structure::RemoveRigidBody(std::string strID)
{
        try
        {
                m_aryRigidBodies.Remove(strID);
        }
        catch(CStdErrorInfo oError)
        {
                oError.m_strError += " RigidBody not found Key: " + strID + " in Structure: " + m_strName;
                THROW_ERROR(oError.m_lError, oError.m_strError);
        }
}


Joint *Structure::FindJoint(std::string strJointID, bool bThrowError)
{
        Joint *lpJoint = NULL;
        CStdMap<std::string, Joint *>::iterator oPos;
        oPos = m_aryJoints.find(Std_CheckString(strJointID));

        if(oPos != m_aryJoints.end())
                lpJoint =  oPos->second;
        else if(bThrowError)
                THROW_TEXT_ERROR(Al_Err_lJointIDNotFound, Al_Err_strJointIDNotFound, " StructureID: " + m_strName + " JointID: " + strJointID);

        return lpJoint;
}


RigidBody *Structure::FindRigidBody(std::string strBodyID, bool bThrowError)
{
        RigidBody *lpBody = NULL;
        CStdMap<std::string, RigidBody *>::iterator oPos;
        oPos = m_aryRigidBodies.find(Std_CheckString(strBodyID));

        if(oPos != m_aryRigidBodies.end())
                lpBody =  oPos->second;
        else if(bThrowError)
                THROW_TEXT_ERROR(Al_Err_lRigidBodyIDNotFound, Al_Err_strRigidBodyIDNotFound, " StructureID: " + m_strName + " BodyID: " + strBodyID);

        return lpBody;
}


Node *Structure::FindNode(std::string strID, bool bThrowError)
{
        Node *lpNode = FindRigidBody(strID, false);

        if(lpNode)
                return lpNode;

        lpNode = FindJoint(strID, false);

        if(lpNode)
                return lpNode;

        if(bThrowError)
                THROW_TEXT_ERROR(Al_Err_lRigidBodyIDNotFound, Al_Err_strRigidBodyIDNotFound, " StructureID: " + m_strName + " BodyID: " + strID);

        return NULL;
}

#pragma region DataAccesMethods

void Structure::SetSystemPointers(Simulator *lpSim, Structure *lpStructure, NeuralModule *lpModule, Node *lpNode, bool bVerify)
{
        AnimatBase::SetSystemPointers(lpSim, lpStructure, lpModule, lpNode, bVerify);
        m_lpMovableSim = lpSim;
}

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

        lpData = MovableItem::GetDataPointer(strDataType);
        if(lpData)
                return lpData;

        THROW_TEXT_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Simulator DataType: " + strDataType);

        return lpData;
}

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

        if(AnimatBase::SetData(strDataType, strValue, false))
                return true;

        if(MovableItem::SetData(strDataType, strValue, false))
                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 Structure::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
{
        AnimatBase::QueryProperties(aryProperties);
        MovableItem::QueryProperties(aryProperties);
}

bool Structure::AddItem(const std::string &strItemType, const std::string &strXml, bool bThrowError, bool bDoNotInit)
{
        std::string strType = Std_CheckString(strItemType);

        if(strType == "RIGIDBODY")
        {
                AddRoot(strXml);
                return true;
        }

        if(strType == "SCRIPT")
        {
                AddScript(strXml);
                return true;
        }

        //If it was not one of those above then we have a problem.
        if(bThrowError)
                THROW_PARAM_ERROR(Al_Err_lInvalidItemType, Al_Err_strInvalidItemType, "Item Type", strItemType);

        return false;
}

bool Structure::RemoveItem(const std::string &strItemType, const std::string &strID, bool bThrowError)
{
        std::string strType = Std_CheckString(strItemType);

        if(strType == "RIGIDBODY")
        {
                RemoveRoot(strID);
                return true;
        }

        if(strType == "SCRIPT")
        {
                RemoveScript(strID);
                return true;
        }

        //If it was not one of those above then we have a problem.
        if(bThrowError)
                THROW_PARAM_ERROR(Al_Err_lInvalidItemType, Al_Err_strInvalidItemType, "Item Type", strItemType);

        return false;
}

void Structure::AddRoot(std::string strXml)
{
        CStdXml oXml;
        oXml.Deserialize(strXml);
        oXml.FindElement("Root");
        oXml.FindChildElement("RigidBody");

        LoadRoot(oXml);
        m_lpBody->Initialize();

        //First create all of the model objects.
        m_lpBody->CreateParts();

        //Then create all of the joints between the models.
        m_lpBody->CreateJoints();

    if(m_lpSim && m_lpBody)
        m_lpSim->NotifyRigidBodyAdded(m_lpBody->ID());
}

void Structure::RemoveRoot(std::string strID, bool bThrowError)
{
        if(m_lpBody && m_lpBody->ID() == strID)
        {
                delete m_lpBody;
                m_lpBody = NULL;

        if(m_lpSim && m_lpBody)
            m_lpSim->NotifyRigidBodyRemoved(strID);
        }
        else
                THROW_PARAM_ERROR(Al_Err_lRigidBodyIDNotFound, Al_Err_strRigidBodyIDNotFound, "ID", strID);
}

void Structure::AddScript(std::string strXml)
{
        ScriptProcessor *lpScript = NULL;
        try
        {
                CStdXml oXml;
                oXml.Deserialize(strXml);
                oXml.FindElement("Root");
                oXml.FindChildElement("Script");

                lpScript = LoadScript(oXml);
                lpScript->Initialize();
                m_lpScript = lpScript;
        }
        catch(CStdErrorInfo oError)
        {
                if(lpScript) delete lpScript;
                lpScript = NULL;
                RELAY_ERROR(oError);
        }
        catch(...)
        {
                if(lpScript) delete lpScript;
                lpScript = NULL;
                THROW_ERROR(Std_Err_lUnspecifiedError, Std_Err_strUnspecifiedError);
        }
}

void Structure::RemoveScript(std::string strID, bool bThrowError)
{
        if(m_lpScript && m_lpScript->ID() == strID)
        {
                delete m_lpScript;
                m_lpScript = NULL;
        }
        else
                THROW_PARAM_ERROR(Al_Err_lRigidBodyIDNotFound, Al_Err_strRigidBodyIDNotFound, "ID", strID);
}

#pragma endregion

void Structure::EnableMotor(std::string strJointID, bool bVal)
{
        MotorizedJoint *lpJoint = dynamic_cast<MotorizedJoint *>(FindJoint(strJointID));
        if(lpJoint)
                lpJoint->EnableMotor(bVal);
        else
                THROW_PARAM_ERROR(Al_Err_lJointNotMotorized, Al_Err_strJointNotMotorized, "ID", strJointID);
}


void Structure::SetMotorInput(std::string strJointID, float fltInput)
{
        MotorizedJoint *lpJoint = dynamic_cast<MotorizedJoint *>(FindJoint(strJointID));
        if(lpJoint)
                lpJoint->MotorInput(fltInput);
        else
                THROW_PARAM_ERROR(Al_Err_lJointNotMotorized, Al_Err_strJointNotMotorized, "ID", strJointID);
}

void Structure::EnableCollision(RigidBody *lpCollisionBody)
{
        CStdMap<std::string, RigidBody *>::iterator oPos;
        RigidBody *lpBody = NULL;
        for(oPos=m_aryRigidBodies.begin(); oPos!=m_aryRigidBodies.end(); ++oPos)
        {
                lpBody = oPos->second;
                lpBody->EnableCollision(lpCollisionBody);
        }
}

void Structure::DisableCollision(RigidBody *lpCollisionBody)
{
        CStdMap<std::string, RigidBody *>::iterator oPos;
        RigidBody *lpBody = NULL;
        for(oPos=m_aryRigidBodies.begin(); oPos!=m_aryRigidBodies.end(); ++oPos)
        {
                lpBody = oPos->second;
                lpBody->DisableCollision(lpCollisionBody);
        }
}


long Structure::CalculateSnapshotByteSize()
{return 0;}

void Structure::SaveKeyFrameSnapshot(byte *aryBytes, long &lIndex)
{}

void Structure::LoadKeyFrameSnapshot(byte *aryBytes, long &lIndex)
{}


void Structure::Load(CStdXml &oXml)
{
        AnimatBase::Load(oXml);

        MovableItem::Load(oXml);

        oXml.IntoElem();  //Into Layout Element

        LoadLayout(oXml);

        if(oXml.FindChildElement("Script", false))
                Script(LoadScript(oXml));

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


void Structure::AddCollisionPair(std::string strID1, std::string strID2)
{
        CollisionPair *lpPair = new CollisionPair();
        lpPair->m_strPart1ID = strID1;
        lpPair->m_strPart2ID = strID2;

        m_aryExcludeCollisionList.Add(lpPair);
}

void Structure::LoadCollisionPair(CStdXml &oXml)
{
        CollisionPair *lpPair = NULL;
        std::string strID1, strID2;

try
{
        oXml.IntoElem();
        strID1 = oXml.GetChildString("Part1ID");
        strID2 = oXml.GetChildString("Part2ID");
        oXml.OutOfElem();

        lpPair = new CollisionPair();
        lpPair->m_strPart1ID = strID1;
        lpPair->m_strPart2ID = strID2;

        m_aryExcludeCollisionList.Add(lpPair);
}
catch(CStdErrorInfo oError)
{
        if(lpPair) delete lpPair;
        RELAY_ERROR(oError);
}
catch(...)
{
        if(lpPair) delete lpPair;
        THROW_ERROR(Std_Err_lUnspecifiedError, Std_Err_strUnspecifiedError);
}
}

void Structure::LoadLayout(CStdXml &oXml)
{
        std::string strModule;
        std::string strType;

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

        bool bFound = false;
        if(oXml.FindChildElement("Body", false))
                bFound = true;
        else if(oXml.FindChildElement("RigidBody", false))
                bFound = true;

        if(bFound)
        {
                LoadRoot(oXml);

                if(oXml.FindChildElement("CollisionExclusionPairs", false))
                {
                        oXml.IntoElem();  //Into CollisionExclusionPairs Element
                        int iChildCount = oXml.NumberOfChildren();
                        for(int iIndex=0; iIndex<iChildCount; iIndex++)
                        {
                                oXml.FindChildByIndex(iIndex);
                                LoadCollisionPair(oXml);
                        }
                        oXml.OutOfElem(); //OutOf CollisionExclusionPairs Element
                }
        }
        else
                m_lpBody = NULL;

}


RigidBody *Structure::LoadRoot(CStdXml &oXml)
{
        std::string strModule;
        std::string strType;

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

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

        m_lpBody->Parent(NULL);
        m_lpBody->SetSystemPointers(m_lpSim, this, NULL, NULL, true);

        m_lpBody->Load(oXml);
        AddRigidBody(m_lpBody);

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


ScriptProcessor *Structure::LoadScript(CStdXml &oXml)
{
        std::string strModule;
        std::string strType;
        ScriptProcessor *lpScript = NULL;

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

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

        lpScript->SetSystemPointers(m_lpSim, this, NULL, NULL, true);

        lpScript->Load(oXml);

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

        }                       // Environment
}                               //AnimatSim