World.hpp

Go to the documentation of this file.

/*
 * Original work Copyright (c) 2006-2011 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_DYNAMICS_WORLD_HPP
#define PLAYRHO_DYNAMICS_WORLD_HPP
 
 
#include <PlayRho/Common/Math.hpp>
#include <PlayRho/Common/Range.hpp>
#include <PlayRho/Dynamics/WorldConf.hpp>
#include <PlayRho/Dynamics/BodyConf.hpp>
#include <PlayRho/Dynamics/BodyAtty.hpp>
#include <PlayRho/Dynamics/FixtureConf.hpp>
#include <PlayRho/Dynamics/WorldCallbacks.hpp>
#include <PlayRho/Dynamics/StepStats.hpp>
#include <PlayRho/Collision/DynamicTree.hpp>
#include <PlayRho/Dynamics/Contacts/ContactKey.hpp>
#include <PlayRho/Dynamics/ContactAtty.hpp>
#include <PlayRho/Dynamics/JointAtty.hpp>
#include <PlayRho/Dynamics/IslandStats.hpp>
 
#include <iterator>
#include <vector>
#include <map>
#include <unordered_set>
#include <memory>
#include <stdexcept>
#include <functional>
 
namespace playrho {
 
class StepConf;
enum class BodyType;
 
namespace d2 {
 
struct BodyConf;
struct JointConf;
struct FixtureConf;
class Body;
class Contact;
class Fixture;
class Joint;
struct Island;
class Shape;
struct ShapeConf;
 
 
class World
{
public:
    using Bodies = std::vector<Body*>;
 
    using Contacts = std::vector<KeyedContactPtr>;
    
    using Joints = std::vector<Joint*>;
    
    using Fixtures = std::vector<Fixture*>;
    
    explicit World(const WorldConf& def = GetDefaultWorldConf());
 
    World(const World& other);
 
    World& operator= (const World& other);
 
    ~World() noexcept;
 
    void Clear();
 
    void SetDestructionListener(DestructionListener* listener) noexcept;
 
    void SetContactListener(ContactListener* listener) noexcept;
 
    Body* CreateBody(const BodyConf& def = GetDefaultBodyConf());
 
    Joint* CreateJoint(const JointConf& def);
 
    void Destroy(Body* body);
 
    void Destroy(Joint* joint);
 
    StepStats Step(const StepConf& conf);
 
    SizedRange<Bodies::iterator> GetBodies() noexcept;
 
    SizedRange<Bodies::const_iterator> GetBodies() const noexcept;
 
    SizedRange<Bodies::const_iterator> GetBodiesForProxies() const noexcept;
 
    SizedRange<Fixtures::const_iterator> GetFixturesForProxies() const noexcept;
 
    SizedRange<Joints::const_iterator> GetJoints() const noexcept;
 
    SizedRange<Joints::iterator> GetJoints() noexcept;
 
    SizedRange<Contacts::const_iterator> GetContacts() const noexcept;
    
    bool IsStepComplete() const noexcept;
    
    bool GetSubStepping() const noexcept;
 
    void SetSubStepping(bool flag) noexcept;
 
    const DynamicTree& GetTree() const noexcept;
 
    bool IsLocked() const noexcept;
 
    void ShiftOrigin(Length2 newOrigin);
 
    Length GetMinVertexRadius() const noexcept;
    
    Length GetMaxVertexRadius() const noexcept;
 
    Frequency GetInvDeltaTime() const noexcept;
    
private:
    friend class WorldAtty;
 
    void SetType(Body& body, playrho::BodyType type);
 
    void RegisterForProxies(Fixture& fixture);
    
    void UnregisterForProxies(const Fixture& fixture);
 
    void RegisterForProxies(Body& body);
 
    void UnregisterForProxies(const Body& body);
 
    Fixture* CreateFixture(Body& body, const Shape& shape,
                           const FixtureConf& def = GetDefaultFixtureConf(),
                           bool resetMassData = true);
 
    bool Destroy(Fixture& fixture, bool resetMassData = true);
    
    void TouchProxies(Fixture& fixture) noexcept;
    
    void SetNewFixtures() noexcept;
 
    using FlagsType = std::uint32_t;
 
    using ProxyId = DynamicTree::Size;
 
    using ContactKeyQueue = std::vector<ContactKey>;
    
    using ProxyQueue = std::vector<ProxyId>;
    
    enum Flag: FlagsType
    {
        e_newFixture    = 0x0001,
 
        e_locked        = 0x0002,
 
        e_substepping   = 0x0020,
        
        e_stepComplete  = 0x0040,
    };
 
    void CopyBodies(std::map<const Body*, Body*>& bodyMap,
                    std::map<const Fixture*, Fixture*>& fixtureMap,
                    SizedRange<World::Bodies::const_iterator> range);
    
    void CopyJoints(const std::map<const Body*, Body*>& bodyMap,
                    SizedRange<World::Joints::const_iterator> range);
    
    void CopyContacts(const std::map<const Body*, Body*>& bodyMap,
                      const std::map<const Fixture*, Fixture*>& fixtureMap,
                      SizedRange<World::Contacts::const_iterator> range);
    
    void InternalClear() noexcept;
 
    void InternalDestroy(Joint& joint) noexcept;
 
    RegStepStats SolveReg(const StepConf& conf);
 
    IslandStats SolveRegIslandViaGS(const StepConf& conf, Island island);
    
    void AddToIsland(Island& island, Body& seed,
                     Bodies::size_type& remNumBodies,
                     Contacts::size_type& remNumContacts,
                     Joints::size_type& remNumJoints);
 
    using BodyStack = std::vector<Body*>;
 
    void AddToIsland(Island& island, BodyStack& stack,
                     Bodies::size_type& remNumBodies,
                     Contacts::size_type& remNumContacts,
                     Joints::size_type& remNumJoints);
    
    void AddContactsToIsland(Island& island, BodyStack& stack, const Body* b);
 
    void AddJointsToIsland(Island& island, BodyStack& stack, const Body* b);
    
    Bodies::size_type RemoveUnspeedablesFromIslanded(const std::vector<Body*>& bodies);
 
    ToiStepStats SolveToi(const StepConf& conf);
 
    IslandStats SolveToi(const StepConf& conf, Contact& contact);
    
    IslandStats SolveToiViaGS(const StepConf& conf, Island& island);
 
    static void UpdateBody(Body& body, const Position& pos, const Velocity& vel);
 
    void ResetBodiesForSolveTOI() noexcept;
 
    void ResetContactsForSolveTOI() noexcept;
    
    void ResetContactsForSolveTOI(Body& body) noexcept;
 
    struct ProcessContactsOutput
    {
        ContactCounter contactsUpdated = 0; 
        ContactCounter contactsSkipped = 0; 
    };
 
    ProcessContactsOutput ProcessContactsForTOI(Island& island, Body& body, Real toi,
                                                const StepConf& conf);
 
    bool Add(Joint* j);
 
    void Remove(const Body& b) noexcept;
 
    void Remove(const Joint& j) noexcept;
 
    void SetStepComplete(bool value) noexcept;
 
    void SetAllowSleeping() noexcept;
 
    void UnsetAllowSleeping() noexcept;
    
    struct UpdateContactsStats
    {
        ContactCounter ignored = 0;
 
        ContactCounter updated = 0;
        
        ContactCounter skipped = 0;
    };
    
    struct DestroyContactsStats
    {
        ContactCounter ignored = 0; 
        ContactCounter erased = 0; 
    };
    
    struct ContactToiData
    {
        Contact* contact = nullptr; 
        Real toi = std::numeric_limits<Real>::infinity(); 
        ContactCounter simultaneous = 0; 
    };
 
    struct UpdateContactsData
    {
        ContactCounter numAtMaxSubSteps = 0; 
        ContactCounter numUpdatedTOI = 0; 
        ContactCounter numValidTOI = 0; 
    
        using dist_iter_type = std::remove_const<decltype(DefaultMaxDistanceIters)>::type;
 
        using toi_iter_type = std::remove_const<decltype(DefaultMaxToiIters)>::type;
        
        using root_iter_type = std::remove_const<decltype(DefaultMaxToiRootIters)>::type;
        
        dist_iter_type maxDistIters = 0; 
        toi_iter_type maxToiIters = 0; 
        root_iter_type maxRootIters = 0; 
    };
    
    UpdateContactsData UpdateContactTOIs(const StepConf& conf);
 
    ContactToiData GetSoonestContact() const noexcept;
 
    bool HasNewFixtures() const noexcept;
    
    void UnsetNewFixtures() noexcept;
    
    ContactCounter FindNewContacts();
    
    DestroyContactsStats DestroyContacts(Contacts& contacts);
    
    UpdateContactsStats UpdateContacts(Contacts& contacts, const StepConf& conf);
    
    void Destroy(Contact* contact, Body* from);
    
    bool Add(ContactKey key);
    
    void RegisterForProcessing(ProxyId pid) noexcept;
 
    void UnregisterForProcessing(ProxyId pid) noexcept;
 
    void InternalDestroy(Contact* contact, Body* from = nullptr);
 
    void CreateProxies(Fixture& fixture, Length aabbExtension);
 
    void DestroyProxies(Fixture& fixture) noexcept;
 
    void InternalTouchProxies(Fixture& fixture) noexcept;
    
    ContactCounter Synchronize(Body& body,
                               Transformation xfm1, Transformation xfm2,
                               Real multiplier, Length extension);
 
    ContactCounter Synchronize(Fixture& fixture,
                               Transformation xfm1, Transformation xfm2,
                               Length2 displacement, Length extension);
    
    void CreateAndDestroyProxies(const StepConf& conf);
 
    void CreateAndDestroyProxies(Fixture& fixture, const StepConf& conf);
    
    PreStepStats::counter_type SynchronizeProxies(const StepConf& conf);
 
    bool IsIslanded(const Body* body) const noexcept;
 
    bool IsIslanded(const Contact* contact) const noexcept;
 
    bool IsIslanded(const Joint* joint) const noexcept;
 
    void SetIslanded(Body* body) noexcept;
 
    void SetIslanded(Contact* contact) noexcept;
 
    void SetIslanded(Joint* joint) noexcept;
 
    void UnsetIslanded(Body* body) noexcept;
 
    void UnsetIslanded(Contact* contact) noexcept;
    
    void UnsetIslanded(Joint* joint) noexcept;
 
    /******** Member variables. ********/
    
    DynamicTree m_tree; 
    
    ContactKeyQueue m_proxyKeys; 
    ProxyQueue m_proxies; 
    Fixtures m_fixturesForProxies; 
    Bodies m_bodiesForProxies; 
    
    Bodies m_bodies; 
 
    Joints m_joints; 
 
    Contacts m_contacts;
    
    DestructionListener* m_destructionListener = nullptr; 
    
    ContactListener* m_contactListener = nullptr; 
    
    FlagsType m_flags = e_stepComplete; 
 
    Frequency m_inv_dt0 = 0;
 
    Positive<Length> m_minVertexRadius;
 
    Positive<Length> m_maxVertexRadius;
};
 
 
 
