Simplex.hpp

Go to the documentation of this file.

/*
 * Original work Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
 * Modified work Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/PlayRho
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
#ifndef PLAYRHO_COLLISION_SIMPLEX_HPP
#define PLAYRHO_COLLISION_SIMPLEX_HPP
 
#include <PlayRho/Common/ArrayList.hpp>
#include <PlayRho/Common/Vector.hpp>
#include <PlayRho/Collision/SimplexEdge.hpp>
#include <array>
 
namespace playrho {
namespace d2 {
 
    using SimplexEdges = ArrayList<SimplexEdge, MaxSimplexEdges,
        std::remove_const<decltype(MaxSimplexEdges)>::type>;
    
    IndexPair3 GetIndexPairs(const SimplexEdges& collection) noexcept;
    
    Length2 CalcSearchDirection(const SimplexEdges& simplexEdges) noexcept;
 
    class Simplex
    {
    public:
 
        using size_type = SimplexEdges::size_type;
 
        using Coefficients = ArrayList<Real, MaxSimplexEdges,
            std::remove_const<decltype(MaxSimplexEdges)>::type>;
        
        struct Cache
        {
            Real metric = GetInvalid<Real>();
 
            IndexPair3 indices = InvalidIndexPair3;
        };
        
        static Cache GetCache(const SimplexEdges& edges) noexcept;
        
        static Real CalcMetric(const SimplexEdges& simplexEdges);
 
        static Simplex Get(const SimplexEdge& s0) noexcept;
 
        static Simplex Get(const SimplexEdge& s0, const SimplexEdge& s1) noexcept;
        
        static Simplex Get(const SimplexEdge& s0, const SimplexEdge& s1, const SimplexEdge& s2) noexcept;
        
        static Simplex Get(const SimplexEdges& edges) noexcept;
 
        Simplex() = default;
 
        PLAYRHO_CONSTEXPR inline SimplexEdges GetEdges() const noexcept;
 
        const SimplexEdge& GetSimplexEdge(size_type index) const noexcept;
 
        PLAYRHO_CONSTEXPR inline Real GetCoefficient(size_type index) const noexcept;
 
        PLAYRHO_CONSTEXPR inline size_type size() const noexcept;
 
    private:
        
        Simplex(const SimplexEdges& simplexEdges, const Coefficients& normalizedWeights) noexcept;
 
        SimplexEdges m_simplexEdges;
 
        Coefficients m_normalizedWeights;
    };
 
    inline Simplex::Cache Simplex::GetCache(const SimplexEdges& edges) noexcept
    {
        return Simplex::Cache{Simplex::CalcMetric(edges), GetIndexPairs(edges)};
    }
 
    inline Simplex::Simplex(const SimplexEdges& simplexEdges,
                            const Coefficients& normalizedWeights) noexcept:
        m_simplexEdges{simplexEdges}, m_normalizedWeights{normalizedWeights}
    {
        assert(simplexEdges.size() == normalizedWeights.size());
#ifndef NDEBUG
        const auto sum = std::accumulate(begin(normalizedWeights), end(normalizedWeights),
                                         Real{0});
        assert(AlmostEqual(Real{1}, sum));
#endif
    }
 
    PLAYRHO_CONSTEXPR inline SimplexEdges Simplex::GetEdges() const noexcept
    {
        return m_simplexEdges;
    }
    
    const inline SimplexEdge& Simplex::GetSimplexEdge(size_type index) const noexcept
    {
        return m_simplexEdges[index];
    }
    
    PLAYRHO_CONSTEXPR inline Real Simplex::GetCoefficient(size_type index) const noexcept
    {
        return m_normalizedWeights[index];
    }
    
    PLAYRHO_CONSTEXPR inline Simplex::size_type Simplex::size() const noexcept
    {
        return m_simplexEdges.size();
    }
 
    inline Length2 GetScaledDelta(const Simplex& simplex, Simplex::size_type index)
    {
        return GetPointDelta(simplex.GetSimplexEdge(index)) * simplex.GetCoefficient(index);
    }
 
    PLAYRHO_CONSTEXPR inline Length2 GetClosestPoint(const Simplex& simplex)
    {
        switch (simplex.size())
        {
            case 1: return GetScaledDelta(simplex, 0);
            case 2: return GetScaledDelta(simplex, 0) + GetScaledDelta(simplex, 1);
            case 3: return Length2{0_m, 0_m};
            default: return Length2{0_m, 0_m};
        }
    }
 
} // namespace d2
} // namespace playrho
 
#endif // PLAYRHO_COLLISION_SIMPLEX_HPP