Body.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_BODY_HPP
#define PLAYRHO_DYNAMICS_BODY_HPP
 
 
#include <PlayRho/Common/Math.hpp>
#include <PlayRho/Common/Range.hpp>
#include <PlayRho/Common/BoundedValue.hpp>
#include <PlayRho/Dynamics/BodyType.hpp>
#include <PlayRho/Dynamics/Fixture.hpp>
#include <PlayRho/Dynamics/Contacts/ContactKey.hpp>
#include <PlayRho/Dynamics/Joints/JointKey.hpp>
#include <PlayRho/Dynamics/MovementConf.hpp>
#include <PlayRho/Collision/MassData.hpp>
 
#include <vector>
#include <memory>
#include <cassert>
#include <utility>
#include <iterator>
 
namespace playrho {
namespace d2 {
 
class World;
struct FixtureConf;
class Shape;
struct BodyConf;
 
class Body
{
public:
    
    using Fixtures = std::vector<Fixture*>;
 
    using KeyedJointPtr = std::pair<Body*, Joint*>;
 
    using Joints = std::vector<KeyedJointPtr>;
    
    using Contacts = std::vector<KeyedContactPtr>;
 
    static PLAYRHO_CONSTEXPR const auto InvalidIslandIndex = static_cast<BodyCounter>(-1);
 
    using FlagsType = std::uint16_t;
    
    enum Flag: FlagsType
    {
        e_islandFlag = FlagsType(0x0001),
 
        e_awakeFlag = FlagsType(0x0002),
        
        e_autoSleepFlag = FlagsType(0x0004),
        
        e_impenetrableFlag = FlagsType(0x0008),
        
        e_fixedRotationFlag = FlagsType(0x0010),
        
        e_enabledFlag = FlagsType(0x0020),
        
        e_velocityFlag = FlagsType(0x0080),
        
        e_accelerationFlag = FlagsType(0x0100),
        
        e_massDataDirtyFlag = FlagsType(0x0200),
    };
    
    static FlagsType GetFlags(BodyType type) noexcept;
 
    static FlagsType GetFlags(const BodyConf& bd) noexcept;
    
    Fixture* CreateFixture(const Shape& shape,
                           const FixtureConf& def = GetDefaultFixtureConf(),
                           bool resetMassData = true);
 
    bool Destroy(Fixture* fixture, bool resetMassData = true);
    
    void DestroyFixtures();
    
    void SetTransform(Length2 location, Angle angle);
 
    Transformation GetTransformation() const noexcept;
 
    Length2 GetLocation() const noexcept;
 
    const Sweep& GetSweep() const noexcept;
 
    Angle GetAngle() const noexcept;
 
    Length2 GetWorldCenter() const noexcept;
 
    Length2 GetLocalCenter() const noexcept;
 
    Velocity GetVelocity() const noexcept;
 
    void SetVelocity(const Velocity& velocity) noexcept;
 
    void SetAcceleration(LinearAcceleration2 linear, AngularAcceleration angular) noexcept;
 
    LinearAcceleration2 GetLinearAcceleration() const noexcept;
 
    AngularAcceleration GetAngularAcceleration() const noexcept;
 
    InvMass GetInvMass() const noexcept;
 
    InvRotInertia GetInvRotInertia() const noexcept;
 
    void SetMassData(const MassData& massData);
 
    void ResetMassData();
 
    Frequency GetLinearDamping() const noexcept;
 
    void SetLinearDamping(NonNegative<Frequency> linearDamping) noexcept;
 
    Frequency GetAngularDamping() const noexcept;
 
    void SetAngularDamping(NonNegative<Frequency> angularDamping) noexcept;
 
    void SetType(BodyType type);
 
    BodyType GetType() const noexcept;
 
    bool IsSpeedable() const noexcept;
 
    bool IsAccelerable() const noexcept;
 
    void SetBullet(bool flag) noexcept;
 
    bool IsImpenetrable() const noexcept;
 
    void SetSleepingAllowed(bool flag) noexcept;
 
