ArticulatedHierarchyContainer.h

Go to the documentation of this file.

/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 3      *
*                (c) 2006-2008 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_CONTAINER_ARTICULATEDHIERARCHYCONTAINER_H
#define SOFA_COMPONENT_CONTAINER_ARTICULATEDHIERARCHYCONTAINER_H

#include <sofa/core/objectmodel/BaseObject.h>
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/simulation/tree/GNode.h>
#include <sofa/helper/io/bvh/BVHLoader.h>
#include <sofa/component/MechanicalObject.h>
#include <sofa/defaulttype/RigidTypes.h>

namespace sofa
{

namespace component
{

namespace container
{

using namespace sofa::defaulttype;
using namespace sofa::simulation::tree;

class ArticulatedHierarchyContainer : public virtual core::objectmodel::BaseObject
{
public:

class ArticulationCenter : public virtual core::objectmodel::BaseObject
{
public:

    class Articulation : public virtual core::objectmodel::BaseObject
    {
    public:

        Articulation();
        ~Articulation(){};

        Data<Vector3> axis;
        Data<bool> rotation;
        Data<bool> translation;
        Data<int> articulationIndex;

        std::vector<double> motion;
    };

    ArticulationCenter();
    ~ArticulationCenter(){};

    Data<int> parentIndex;
    Data<int> childIndex;
    Data<Vector3> globalPosition;
    Data<Vector3> posOnParent;
    Data<Vector3> posOnChild;
    Data<int> articulationProcess;

    
    vector<Articulation*> articulations;
    
    Vector3 posOnChildGlobal(Quat localToGlobal)
    {
        Vector3 result = localToGlobal.rotate(posOnChild.getValue());
        return result;
    }

    Vector3 initTranslateChild(Quat objectRotation)
    {
        Vector3 PAParent = posOnParent.getValue() - Vector3(0,0,0);
        Vector3 PAChild = posOnChild.getValue() - Vector3(0,0,0);
        return objectRotation.rotate(PAParent - PAChild);
    }

    Vector3 translateChild(Quat object1Rotation, Quat object2Rotation)
    {
        Vector3 APChild = Vector3(0,0,0) - posOnChild.getValue();
        Vector3 AP1 = object2Rotation.rotate(APChild);
        Vector3 AP2 = object1Rotation.rotate(AP1);
        return AP2 - AP1;
    }

    Vector3 correctPosChild(Vector3 object1Pos, Quat object1Rot, Vector3 object2Pos, Quat object2Rot)
    {
        Vector3 result;
        Vector3 PAParent = posOnParent.getValue() - Vector3(0,0,0);
        Vector3 PAChild = posOnChild.getValue() - Vector3(0,0,0);
        Vector3 A1 = object1Pos + object1Rot.rotate(PAParent);
        Vector3 A2 = object2Pos + object2Rot.rotate(PAChild);
        
        result = A1 - A2;

        return result;

    }

    vector<Articulation*> getArticulations();
    
    Quat OrientationArticulationCenter;
    Vector3 DisplacementArticulationCenter;
    Vector3 Disp_Rotation;
    
    
}; // end ArticulationCenter

    ArticulatedHierarchyContainer();

    ~ArticulatedHierarchyContainer(){}

    

    void init();

    void setFilename(std::string f){filename.setValue(f);}
    
    vector<ArticulationCenter*> getArticulationCenters();
    ArticulationCenter* getArticulationCenterAsChild(int index);
    vector<ArticulationCenter*> getAcendantList(int index);

    vector<ArticulationCenter*> articulationCenters;
    vector<ArticulationCenter*> acendantList;

    bool chargedFromFile;
    int numOfFrames;
    double dtbvh;

  protected:
    Data<std::string> filename;
private:

    
    unsigned int id;
    sofa::helper::io::bvh::BVHJoint* joint;
    void buildCenterArticulationsTree(sofa::helper::io::bvh::BVHJoint*, int id_buf, const char* name, simulation::tree::GNode* node);
};

} // namespace container

} // namespace component

} // namespace sofa

#endif