KnotIdentificationBehavior.h

// This file is a part of the OpenSurgSim project.
// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SURGSIM_BLOCKS_KNOTIDENTIFICATIONBEHAVIOR_H
#define SURGSIM_BLOCKS_KNOTIDENTIFICATIONBEHAVIOR_H

#include <vector>

#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Framework/Behavior.h"

namespace SurgSim
{
namespace Physics
{
class Fem1DRepresentation;
}
}

namespace SurgSim
{
namespace Blocks
{
class KnotIdentificationBehavior : public SurgSim::Framework::Behavior
{
public:
    explicit KnotIdentificationBehavior(const std::string& name);

    void setFem1d(const std::shared_ptr<SurgSim::Framework::Component>& fem1d);

    const std::shared_ptr<SurgSim::Physics::Fem1DRepresentation>& getFem1d() const;

    const std::string& getKnotName();

    void update(double dt) override;
    int getTargetManagerType() const override;
    bool doInitialize() override;
    bool doWakeUp() override;

    void addKnownKnotCode(const std::string& name, const std::vector<int>& code, const std::vector<int>& signs);

    void clearKnownKnotCodes();

    struct Crossing
    {
        int id;
        size_t segmentId;
        double segmentLocation;
        int sign; 
        Crossing(int id, size_t segmentId, double segmentLocation, int sign)
            : id(id), segmentId(segmentId), segmentLocation(segmentLocation), sign(sign) {}
        bool operator==(const Crossing& rhs) const
        {
            return (id == rhs.id) && (segmentId == rhs.segmentId) && (segmentLocation == rhs.segmentLocation) &&
                (sign == rhs.sign);
        }
    };

protected:
    std::map<std::string, std::vector<std::vector<Crossing>>> m_knownLists;

    std::string detectAndIdentifyKnot(const SurgSim::Math::Matrix33d& projection);

    std::vector<Crossing> getGaussCode(const SurgSim::Math::Matrix33d& projection);

    void buildNodeData(const Math::Vector3d& projectionX,
        const Math::Vector3d& projectionY,
        std::vector<SurgSim::Math::Vector3d>* nodes3d,
        std::vector<SurgSim::Math::Vector2d>* nodes2d,
        std::vector<SurgSim::Math::Vector3d>* segments3d);

    std::vector<Crossing> calculateCrossings(const Math::Vector3d& projectionZ,
        const std::vector<SurgSim::Math::Vector3d>& nodes3d,
        const std::vector<SurgSim::Math::Vector2d>& nodes2d,
        const std::vector<SurgSim::Math::Vector3d>& segments3d);

    void performReidmeisterMoves(std::vector<Crossing>* gaussCode);

    bool tryReidmeisterMove1(std::vector<Crossing>* gaussCode, std::vector<int>* erased);

    bool tryReidmeisterMove2(std::vector<Crossing>* gaussCode, std::vector<int>* erased);

    struct ReidmeisterMove3Data
    {
        std::vector<Crossing> code;
        size_t i;
        size_t iCount;
        size_t m;
        size_t n;
        ReidmeisterMove3Data()
            : i(std::numeric_limits<size_t>::max()), iCount(0),
              m(std::numeric_limits<size_t>::max()), n(std::numeric_limits<size_t>::max()) {}
    };

    bool tryReidmeisterMove3(std::vector<Crossing>* gaussCode, ReidmeisterMove3Data* data);

    void adjustGaussCodeForErasedCrossings(std::vector<Crossing>* gaussCode);

    std::string identifyKnot(const std::vector<Crossing>& gaussCode);

    std::shared_ptr<SurgSim::Physics::Fem1DRepresentation> m_fem1d;

    std::vector<SurgSim::Math::Matrix33d> m_projections;

    boost::mutex m_mutex;

    std::string m_knotName;

private:
    size_t nextIndex(const std::vector<Crossing>& code, size_t i);

    size_t prevIndex(const std::vector<Crossing>& code, size_t i);

    bool isSameSign(const std::vector<Crossing>& code, size_t i, size_t j);

    bool isSameCross(const std::vector<Crossing>& code, size_t i, size_t j);

    bool doesOverlap(const std::vector<Crossing>& code, size_t i, size_t j, size_t k, size_t l);

    void erase(std::vector<Crossing>* code, size_t i, size_t j);

    void erase(std::vector<Crossing>* code, size_t i, size_t j, size_t k, size_t l);

    size_t getComplementCross(const std::vector<Crossing>& code, size_t i);

    bool hasCommonNeighbor(const std::vector<Crossing>& code, size_t i, size_t j, size_t* k, size_t* l);

    bool isSameCode(const std::vector<Crossing>& code, const std::vector<Crossing>& knot);
};

} // namespace Blocks
} // namespace SurgSim

#endif // SURGSIM_BLOCKS_KNOTIDENTIFICATIONBEHAVIOR_H