body.hpp

Go to the documentation of this file.

#pragma once
 
#include "math.hpp"
 
T2D_NAMESPACE_BEGIN
 
template<class TileData>
class World;
 
enum class BodyType : uint8_t {
    Tile,
    Bullet,
    Invalid
};
 
class WorldBody {
    template<class TileData>
    friend class World;
    friend class ResolveMethods;
public:
    WorldBody(uint32_t id, BodyType bodyType) 
        : m_id(id), m_bodyType(bodyType) {}
 
    uint32_t id() const {
        return m_id;
    }
 
    BodyType bodyType() const {
        return m_bodyType;
    }
 
    Transform& transform() {
        return m_transform;
    }
 
    const Transform& transform() const {
        return m_transform;
    }
 
    void moveBy(const vec2& amount) {
        m_flags[NEEDS_REINSERT] = true;
        m_transform.pos += amount;
    }
 
    void rotateBy(Float amount) {
        m_flags[NEEDS_REINSERT] = true;
        m_transform.rot += amount;
        m_transform.update();
    }
 
    void setPos(const vec2& pos) {
        m_flags[NEEDS_REINSERT] = true;
        m_transform.pos = pos;
    }
 
    void setRot(Float rot) {
        m_flags[NEEDS_REINSERT] = true;
        m_transform.rot = rot;
        m_transform.update();
    }
 
    virtual void integrate(Float dt) = 0;
 
    enum FlagIndexes {
        NEEDS_REINSERT = 0, // needs to be reinserted into the spatial tree or grid.
        IS_STATIC = 1 // the body is static
    };
 
    bool isStatic() const {
        return m_flags[IS_STATIC];
    }
 
    virtual void setStatic(bool static_) {
        m_flags[IS_STATIC] = static_;
    }
 
    virtual vec2 getWorldPoint(const vec2& localPoint) const {
        return m_transform.getWorldPoint(localPoint);
    }
 
    virtual vec2 getLocalPoint(const vec2& worldPoint) const {
        return m_transform.getLocalPoint(worldPoint);
    }
 
    virtual vec2 getWorldPos() {
        return m_transform.pos;
    }
 
    void addLinearVel(const vec2& vel) {
        m_linearVelocity += vel * (Float)!m_flags[IS_STATIC];
    }
 
    Float mass() const {
        return m_mass;
    }
 
    Float inverseMass() const {
        return m_inverseMass;
    }
 
    virtual AABB<Float> getAABB() const = 0;
 
    // get the local AABB (of the entire object) in the localSpace of spaceTransform
    inline AABB<Float> getAABB(const Transform& spaceTransform, const vec2& localOffset = { Float(0.5f), Float(0.5f) }) const {
        return getAABB(getAABB(), spaceTransform, localOffset);
    }
 
    // get the some local AABB in the localSpace of spaceTransform
    inline AABB<Float> getAABB(const AABB<Float>& localAABB, const Transform& spaceTransform, const vec2& localOffset = { Float(0.5f), Float(0.5f) }) const {
        vec2 relPos = spaceTransform.getLocalPoint(getWorldPoint(localAABB.midpoint()), localOffset);
        return AABB<Float>(relPos, localAABB.width() * 0.5f, localAABB.height() * 0.5f).rotate(spaceTransform.sincos - m_transform.sincos);
    }
 
protected:
    Transform m_transform;
    Float m_mass = 0.0f;
    Float m_inverseMass = 0;
    vec2 m_linearVelocity = { 0.0f, 0.0f };
 
protected:
    std::bitset<4> m_flags;
    BodyType m_bodyType = BodyType::Invalid;
private:
    uint32_t m_id = 0;
};
 
class Body : public WorldBody {
    friend class ImpulseMethod;
    template<class TileData>
    friend class World;
 
public:
    Body(uint32_t id, BodyType type)
        : WorldBody(id, type) {}
 
    virtual void integrate(Float dt) override {
        assert(isValid());
 
        // Using the semi implicit euler method
 
        // A(v) = force / mass
        // V = v + A(v) * dt
        // P = p + V * dt;
        m_linearVelocity = m_linearVelocity + m_force * m_inverseMass * dt;
        vec2 newPos = m_transform.pos + m_linearVelocity * dt;
        m_force = { 0.0f, 0.0f };
 
        m_angularVelocity = m_angularVelocity + m_torque * m_inverseI * dt;
        Float rot = m_transform.rot + m_angularVelocity * dt;
        m_torque = 0.0f;
 
        m_transform.update();
 
        if(!nearlyEqual(newPos, m_transform.pos, 0.1f) || !nearlyEqual(rot, m_transform.rot, 0.05f))
            m_flags[NEEDS_REINSERT] = true;
 
        m_transform.pos = newPos;
        m_transform.rot = rot;
    }
 
    virtual vec2 getWorldPoint(const vec2& localPoint) const override {
        return m_transform.getWorldPoint(localPoint, m_com);
    }
 
    virtual vec2 getLocalPoint(const vec2& worldPoint) const override {
        return m_transform.getLocalPoint(worldPoint, m_com);
    }
 
    virtual vec2 getWorldPos() override {
        return m_transform.pos + m_com;
    }
 
    virtual void setStatic(bool static_) override {
        WorldBody::setStatic(static_);
 
        if (static_) {
            m_inverseI = 0;
            m_inverseMass = 0;
            m_linearVelocity = vec2(0);
            m_angularVelocity = 0;
            m_com = { 0, 0 };
        }
        else {
            m_inverseI = (Float)1 / m_I;
            m_inverseMass = (Float)1 / m_mass;
            m_com = m_unweightedCom * m_inverseMass;
        }
    }
 
    void addAngularVel(const Float& vel) {
        m_angularVelocity = vel * (Float)!m_flags[IS_STATIC];
    }
 
    const vec2& com() const {
        return m_com;
    }
 
    const vec2& centroid() const {
        return m_centroid;
    }
 
    Float rotationalInertia() const {
        return m_I;
    }
 
protected:
    Float m_inverseI = 0;
 
    vec2 m_centroid = { 0.0f, 0.0f };
    vec2 m_com = { 0.0f, 0.0f };
    Float m_I = 0.0f;
    vec2 m_unweightedCom = { 0.0f, 0.0f };
    vec2 m_unweightedCentroid = { 0.0f, 0.0f };
 
    vec2 m_force = { 0.0f, 0.0f };
    Float m_angularVelocity = 0.0f;
    Float m_torque = 0.0f;
};
 
class BulletBody : public WorldBody {
public:
    BulletBody(uint32_t id, Float radius)
        : WorldBody(id, BodyType::Bullet), m_radius(radius) {}
 
    virtual void integrate(Float dt) override {
        m_transform.pos = m_transform.pos + m_linearVelocity * dt;
    }
 
    virtual AABB<Float> getAABB() const {
        const Float m_aabbRadius = m_radius * sqrt<Float>(2.0f);
 
        return AABB<Float>(m_transform.pos, m_aabbRadius, m_aabbRadius);
    }
 
private:
    Float m_radius;
    Float m_mass = 0.0f;
};
 
T2D_NAMESPACE_END