inline SizedRange<World::Bodies::iterator> World::GetBodies() noexcept
{
    return {begin(m_bodies), end(m_bodies), size(m_bodies)};
}
 
inline SizedRange<World::Bodies::const_iterator> World::GetBodies() const noexcept
{
    return {begin(m_bodies), end(m_bodies), size(m_bodies)};
}
 
inline SizedRange<World::Bodies::const_iterator> World::GetBodiesForProxies() const noexcept
{
    return {cbegin(m_bodiesForProxies), cend(m_bodiesForProxies), size(m_bodiesForProxies)};
}
 
inline SizedRange<World::Fixtures::const_iterator> World::GetFixturesForProxies() const noexcept
{
    return {cbegin(m_fixturesForProxies), cend(m_fixturesForProxies), size(m_fixturesForProxies)};
}
 
inline SizedRange<World::Joints::const_iterator> World::GetJoints() const noexcept
{
    return {begin(m_joints), end(m_joints), size(m_joints)};
}
 
inline SizedRange<World::Joints::iterator> World::GetJoints() noexcept
{
    return {begin(m_joints), end(m_joints), size(m_joints)};
}
 
inline SizedRange<World::Contacts::const_iterator> World::GetContacts() const noexcept
{
    return {begin(m_contacts), end(m_contacts), size(m_contacts)};
}
 
