VelocityConstraint.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_DYNAMICS_CONTACTS_VELOCITYCONSTRAINT_HPP
#define PLAYRHO_DYNAMICS_CONTACTS_VELOCITYCONSTRAINT_HPP
 
#include <PlayRho/Common/Math.hpp>
#include <PlayRho/Dynamics/Contacts/BodyConstraint.hpp>
 
namespace playrho {
 
class StepConf;
 
namespace d2 {
 
class WorldManifold;
 
class VelocityConstraint
{
public:
    
    using size_type = std::remove_const<decltype(MaxManifoldPoints)>::type;
    
    struct Conf
    {
        Real dtRatio = 1; 
        LinearVelocity velocityThreshold = DefaultVelocityThreshold; 
        bool blockSolve = true; 
    };
    
    static PLAYRHO_CONSTEXPR inline Conf GetDefaultConf() noexcept
    {
        return Conf{};
    }
 
    VelocityConstraint() = default;
    
    VelocityConstraint(const VelocityConstraint& copy) = default;
    
    VelocityConstraint& operator= (const VelocityConstraint& copy) = default;
    
    VelocityConstraint(Real friction, Real restitution, LinearVelocity tangentSpeed,
                       const WorldManifold& worldManifold,
                       BodyConstraint& bA,
                       BodyConstraint& bB,
                       Conf conf = GetDefaultConf());
    
    UnitVec GetNormal() const noexcept { return m_normal; }
    
    UnitVec GetTangent() const noexcept { return GetFwdPerpendicular(m_normal); }
    
    size_type GetPointCount() const noexcept { return m_pointCount; }
    
    InvMass22 GetK() const noexcept;
    
    Mass22 GetNormalMass() const noexcept;
    
    Real GetFriction() const noexcept { return m_friction; }
    
    Real GetRestitution() const noexcept { return m_restitution; }
    
    LinearVelocity GetTangentSpeed() const noexcept { return m_tangentSpeed; }
    
    BodyConstraint* GetBodyA() const noexcept { return m_bodyA; }
    
    BodyConstraint* GetBodyB() const noexcept { return m_bodyB; }
    
    Momentum GetNormalImpulseAtPoint(size_type index) const noexcept;
    
    Momentum GetTangentImpulseAtPoint(size_type index) const noexcept;
    
    LinearVelocity GetVelocityBiasAtPoint(size_type index) const noexcept;
    
    Mass GetNormalMassAtPoint(size_type index) const noexcept;
    
    Mass GetTangentMassAtPoint(size_type index) const noexcept;
    
    Length2 GetPointRelPosA(size_type index) const noexcept;
    
    Length2 GetPointRelPosB(size_type index) const noexcept;
    
    void SetNormalImpulseAtPoint(size_type index, Momentum value);
    
    void SetTangentImpulseAtPoint(size_type index, Momentum value);
    
    struct Point
    {
        Length2 relA = Length2{};
        
        Length2 relB = Length2{};
        
        Momentum normalImpulse = 0_Ns;
        
        Momentum tangentImpulse = 0_Ns;
        
        Mass normalMass = 0_kg;
        
        Mass tangentMass = 0_kg;
        
        LinearVelocity velocityBias = 0_mps;
    };
    
    const Point& GetPointAt(size_type index) const
    {
        assert(index < MaxManifoldPoints);
        return m_points[index];
    }
    
private:
    
    void AddPoint(Momentum normalImpulse, Momentum tangentImpulse,
                  Length2 relA, Length2 relB, Conf conf);
    
    void RemovePoint() noexcept;
    
    Point GetPoint(Momentum normalImpulse, Momentum tangentImpulse,
                   Length2 relA, Length2 relB, Conf conf) const noexcept;
    
    Point& PointAt(size_type index)
    {
        assert(index < MaxManifoldPoints);
        return m_points[index];
    }
    
    Point m_points[MaxManifoldPoints]; 
    
    // K and normalMass fields are only used for the block solver.
    
    InvMass3 m_K = InvMass3{};
    
    Mass3 m_normalMass = Mass3{};
    
    BodyConstraint* m_bodyA = nullptr; 
    BodyConstraint* m_bodyB = nullptr; 
    
    UnitVec m_normal = GetInvalid<UnitVec>(); 
    
    Real m_friction = GetInvalid<Real>();
    
    Real m_restitution = GetInvalid<Real>(); 
    
    LinearVelocity m_tangentSpeed = GetInvalid<decltype(m_tangentSpeed)>(); 
    
