Manifold.hpp

Go to the documentation of this file.

/*
 * Original work Copyright (c) 2006-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_MANIFOLD_HPP
#define PLAYRHO_COLLISION_MANIFOLD_HPP
 
#include <PlayRho/Common/Math.hpp>
#include <PlayRho/Collision/ContactFeature.hpp>
#include <PlayRho/Collision/IndexPair.hpp>
 
namespace playrho {
namespace d2 {
 
class DistanceProxy;
struct Transformation;
 
class alignas(64) Manifold
{
public:
    
    using size_type = std::remove_const<decltype(MaxManifoldPoints)>::type;
 
    using CfIndex = ContactFeature::Index;
    
    using CfType = ContactFeature::Type;
 
    struct Conf;
 
    enum Type: std::uint8_t
    {
        e_unset,
        
        e_circles,
 
        e_faceA,
 
        e_faceB
    };
    
    struct Point
    {
        Length2 localPoint;
 
        ContactFeature contactFeature;
        
        Momentum normalImpulse = 0_Ns;
        
        Momentum tangentImpulse = 0_Ns;
    };
    
    // For Circles type manifolds...
    
    static inline Manifold GetForCircles(Length2 vA, CfIndex iA, Length2 vB, CfIndex iB) noexcept
    {
        return Manifold{e_circles, GetInvalid<UnitVec>(), vA, 1, {{
            Point{vB, GetVertexVertexContactFeature(iA, iB)}
        }}};
    }
 
    // For Face A type manifolds...
    
    static inline Manifold GetForFaceA(UnitVec normalA, Length2 faceA) noexcept
    {
        return Manifold{e_faceA, normalA, faceA, 0, {{}}};
    }
    
    static inline Manifold GetForFaceA(UnitVec ln, Length2 lp,
                                       const Point& mp1) noexcept
    {
        //assert(mp1.contactFeature.typeA == ContactFeature::e_face || mp1.contactFeature.typeB == ContactFeature::e_face);
        return Manifold{e_faceA, ln, lp, 1, {{mp1}}};
    }
    
    static inline Manifold GetForFaceA(UnitVec ln, Length2 lp,
                                       const Point& mp1, const Point& mp2) noexcept
    {
        //assert(mp1.contactFeature.typeA == ContactFeature::e_face || mp1.contactFeature.typeB == ContactFeature::e_face);
        //assert(mp2.contactFeature.typeA == ContactFeature::e_face || mp2.contactFeature.typeB == ContactFeature::e_face);
        //assert(mp1.contactFeature != mp2.contactFeature);
        return Manifold{e_faceA, ln, lp, 2, {{mp1, mp2}}};
    }
    
    // For Face B...
    
    static inline Manifold GetForFaceB(UnitVec ln, Length2 lp) noexcept
    {
        return Manifold{e_faceB, ln, lp, 0, {{}}};
    }
    
    static inline Manifold GetForFaceB(UnitVec ln, Length2 lp,
                                       const Point& mp1) noexcept
    {
        //assert(mp1.contactFeature.typeA == ContactFeature::e_face || mp1.contactFeature.typeB == ContactFeature::e_face);
        return Manifold{e_faceB, ln, lp, 1, {{mp1}}};
    }
    
    static inline Manifold GetForFaceB(UnitVec ln, Length2 lp,
                                       const Point& mp1, const Point& mp2) noexcept
    {
        //assert(mp1.contactFeature.typeA == ContactFeature::e_face || mp1.contactFeature.typeB == ContactFeature::e_face);
        //assert(mp2.contactFeature.typeA == ContactFeature::e_face || mp2.contactFeature.typeB == ContactFeature::e_face);
        //assert(mp1.contactFeature != mp2.contactFeature);
        return Manifold{e_faceB, ln, lp, 2, {{mp1, mp2}}};
    }
    
    static inline Manifold GetForFaceA(UnitVec na, CfIndex ia, Length2 pa) noexcept
    {
        return Manifold{e_faceA, na, pa, 0, {{
            Point{GetInvalid<Length2>(), ContactFeature{ContactFeature::e_face, ia, ContactFeature::e_face, 0}},
            Point{GetInvalid<Length2>(), ContactFeature{ContactFeature::e_face, ia, ContactFeature::e_face, 0}}
        }}};
    }
    
    static inline Manifold GetForFaceB(UnitVec nb, CfIndex ib, Length2 pb) noexcept
    {
        return Manifold{e_faceB, nb, pb, 0, {{
            Point{GetInvalid<Length2>(), ContactFeature{ContactFeature::e_face, 0, ContactFeature::e_face, ib}},
            Point{GetInvalid<Length2>(), ContactFeature{ContactFeature::e_face, 0, ContactFeature::e_face, ib}}
        }}};
    }
 
    static inline Manifold GetForFaceA(UnitVec na, CfIndex ia, Length2 pa,
                                          CfType tb0, CfIndex ib0, Length2 pb0) noexcept
    {
        return Manifold{e_faceA, na, pa, 1, {{
            Point{pb0, ContactFeature{ContactFeature::e_face, ia, tb0, ib0}},
            Point{pb0, ContactFeature{ContactFeature::e_face, ia, tb0, ib0}}
        }}};
    }
    
    static inline Manifold GetForFaceB(UnitVec nb, CfIndex ib, Length2 pb,
                                          CfType ta0, CfIndex ia0, Length2 pa0) noexcept
    {
        return Manifold{e_faceB, nb, pb, 1, {{
            Point{pa0, ContactFeature{ta0, ia0, ContactFeature::e_face, ib}},
            Point{pa0, ContactFeature{ta0, ia0, ContactFeature::e_face, ib}}
        }}};
    }
    
    static inline Manifold GetForFaceA(UnitVec na, CfIndex ia, Length2 pa,
                                          CfType tb0, CfIndex ib0, Length2 pb0,
                                          CfType tb1, CfIndex ib1, Length2 pb1) noexcept
    {
        return Manifold{e_faceA, na, pa, 2, {{
            Point{pb0, ContactFeature{ContactFeature::e_face, ia, tb0, ib0}},
            Point{pb1, ContactFeature{ContactFeature::e_face, ia, tb1, ib1}}
        }}};
    }
 
    static inline Manifold GetForFaceB(UnitVec nb, CfIndex ib, Length2 pb,
                                          CfType ta0, CfIndex ia0, Length2 pa0,
                                          CfType ta1, CfIndex ia1, Length2 pa1) noexcept
    {
        return Manifold{e_faceB, nb, pb, 2, {{
            Point{pa0, ContactFeature{ta0, ia0, ContactFeature::e_face, ib}},
            Point{pa1, ContactFeature{ta1, ia1, ContactFeature::e_face, ib}}
        }}};
    }
 
    Manifold() = default;
    
    Manifold(const Manifold& copy) = default;
    
    PLAYRHO_CONSTEXPR inline Type GetType() const noexcept { return m_type; }
    
    PLAYRHO_CONSTEXPR inline size_type GetPointCount() const noexcept { return m_pointCount; }
    
    PLAYRHO_CONSTEXPR inline ContactFeature GetContactFeature(size_type index) const noexcept
    {
        assert(index < m_pointCount);
        return m_points[index].contactFeature;
    }
 
    PLAYRHO_CONSTEXPR inline Momentum2 GetContactImpulses(size_type index) const noexcept
    {
        assert(index < m_pointCount);
        return Momentum2{m_points[index].normalImpulse, m_points[index].tangentImpulse};
    }
 
    void SetContactImpulses(size_type index, Momentum2 value) noexcept
    {
        assert(index < m_pointCount);
        m_points[index].normalImpulse = get<0>(value);
        m_points[index].tangentImpulse = get<1>(value);
    }
 
    const Point& GetPoint(size_type index) const noexcept
    {
        assert((0 <= index) && (index < m_pointCount));
        return m_points[index];
    }
    
    void SetPointImpulses(size_type index, Momentum n, Momentum t)
    {
        assert((index < m_pointCount) || (index < MaxManifoldPoints && n == 0_Ns && t == 0_Ns));
        m_points[index].normalImpulse = n;
        m_points[index].tangentImpulse = t;
    }
    
    void AddPoint(const Point& mp) noexcept;
 
    void AddPoint(CfType type, CfIndex index, Length2 point) noexcept;
 
    PLAYRHO_CONSTEXPR inline UnitVec GetLocalNormal() const noexcept
    {
        assert(m_type != e_unset);
        assert(m_type != e_circles);
        return m_localNormal;
    }
    
    PLAYRHO_CONSTEXPR inline Length2 GetLocalPoint() const noexcept
    {
        assert(m_type != e_unset);
        return m_localPoint;
    }
    
    PLAYRHO_CONSTEXPR inline Length2 GetOpposingPoint(size_type index) const noexcept
    {
        assert((0 <= index) && (index < m_pointCount));
        return m_points[index].localPoint;
    }
 
private:
    
    struct PointArray
    {
        Point elements[MaxManifoldPoints]; 
 
        PLAYRHO_CONSTEXPR inline Point& operator[](std::size_t i) { return elements[i]; }
        
        PLAYRHO_CONSTEXPR const Point& operator[](std::size_t i) const { return elements[i]; }
    };
 
    PLAYRHO_CONSTEXPR inline Manifold(Type t, UnitVec ln, Length2 lp, size_type n, const PointArray& mpa) noexcept;
    
    Type m_type = e_unset; 
    size_type m_pointCount = 0; 
    
    UnitVec m_localNormal = GetInvalid<decltype(m_localNormal)>();
 
    Length2 m_localPoint = GetInvalid<Length2>();
    
    PointArray m_points; 
};
 
struct Manifold::Conf
{
    Length linearSlop = DefaultLinearSlop;
 
    Length targetDepth = DefaultLinearSlop * Real{3};
    
    Length tolerance = DefaultLinearSlop / Real{4}; 
    
    Real maxCirclesRatio = DefaultCirclesRatio;
};
 
PLAYRHO_CONSTEXPR inline Manifold::Conf GetDefaultManifoldConf() noexcept
{
    return Manifold::Conf{};
}
 
PLAYRHO_CONSTEXPR inline Manifold::Manifold(Type t, UnitVec ln, Length2 lp, size_type n,
                                          const PointArray& mpa) noexcept:
    m_type{t}, m_pointCount{n}, m_localNormal{ln}, m_localPoint{lp}, m_points{mpa}
{
    assert(t != e_unset || n == 0);
    assert(t == e_unset || IsValid(lp));
    assert((t == e_unset) || (t == e_circles) || IsValid(ln));
    assert((t != e_circles) || (n == 1 && !IsValid(ln)));
    //assert((t != e_circles) || (n == 1 && !IsValid(ln) && mpa[0].contactFeature.typeA == ContactFeature::e_vertex && mpa[0].contactFeature.typeB == ContactFeature::e_vertex));
}
 
inline void Manifold::AddPoint(const Point& mp) noexcept
{
    assert(m_type != e_unset);
    assert(m_type != e_circles || m_pointCount == 0);
    assert(m_pointCount < MaxManifoldPoints);
    // assert((m_pointCount == 0) || (mp.contactFeature != m_points[0].contactFeature));
    //assert((m_type != e_circles) || (mp.contactFeature.typeA == ContactFeature::e_vertex || mp.contactFeature.typeB == ContactFeature::e_vertex));
    //assert((m_type != e_faceA) || ((mp.contactFeature.typeA == ContactFeature::e_face) && (m_pointCount == 0 || mp.contactFeature.indexA == m_points[0].contactFeature.indexA)));
    //assert((m_type != e_faceB) || (mp.contactFeature.typeB == ContactFeature::e_face));
    m_points[m_pointCount] = mp;
    ++m_pointCount;
}
 
inline void Manifold::AddPoint(CfType type, CfIndex index, Length2 point) noexcept
{
    assert(m_pointCount < MaxManifoldPoints);
    switch (m_type)
    {
        case e_unset:
            break;
        case e_circles:
            break;
        case e_faceA:
            m_points[m_pointCount].localPoint = point;
            m_points[m_pointCount].contactFeature.typeB = type;
            m_points[m_pointCount].contactFeature.indexB = index;
            ++m_pointCount;
            break;
        case e_faceB:
            m_points[m_pointCount].localPoint = point;
            m_points[m_pointCount].contactFeature.typeA = type;
            m_points[m_pointCount].contactFeature.indexA = index;
            ++m_pointCount;
            break;
    }
}
 
// Free functions...
 
bool operator==(const Manifold::Point& lhs, const Manifold::Point& rhs) noexcept;
 
bool operator!=(const Manifold::Point& lhs, const Manifold::Point& rhs) noexcept;
 
bool operator==(const Manifold& lhs, const Manifold& rhs) noexcept;
 
bool operator!=(const Manifold& lhs, const Manifold& rhs) noexcept;
 
Manifold GetManifold(bool flipped,
                     const DistanceProxy& shape0, const Transformation& xf0,
                     const VertexCounter idx0,
                     const DistanceProxy& shape1, const Transformation& xf1,
                     const VertexCounter2 indices1,
                     const Manifold::Conf conf);
 
Manifold GetManifold(bool flipped, Length totalRadius,
                     const DistanceProxy& shape, const Transformation& sxf,
                     Length2 point, const Transformation& xfm);
 
Manifold GetManifold(Length2 locationA, const Transformation& xfA,
                     Length2 locationB, const Transformation& xfB,
                     Length totalRadius) noexcept;
 
Manifold CollideShapes(const DistanceProxy& shapeA, const Transformation& xfA,
                       const DistanceProxy& shapeB, const Transformation& xfB,
                       Manifold::Conf conf = GetDefaultManifoldConf());
#if 0
Manifold CollideCached(const DistanceProxy& shapeA, const Transformation& xfA,
                       const DistanceProxy& shapeB, const Transformation& xfB,
                       Manifold::Conf conf = GetDefaultManifoldConf());
#endif
 
#ifdef DEFINE_GET_MANIFOLD
Manifold GetManifold(const DistanceProxy& proxyA, const Transformation& transformA,
                     const DistanceProxy& proxyB, const Transformation& transformB);
#endif
 
#if 0
Length2 GetLocalPoint(const DistanceProxy& proxy, ContactFeature::Type type,
                       ContactFeature::Index index);
#endif
 
const char* GetName(Manifold::Type type) noexcept;
 
} // namespace d2
 
template <>
PLAYRHO_CONSTEXPR inline bool IsValid(const d2::Manifold& value) noexcept
{
    return value.GetType() != d2::Manifold::e_unset;
}
 
} // namespace playrho
 
#endif // PLAYRHO_COLLISION_MANIFOLD_HPP