inline bool World::IsLocked() const noexcept
{
    return (m_flags & e_locked) == e_locked;
}
 
inline bool World::IsStepComplete() const noexcept
{
    return (m_flags & e_stepComplete) != 0u;
}
 
inline void World::SetStepComplete(bool value) noexcept
{
    if (value)
    {
        m_flags |= e_stepComplete;
    }
    else
    {
        m_flags &= ~e_stepComplete;        
    }
}
 
inline bool World::GetSubStepping() const noexcept
{
    return (m_flags & e_substepping) != 0u;
}
 
inline void World::SetSubStepping(bool flag) noexcept
{
    if (flag)
    {
        m_flags |= e_substepping;
    }
    else
    {
        m_flags &= ~e_substepping;
    }
}
 
inline bool World::HasNewFixtures() const noexcept
{
    return (m_flags & e_newFixture) != 0u;
}
 
inline void World::SetNewFixtures() noexcept
{
    m_flags |= e_newFixture;
}
 
inline void World::UnsetNewFixtures() noexcept
{
    m_flags &= ~e_newFixture;
}
 
inline Length World::GetMinVertexRadius() const noexcept
{
    return m_minVertexRadius;
}
 
inline Length World::GetMaxVertexRadius() const noexcept
{
    return m_maxVertexRadius;
}
 