    bool IsSleepingAllowed() const noexcept;
 
    void SetAwake() noexcept;
 
    void UnsetAwake() noexcept;
 
    bool IsAwake() const noexcept;
 
    Time GetUnderActiveTime() const noexcept;
 
    void SetUnderActiveTime(Time value) noexcept;
 
    void ResetUnderActiveTime() noexcept;
 
    void SetEnabled(bool flag);
 
    bool IsEnabled() const noexcept;
 
    void SetFixedRotation(bool flag);
 
    bool IsFixedRotation() const noexcept;
 
    SizedRange<Fixtures::const_iterator> GetFixtures() const noexcept;
 
    SizedRange<Fixtures::iterator> GetFixtures() noexcept;
    
    SizedRange<Joints::const_iterator> GetJoints() const noexcept;
 
    SizedRange<Joints::iterator> GetJoints() noexcept;
    
    SizedRange<Contacts::const_iterator> GetContacts() const noexcept;
 
    void* GetUserData() const noexcept;
 
    void SetUserData(void* data) noexcept;
 
    World* GetWorld() const noexcept;
 
    bool IsMassDataDirty() const noexcept;
    
private:
 
    friend class BodyAtty;
    
    Body() = delete;
    
    Body(const Body& other) = delete;
 
    Body(World* world, const BodyConf& bd);
    
    ~Body() noexcept;
    
    bool IsIslanded() const noexcept;
 
    void SetIslandedFlag() noexcept;
    
    void UnsetIslandedFlag() noexcept;
    
    void SetAwakeFlag() noexcept;
 
    void UnsetAwakeFlag() noexcept;
 
    void Advance(Real alpha) noexcept;
 
    void SetMassDataDirty() noexcept;
    
    void UnsetMassDataDirty() noexcept;
 
    void SetEnabledFlag() noexcept;
    
    void UnsetEnabledFlag() noexcept;
 
    bool Insert(ContactKey key, Contact* contact);
    
    bool Insert(Joint* joint);
 
    bool Erase(const Contact* contact);
    
    bool Erase(const Joint* joint);
 
    void ClearContacts();
    
    void ClearJoints();
 
    void SetTransformation(Transformation value) noexcept;
 
    //
    // Member variables. Try to keep total size small.
    //
 
    Transformation m_xf;
 
    Sweep m_sweep; 
 
    Velocity m_velocity; 
    FlagsType m_flags = 0; 
 
    LinearAcceleration2 m_linearAcceleration = LinearAcceleration2{};
 
    World* const m_world; 
    void* m_userData; 
    
    Fixtures m_fixtures; 
    Contacts m_contacts; 
    Joints m_joints; 
 
    AngularAcceleration m_angularAcceleration = AngularAcceleration{0};
 
    InvMass m_invMass = 0;
 
    InvRotInertia m_invRotI = 0;
 
    NonNegative<Frequency> m_linearDamping; 
    NonNegative<Frequency> m_angularDamping; 
 