    size_type m_pointCount = 0; 
};
 
inline void VelocityConstraint::RemovePoint() noexcept
{
    assert(m_pointCount > 0);
    m_points[m_pointCount - 1] = Point{};
    --m_pointCount;
}
 
inline InvMass22 VelocityConstraint::GetK() const noexcept
{
    return InvMass22{
        InvMass2{get<0>(m_K), get<2>(m_K)},
        InvMass2{get<2>(m_K), get<1>(m_K)}
    };
}
 
inline Mass22 VelocityConstraint::GetNormalMass() const noexcept
{
    return Mass22{
        Mass2{get<0>(m_normalMass), get<2>(m_normalMass)},
        Mass2{get<2>(m_normalMass), get<1>(m_normalMass)}
    };
}
 
inline Length2 VelocityConstraint::GetPointRelPosA(VelocityConstraint::size_type index) const noexcept
{
    return GetPointAt(index).relA;
}
 
inline Length2 VelocityConstraint::GetPointRelPosB(VelocityConstraint::size_type index) const noexcept
{
    return GetPointAt(index).relB;
}
 
inline LinearVelocity VelocityConstraint::GetVelocityBiasAtPoint(size_type index) const noexcept
{
    return GetPointAt(index).velocityBias;
}
 
inline Mass VelocityConstraint::GetNormalMassAtPoint(VelocityConstraint::size_type index) const noexcept
{
    return GetPointAt(index).normalMass;
}
 
inline Mass VelocityConstraint::GetTangentMassAtPoint(VelocityConstraint::size_type index) const noexcept
{
    return GetPointAt(index).tangentMass;
}
 
inline Momentum VelocityConstraint::GetNormalImpulseAtPoint(VelocityConstraint::size_type index) const noexcept
{
    return GetPointAt(index).normalImpulse;
}
 
inline Momentum VelocityConstraint::GetTangentImpulseAtPoint(VelocityConstraint::size_type index) const noexcept
{
    return GetPointAt(index).tangentImpulse;
}
 
inline void VelocityConstraint::SetNormalImpulseAtPoint(VelocityConstraint::size_type index, Momentum value)
{
    PointAt(index).normalImpulse = value;
}
 
inline void VelocityConstraint::SetTangentImpulseAtPoint(VelocityConstraint::size_type index, Momentum value)
{
    PointAt(index).tangentImpulse = value;
}
 
// Free functions...
 
VelocityConstraint::Conf GetRegVelocityConstraintConf(const StepConf& conf) noexcept;
 
VelocityConstraint::Conf GetToiVelocityConstraintConf(const StepConf& conf) noexcept;
 
inline UnitVec GetNormal(const VelocityConstraint& vc) noexcept
{
    return vc.GetNormal();
}
 
inline UnitVec GetTangent(const VelocityConstraint& vc) noexcept
{
    return vc.GetTangent();
}
 
inline InvMass GetInvMass(const VelocityConstraint& vc) noexcept
{
    return vc.GetBodyA()->GetInvMass() + vc.GetBodyB()->GetInvMass();
}
 
inline Length2 GetPointRelPosA(const VelocityConstraint& vc,
                                VelocityConstraint::size_type index)
{
    return vc.GetPointRelPosA(index);
}
 
inline Length2 GetPointRelPosB(const VelocityConstraint& vc,
                                VelocityConstraint::size_type index)
{
    return vc.GetPointRelPosB(index);
}
 
inline LinearVelocity GetVelocityBiasAtPoint(const VelocityConstraint& vc, VelocityConstraint::size_type index)
{
    return vc.GetVelocityBiasAtPoint(index);
}
 
inline Mass GetNormalMassAtPoint(const VelocityConstraint& vc,
                                 VelocityConstraint::size_type index)
{
    return vc.GetNormalMassAtPoint(index);
}
 
inline Mass GetTangentMassAtPoint(const VelocityConstraint& vc,
                                  VelocityConstraint::size_type index)
{
    return vc.GetTangentMassAtPoint(index);
}
 
inline Momentum GetNormalImpulseAtPoint(const VelocityConstraint& vc,
                                        VelocityConstraint::size_type index)
{
    return vc.GetNormalImpulseAtPoint(index);
}
 
inline Momentum GetTangentImpulseAtPoint(const VelocityConstraint& vc,
                                         VelocityConstraint::size_type index)
{
    return vc.GetTangentImpulseAtPoint(index);
}
 
inline Momentum2 GetNormalImpulses(const VelocityConstraint& vc)
{
    return Momentum2{GetNormalImpulseAtPoint(vc, 0), GetNormalImpulseAtPoint(vc, 1)};
}
 
inline Momentum2 GetTangentImpulses(const VelocityConstraint& vc)
{
    return Momentum2{GetTangentImpulseAtPoint(vc, 0), GetTangentImpulseAtPoint(vc, 1)};
}
 
inline void SetNormalImpulseAtPoint(VelocityConstraint& vc, VelocityConstraint::size_type index, Momentum value)
{
    vc.SetNormalImpulseAtPoint(index, value);
}
 
inline void SetTangentImpulseAtPoint(VelocityConstraint& vc, VelocityConstraint::size_type index, Momentum value)
{
    vc.SetTangentImpulseAtPoint(index, value);
}
 
inline void SetNormalImpulses(VelocityConstraint& vc, const Momentum2 impulses)
{
    SetNormalImpulseAtPoint(vc, 0, impulses[0]);
    SetNormalImpulseAtPoint(vc, 1, impulses[1]);
}
 
inline void SetTangentImpulses(VelocityConstraint& vc, const Momentum2 impulses)
{
    SetTangentImpulseAtPoint(vc, 0, impulses[0]);
    SetTangentImpulseAtPoint(vc, 1, impulses[1]);
}
 
} // namespace d2
} // namespace playrho
 
#endif // PLAYRHO_DYNAMICS_CONTACTS_VELOCITYCONSTRAINT_HPP