inline Frequency World::GetInvDeltaTime() const noexcept
{
    return m_inv_dt0;
}
 
inline const DynamicTree& World::GetTree() const noexcept
{
    return m_tree;
}
 
inline void World::SetDestructionListener(DestructionListener* listener) noexcept
{
    m_destructionListener = listener;
}
 
inline void World::SetContactListener(ContactListener* listener) noexcept
{
    m_contactListener = listener;
}
 
inline bool World::IsIslanded(const Body* body) const noexcept
{
    return BodyAtty::IsIslanded(*body);
}
 
inline bool World::IsIslanded(const Contact* contact) const noexcept
{
    return ContactAtty::IsIslanded(*contact);
}
 
inline bool World::IsIslanded(const Joint* joint) const noexcept
{
    return JointAtty::IsIslanded(*joint);
}
 
inline void World::SetIslanded(Body* body) noexcept
{
    BodyAtty::SetIslanded(*body);
}
 
inline void World::SetIslanded(Contact* contact) noexcept
{
    ContactAtty::SetIslanded(*contact);
}
 
inline void World::SetIslanded(Joint* joint) noexcept
{
    JointAtty::SetIslanded(*joint);
}
 
inline void World::UnsetIslanded(Body* body) noexcept
{
    BodyAtty::UnsetIslanded(*body);
}
 
inline void World::UnsetIslanded(Contact* contact) noexcept
{
    ContactAtty::UnsetIslanded(*contact);
}
 
inline void World::UnsetIslanded(Joint* joint) noexcept
{
    JointAtty::UnsetIslanded(*joint);
}
 
inline void World::RegisterForProcessing(ProxyId pid) noexcept
{
    assert(pid != DynamicTree::GetInvalidSize());
    m_proxies.push_back(pid);
}
 
// Free functions.
 
inline BodyCounter GetBodyCount(const World& world) noexcept
{
    return static_cast<BodyCounter>(size(world.GetBodies()));
}
 
inline JointCounter GetJointCount(const World& world) noexcept
{
    return static_cast<JointCounter>(size(world.GetJoints()));
}
 
inline ContactCounter GetContactCount(const World& world) noexcept
{
    return static_cast<ContactCounter>(size(world.GetContacts()));
}
 
ContactCounter GetTouchingCount(const World& world) noexcept;
 
StepStats Step(World& world, Time delta,
               TimestepIters velocityIterations = 8,
               TimestepIters positionIterations = 3);
 
std::size_t GetFixtureCount(const World& world) noexcept;
 
std::size_t GetShapeCount(const World& world) noexcept;
 
BodyCounter GetAwakeCount(const World& world) noexcept;
 
BodyCounter Awaken(World& world) noexcept;
 
template <class F>
void SetAccelerations(World& world, F fn) noexcept
{
    const auto bodies = world.GetBodies();
    std::for_each(begin(bodies), end(bodies), [&](World::Bodies::value_type &b) {
        SetAcceleration(GetRef(b), fn(GetRef(b)));
    });
}
 
void SetAccelerations(World& world, Acceleration acceleration) noexcept;
 
void SetAccelerations(World& world, LinearAcceleration2 acceleration) noexcept;
 
inline void ClearForces(World& world) noexcept
{
    SetAccelerations(world, Acceleration{
        LinearAcceleration2{0_mps2, 0_mps2}, 0 * RadianPerSquareSecond
    });
}
 
Body* FindClosestBody(const World& world, Length2 location) noexcept;
 
} // namespace d2
 
RegStepStats& Update(RegStepStats& lhs, const IslandStats& rhs) noexcept;
 
} // namespace playrho
 
#endif // PLAYRHO_DYNAMICS_WORLD_HPP