Location.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_DATASTRUCTURES_LOCATION_H
#define SURGSIM_DATASTRUCTURES_LOCATION_H

#include <vector>

#include "SurgSim/DataStructures/OptionalValue.h"
#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
#include "SurgSim/DataStructures/OctreeNode.h"
#include "SurgSim/Math/Vector.h"

namespace SurgSim
{
namespace DataStructures
{

struct Location
{
public:
    enum Type {TRIANGLE, ELEMENT};

    Location()
    {
    }

    Location(const Location& other)
        : rigidLocalPosition(other.rigidLocalPosition),
          octreeNodePath(other.octreeNodePath),
          index(other.index),
          triangleMeshLocalCoordinate(other.triangleMeshLocalCoordinate),
          elementMeshLocalCoordinate(other.elementMeshLocalCoordinate)
    {}

    explicit Location(const SurgSim::Math::Vector3d& localPosition)
    {
        rigidLocalPosition.setValue(localPosition);
    }

    explicit Location(const SurgSim::DataStructures::OctreePath& nodePath)
    {
        octreeNodePath.setValue(nodePath);
    }

    explicit Location(const size_t val)
    {
        index.setValue(val);
    }

    Location(const SurgSim::DataStructures::IndexedLocalCoordinate& localCoordinate, Type meshType)
    {
        switch (meshType)
        {
            case TRIANGLE:
                triangleMeshLocalCoordinate.setValue(localCoordinate);
                break;
            case ELEMENT:
                elementMeshLocalCoordinate.setValue(localCoordinate);
                break;
            default:
                SURGSIM_FAILURE() << "Unknown location";
                break;
        }
    }

    const SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate>& get(Type meshType)
    {
        static SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate> null;
        switch (meshType)
        {
            case TRIANGLE:
                return triangleMeshLocalCoordinate;
                break;
            case ELEMENT:
                return elementMeshLocalCoordinate;
                break;
            default:
                SURGSIM_FAILURE() << "Unknown location";
                break;
        }
        return null;
    }

    bool isApprox(const Location& other, double precision = std::numeric_limits<double>::epsilon()) const
    {
        bool result = false;

        if (rigidLocalPosition.hasValue() && other.rigidLocalPosition.hasValue())
        {
            auto const& vector1 = rigidLocalPosition.getValue();
            auto const& vector2 = other.rigidLocalPosition.getValue();

            result = (vector1.isZero(precision) && vector2.isZero(precision)) || vector1.isApprox(vector2, precision);
        }
        else if (octreeNodePath.hasValue() && other.octreeNodePath.hasValue())
        {
            result = (octreeNodePath.getValue() == other.octreeNodePath.getValue());
        }
        else if (index.hasValue() && other.index.hasValue())
        {
            result = (index.getValue() == other.index.getValue());
        }
        else if (triangleMeshLocalCoordinate.hasValue() && other.triangleMeshLocalCoordinate.hasValue())
        {
            result = triangleMeshLocalCoordinate.getValue().isApprox(other.triangleMeshLocalCoordinate.getValue());
        }
        else if (elementMeshLocalCoordinate.hasValue() && other.elementMeshLocalCoordinate.hasValue())
        {
            result = elementMeshLocalCoordinate.getValue().isApprox(other.elementMeshLocalCoordinate.getValue());
        }
        else
        {
            SURGSIM_FAILURE() << "Invalid location type";
        }

        return result;
    }

    SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d> rigidLocalPosition;
    SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::OctreePath> octreeNodePath;
    SurgSim::DataStructures::OptionalValue<size_t> index;
    SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate> triangleMeshLocalCoordinate;
    SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate> elementMeshLocalCoordinate;
};


template <typename charT, typename traits, typename T>
std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
        const SurgSim::DataStructures::OptionalValue<T>& val)
{
    if (val.hasValue())
    {
        out << val.getValue();
    }
    else
    {
        out << "<empty>";
    }
    return out;
}

template <typename charT, typename traits>
std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
        const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& val)
{
    if (val.hasValue())
    {
        out << val.getValue().transpose();
    }
    else
    {
        out << "<empty>";
    }
    return out;
}


template <typename charT, typename traits>
std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
        const SurgSim::DataStructures::IndexedLocalCoordinate& val)
{
    out << "[ " << val.index << " : " << val.coordinate.transpose() << " ]";
    return out;
}


template <typename charT, typename traits>
std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
        const Location& loc)
{
    out << "ElementMesh: " << loc.elementMeshLocalCoordinate << std::endl;
    out << "Index: " << loc.index << std::endl;
    out << "RigidLocal: " << loc.rigidLocalPosition << std::endl;
    out << "TriangleMeshLocal: " << loc.triangleMeshLocalCoordinate << std::endl;
    return out;
}

}; // namespace DataStructures
}; // namespace SurgSim

#endif // SURGSIM_DATASTRUCTURES_LOCATION_H