FootOperator.h

#pragma once
// ===========================================================================
// Copyright (c) 2015 Toolchefs Ltd. All rights reserved.
//
// Use of this software is subject to the terms of the Toolchefs license
// agreement provided at the time of installation or download, or which
// otherwise accompanies this software in either electronic or hard copy form.
// ===========================================================================
 
#include <AtomsGraph/Ports.h>
 
#include <Atoms/Globals.h>
#include <Atoms/Graph/Operator.h>
 
namespace Atoms
{
    struct FootCache
    {
        FootCache() : wasDown(false), initialized(false), detachingFromGround(false), frameSinceUp(10000u), lastValue(2.0){};
 
        bool wasDown;
        bool initialized;
        bool detachingFromGround;
 
        double lastValue;
 
        bool ik;
        unsigned short frameSinceUp;
        AtomsCore::Matrix tipMatrix;
        AtomsCore::Vector3 poleVector;
 
        unsigned int numMidJoints;
        std::vector<unsigned short> midJointIds;
        
        AtomsCore::Vector3 tipWorldZAxis, tipWorldPos;
 
        double ankleLength;
        AtomsCore::Vector3 anklePlaneNormal;
 
        AtomsCore::Quaternion rootQ, ikQ, tipQ;
        AtomsCore::Vector3 rootT, ikT, tipT;
 
        AtomsCore::Quaternion rootQdiff, ikQdiff, tipQdiff;
        AtomsCore::Vector3 rootTdiff, ikTdiff, tipTdiff;
 
        std::vector<AtomsCore::Quaternion> midQs;
        std::vector<AtomsCore::Vector3> midTs;
        std::vector<AtomsCore::Quaternion> midQdiffs;
        std::vector<AtomsCore::Vector3> midTdiffs;
    };
 
    class ATOMS_EXPORT FootOperator : public Operator
    {
    public:
 
        NODE_STANDARD_MEMBERS
        
        FootOperator();
        
        virtual ~FootOperator();
        
        bool    compute(const AtomsGraph::ComputeData* computeData);
 
        void reset();
        
    private:
        AtomsGraph::PosePort* m_inPose;
        AtomsGraph::DoublePort* m_FootThreshold;
        AtomsGraph::DoublePort* m_softIkDistance;
        AtomsGraph::BooleanPort* m_debug;
 
        //std::map<unsigned short, FootCache> m_feetCache;
        std::vector<FootCache> m_feetCache;
 
        AtomsPtr<AtomsCore::MapMetadata> m_hfAnimatedMetaMap;
 
        bool m_first;
 
    public:
 
        static const std::string ikSolverKey;
        static const std::string ikTolleranceKey;
        static const std::string ikMaxIterationsKey;
    };
}