    Time m_underActiveTime = 0;
};
 
 
inline Body::FlagsType Body::GetFlags(BodyType type) noexcept
{
    auto flags = FlagsType{0};
    switch (type)
    {
        case BodyType::Dynamic:   flags |= (e_velocityFlag|e_accelerationFlag); break;
        case BodyType::Kinematic: flags |= (e_impenetrableFlag|e_velocityFlag); break;
        case BodyType::Static:    flags |= (e_impenetrableFlag); break;
    }
    return flags;
}
 
inline BodyType Body::GetType() const noexcept
{
    switch (m_flags & (e_accelerationFlag|e_velocityFlag))
    {
        case e_velocityFlag|e_accelerationFlag: return BodyType::Dynamic;
        case e_velocityFlag: return BodyType::Kinematic;
        default: break; // handle case 0 this way so compiler doesn't warn of no default handling.
    }
    return BodyType::Static;
}
 
inline Transformation Body::GetTransformation() const noexcept
{
    return m_xf;
}
 
inline Length2 Body::GetLocation() const noexcept
{
    return GetTransformation().p;
}
 
inline const Sweep& Body::GetSweep() const noexcept
{
    return m_sweep;
}
 
inline Angle Body::GetAngle() const noexcept
{
    return GetSweep().pos1.angular;
}
 
inline Length2 Body::GetWorldCenter() const noexcept
{
    return GetSweep().pos1.linear;
}
 
inline Length2 Body::GetLocalCenter() const noexcept
{
    return GetSweep().GetLocalCenter();
}
 
inline Velocity Body::GetVelocity() const noexcept
{
    return m_velocity;
}
 
inline InvMass Body::GetInvMass() const noexcept
{
    return m_invMass;
}
 
inline InvRotInertia Body::GetInvRotInertia() const noexcept
{
    return m_invRotI;
}
 
inline Frequency Body::GetLinearDamping() const noexcept
{
    return m_linearDamping;
}
 
inline void Body::SetLinearDamping(NonNegative<Frequency> linearDamping) noexcept
{
    m_linearDamping = linearDamping;
}
 
inline Frequency Body::GetAngularDamping() const noexcept
{
    return m_angularDamping;
}
 
inline void Body::SetAngularDamping(NonNegative<Frequency> angularDamping) noexcept
{
    m_angularDamping = angularDamping;
}
 
inline void Body::SetBullet(bool flag) noexcept
{
    if (flag)
    {
        m_flags |= e_impenetrableFlag;
    }
    else
    {
        m_flags &= ~e_impenetrableFlag;
    }
}
 
inline bool Body::IsImpenetrable() const noexcept
{
    return (m_flags & e_impenetrableFlag) != 0;
}
 
inline void Body::SetAwakeFlag() noexcept
{
    // Protect the body's invariant that only "speedable" bodies can be awake.
    assert(IsSpeedable());
 
    m_flags |= e_awakeFlag;
}
 
inline void Body::UnsetAwakeFlag() noexcept
{
    assert(!IsSpeedable() || IsSleepingAllowed());
    m_flags &= ~e_awakeFlag;
}
 
inline void Body::SetAwake() noexcept
{
    // Ignore this request unless this body is speedable so as to maintain the body's invariant
    // that only "speedable" bodies can be awake.
    if (IsSpeedable())
    {
        SetAwakeFlag();
        ResetUnderActiveTime();
    }
}
 
inline void Body::UnsetAwake() noexcept
{
    if (!IsSpeedable() || IsSleepingAllowed())
    {
        UnsetAwakeFlag();
        m_underActiveTime = 0;
        m_velocity = Velocity{LinearVelocity2{}, 0_rpm};
    }
}
 
inline bool Body::IsAwake() const noexcept
{
    return (m_flags & e_awakeFlag) != 0;
}
 
inline Time Body::GetUnderActiveTime() const noexcept
{
    return m_underActiveTime;
}
 
inline void Body::SetUnderActiveTime(Time value) noexcept
{
    if ((value == 0_s) || IsAccelerable())
    {
        m_underActiveTime = value;
    }
}
 
inline void Body::ResetUnderActiveTime() noexcept
{
    m_underActiveTime = 0_s;
}
 
inline bool Body::IsEnabled() const noexcept
{
    return (m_flags & e_enabledFlag) != 0;
}
 
inline bool Body::IsFixedRotation() const noexcept
{
    return (m_flags & e_fixedRotationFlag) != 0;
}
 
inline bool Body::IsSpeedable() const noexcept
{
    return (m_flags & e_velocityFlag) != 0;
}
 
inline bool Body::IsAccelerable() const noexcept
{
    return (m_flags & e_accelerationFlag) != 0;
}
 
inline void Body::SetSleepingAllowed(bool flag) noexcept
{
    if (flag)
    {
        m_flags |= e_autoSleepFlag;
    }
    else if (IsSpeedable())
    {
        m_flags &= ~e_autoSleepFlag;
        SetAwakeFlag();
        ResetUnderActiveTime();
    }
}
 
inline bool Body::IsSleepingAllowed() const noexcept
{
    return (m_flags & e_autoSleepFlag) != 0;
}
 
inline SizedRange<Body::Fixtures::const_iterator> Body::GetFixtures() const noexcept
{
    return {begin(m_fixtures), end(m_fixtures), size(m_fixtures)};
}
 
inline SizedRange<Body::Fixtures::iterator> Body::GetFixtures() noexcept
{
    return {begin(m_fixtures), end(m_fixtures), size(m_fixtures)};
}
 
inline SizedRange<Body::Joints::const_iterator> Body::GetJoints() const noexcept
{
    return {begin(m_joints), end(m_joints), size(m_joints)};
}
 
inline SizedRange<Body::Joints::iterator> Body::GetJoints() noexcept
{
    return {begin(m_joints), end(m_joints), size(m_joints)};
}
 
inline SizedRange<Body::Contacts::const_iterator> Body::GetContacts() const noexcept
{
    return {begin(m_contacts), end(m_contacts), size(m_contacts)};
}
 
inline void Body::SetUserData(void* data) noexcept
{
    m_userData = data;
}
 
inline void* Body::GetUserData() const noexcept
{
    return m_userData;
}
 
inline LinearAcceleration2 Body::GetLinearAcceleration() const noexcept
{
    return m_linearAcceleration;
}
 
inline AngularAcceleration Body::GetAngularAcceleration() const noexcept
{
    return m_angularAcceleration;
}
 
inline void Body::Advance(Real alpha) noexcept
{
    //assert(m_sweep.GetAlpha0() <= alpha);
    assert(IsSpeedable() || m_sweep.pos1 == m_sweep.pos0);
 
    // Advance to the new safe time. This doesn't sync the broad-phase.
    m_sweep.Advance0(alpha);
    m_sweep.pos1 = m_sweep.pos0;
    SetTransformation(GetTransform1(m_sweep));
}
 
inline World* Body::GetWorld() const noexcept
{
    return m_world;
}
 
inline void Body::SetMassDataDirty() noexcept
{
    m_flags |= e_massDataDirtyFlag;
}
 
inline void Body::UnsetMassDataDirty() noexcept
{
    m_flags &= ~e_massDataDirtyFlag;
}
 
inline bool Body::IsMassDataDirty() const noexcept
{
    return (m_flags & e_massDataDirtyFlag) != 0;
}
 
inline void Body::SetEnabledFlag() noexcept
{
    m_flags |= e_enabledFlag;
}
 
inline void Body::UnsetEnabledFlag() noexcept
{
    m_flags &= ~e_enabledFlag;
}
 
inline bool Body::IsIslanded() const noexcept
{
    return (m_flags & e_islandFlag) != 0;
}
 
inline void Body::SetIslandedFlag() noexcept
{
    m_flags |= e_islandFlag;
}
 
inline void Body::UnsetIslandedFlag() noexcept
{
    m_flags &= ~e_islandFlag;
}
 
// Free functions...
 
inline Acceleration GetAcceleration(const Body& body) noexcept
{
    return Acceleration{body.GetLinearAcceleration(), body.GetAngularAcceleration()};
}
 
inline void SetAcceleration(Body& body, Acceleration value) noexcept
{
    body.SetAcceleration(value.linear, value.angular);
}
 
Acceleration CalcGravitationalAcceleration(const Body& body) noexcept;
    
inline bool Awaken(Body& body) noexcept
{
    if (!body.IsAwake() && body.IsSpeedable())
    {
        body.SetAwake();
        return true;
    }
    return false;
}
 
inline bool Unawaken(Body& body) noexcept
{
    if (body.IsAwake() && body.IsSleepingAllowed())
    {
        body.UnsetAwake();
        return true;
    }
    return false;
}
 
bool ShouldCollide(const Body& lhs, const Body& rhs) noexcept;
 
inline Position GetPosition1(const Body& body) noexcept
{
    return body.GetSweep().pos1;
}
 
inline Mass GetMass(const Body& body) noexcept
{
    const auto invMass = body.GetInvMass();
    return (invMass != InvMass{0})? Mass{Real{1} / invMass}: 0_kg;
}
 
inline void SetLinearAcceleration(Body& body, LinearAcceleration2 value) noexcept
{
    body.SetAcceleration(value, body.GetAngularAcceleration());
}
 
inline void SetAngularAcceleration(Body& body, AngularAcceleration value) noexcept
{
    body.SetAcceleration(body.GetLinearAcceleration(), value);
}
 
inline void ApplyLinearAcceleration(Body& body, LinearAcceleration2 amount)
{
    SetLinearAcceleration(body, body.GetLinearAcceleration() + amount);
}
 
inline void SetForce(Body& body, Force2 force, Length2 point) noexcept
{
    const auto linAccel = LinearAcceleration2{force * body.GetInvMass()};
    const auto invRotI = body.GetInvRotInertia();
    const auto dp = point - body.GetWorldCenter();
    const auto cp = Torque{Cross(dp, force) / Radian};
    const auto angAccel = AngularAcceleration{cp * invRotI};
    body.SetAcceleration(linAccel, angAccel);
}
 
inline void ApplyForce(Body& body, Force2 force, Length2 point) noexcept
{
    // Torque is L^2 M T^-2 QP^-1.
    const auto linAccel = LinearAcceleration2{force * body.GetInvMass()};
    const auto invRotI = body.GetInvRotInertia(); // L^-2 M^-1 QP^2
    const auto dp = Length2{point - body.GetWorldCenter()}; // L
    const auto cp = Torque{Cross(dp, force) / Radian}; // L * M L T^-2 is L^2 M T^-2
    // L^2 M T^-2 QP^-1 * L^-2 M^-1 QP^2 = QP T^-2;
    const auto angAccel = AngularAcceleration{cp * invRotI};
    body.SetAcceleration(body.GetLinearAcceleration() + linAccel,
                         body.GetAngularAcceleration() + angAccel);
}
 
inline void ApplyForceToCenter(Body& body, Force2 force) noexcept
{
    const auto linAccel = body.GetLinearAcceleration() + force * body.GetInvMass();
    const auto angAccel = body.GetAngularAcceleration();
    body.SetAcceleration(linAccel, angAccel);
}
 
inline void SetTorque(Body& body, Torque torque) noexcept
{
    const auto linAccel = body.GetLinearAcceleration();
    const auto invRotI = body.GetInvRotInertia();
    const auto angAccel = torque * invRotI;
    body.SetAcceleration(linAccel, angAccel);
}
 
inline void ApplyTorque(Body& body, Torque torque) noexcept
{
    const auto linAccel = body.GetLinearAcceleration();
    const auto invRotI = body.GetInvRotInertia();
    const auto angAccel = body.GetAngularAcceleration() + torque * invRotI;
    body.SetAcceleration(linAccel, angAccel);
}
 
inline void ApplyLinearImpulse(Body& body, Momentum2 impulse, Length2 point) noexcept
{
    auto velocity = body.GetVelocity();
    velocity.linear += body.GetInvMass() * impulse;
    const auto invRotI = body.GetInvRotInertia();
    const auto dp = point - body.GetWorldCenter();
    velocity.angular += AngularVelocity{invRotI * Cross(dp, impulse) / Radian};
    body.SetVelocity(velocity);
}
 
inline void ApplyAngularImpulse(Body& body, AngularMomentum impulse) noexcept
{
    auto velocity = body.GetVelocity();
    const auto invRotI = body.GetInvRotInertia();
    velocity.angular += AngularVelocity{invRotI * impulse};
    body.SetVelocity(velocity);
}
 
Force2 GetCentripetalForce(const Body& body, Length2 axis);
 
inline RotInertia GetRotInertia(const Body& body) noexcept
{
    return Real{1} / body.GetInvRotInertia();
}
 
inline RotInertia GetLocalRotInertia(const Body& body) noexcept
{
    return GetRotInertia(body)
         + GetMass(body) * GetMagnitudeSquared(body.GetLocalCenter()) / SquareRadian;
}
 
inline LinearVelocity2 GetLinearVelocity(const Body& body) noexcept
{
    return body.GetVelocity().linear;
}
 
inline AngularVelocity GetAngularVelocity(const Body& body) noexcept
{
    return body.GetVelocity().angular;
}
 
inline void SetLinearVelocity(Body& body, const LinearVelocity2 v) noexcept
{
    body.SetVelocity(Velocity{v, GetAngularVelocity(body)});
}
 
inline void SetAngularVelocity(Body& body, AngularVelocity omega) noexcept
{
    body.SetVelocity(Velocity{GetLinearVelocity(body), omega});
}
 
inline Length2 GetWorldPoint(const Body& body, const Length2 localPoint) noexcept
{
    return Transform(localPoint, body.GetTransformation());
}
 
inline Length2 GetWorldVector(const Body& body, const Length2 localVector) noexcept
{
    return Rotate(localVector, body.GetTransformation().q);
}
 
inline UnitVec GetWorldVector(const Body& body, const UnitVec localVector) noexcept
{
    return Rotate(localVector, body.GetTransformation().q);
}
 
inline Length2 GetLocalPoint(const Body& body, const Length2 worldPoint) noexcept
{
    return InverseTransform(worldPoint, body.GetTransformation());
}
 
inline UnitVec GetLocalVector(const Body& body, const UnitVec uv) noexcept
{
    return InverseRotate(uv, body.GetTransformation().q);
}
 
inline LinearVelocity2 GetLinearVelocityFromWorldPoint(const Body& body,
                                                        const Length2 worldPoint) noexcept
{
    const auto velocity = body.GetVelocity();
    const auto worldCtr = body.GetWorldCenter();
    const auto dp = Length2{worldPoint - worldCtr};
    const auto rlv = LinearVelocity2{GetRevPerpendicular(dp) * (velocity.angular / Radian)};
    return velocity.linear + rlv;
}
 
inline LinearVelocity2 GetLinearVelocityFromLocalPoint(const Body& body,
                                                        const Length2 localPoint) noexcept
{
    return GetLinearVelocityFromWorldPoint(body, GetWorldPoint(body, localPoint));
}
 
inline Force2 GetForce(const Body& body) noexcept
{
    return body.GetLinearAcceleration() * GetMass(body);
}
 
inline Torque GetTorque(const Body& body) noexcept
{
    return body.GetAngularAcceleration() * GetRotInertia(body);
}
 
Velocity Cap(Velocity velocity, Time h, MovementConf conf) noexcept;
 
Velocity GetVelocity(const Body& body, Time h) noexcept;
 
BodyCounter GetWorldIndex(const Body* body) noexcept;
 
std::size_t GetFixtureCount(const Body& body) noexcept;
 
void RotateAboutWorldPoint(Body& body, Angle amount, Length2 worldPoint);
 
void RotateAboutLocalPoint(Body& body, Angle amount, Length2 localPoint);
 
inline Length2 GetLocation(const Body& body) noexcept
{
    return body.GetTransformation().p;
}
 
inline Angle GetAngle(const Body& body) noexcept
{
    return body.GetSweep().pos1.angular;
}
 
inline Transformation GetTransformation(const Body& body) noexcept
{
    return body.GetTransformation();
}
 
inline void SetTransformation(Body& body, const Transformation& xfm) noexcept
{
    body.SetTransform(xfm.p, GetAngle(xfm.q));
}
 
inline Position GetPosition(const Body& body) noexcept
{
    return Position{body.GetLocation(), body.GetAngle()};
}
 
inline void SetLocation(Body& body, Length2 value) noexcept
{
    body.SetTransform(value, GetAngle(body));
}
 
inline void SetAngle(Body& body, Angle value) noexcept
{
    body.SetTransform(GetLocation(body), value);
}
 
} // namespace d2
} // namespace playrho
 
#endif // PLAYRHO_DYNAMICS_BODY_HPP