orientedbox2d.h

Go to the documentation of this file.

/*
* Copyright 2016 Autodesk, Inc. All rights reserved.
* Use of this software is subject to the terms of the Autodesk license agreement and any attachments or Appendices thereto provided at the time of installation or download,
* or which otherwise accompanies this software in either electronic or hard copy form, or which is signed by you and accepted by Autodesk.
*/

#pragma once

#include "gwnavruntime/math/vec3f.h"
#include "gwnavruntime/kernel/SF_Debug.h"
#include "gwnavruntime/math/box3f.h"

namespace Kaim
{

class OrientedBox2d
{
    KY_DEFINE_NEW_DELETE_OPERATORS(Stat_Default_Mem)
public:
    // ---------------------------------- Constructors ----------------------------------

    OrientedBox2d() : m_forward_length(0.0f), m_left_length(0.0f), m_height(0.0f) {}

    OrientedBox2d(const Vec3f& corner, const Vec2f& forward, KyFloat32 forward_length, KyFloat32 left_length, KyFloat32 height) :
        m_corner(corner), m_forward_length(forward_length), m_left_length(left_length), m_height(height)
    {
        forward.GetNormalized(m_forward_dir2d);
    }

    // ---------------------------------- Main API Functions ----------------------------------

    bool IsValid() { return m_forward_dir2d != Vec2f::Zero(); }

    void Clear() { Set(Vec3f::Zero(), Vec2f::Zero(), 0.0f, 0.0f, 0.0f); }

    void Set(const Vec3f& corner, const Vec2f& forward_dir2d, KyFloat32 forward_length, KyFloat32 left_length, KyFloat32 height)
    {
        KY_DEBUG_ASSERTN(forward_dir2d.GetLength() == Vec2f(forward_dir2d).Normalize(), ("Wrong OrientedBox2d.Set() parameters, forward_dir2d should be normalized"));

        m_corner         = corner;
        m_forward_dir2d  = forward_dir2d;
        m_forward_length = forward_length;
        m_left_length    = left_length;
        m_height         = height;
    }

    void InitAs2dInflatedSegment(const Vec3f& start, const Vec3f& dest, KyFloat32 radius);

    bool IsInside2d(const Vec3f& pos);

    Box3f ComputeAABB() const;

public:
    // ---------------------------------- Public Data Members ----------------------------------
    Vec3f m_corner;
    Vec2f m_forward_dir2d;
    KyFloat32 m_forward_length;
    KyFloat32 m_left_length;
    KyFloat32 m_height;
};

inline void OrientedBox2d::InitAs2dInflatedSegment(const Vec3f& P, const Vec3f& Q, KyFloat32 radius)
{
    Vec2f dir2d = Q.Get2d() - P.Get2d();
    KyFloat32 length = dir2d.GetNormalized(m_forward_dir2d);

    if (length == 0.0f)
        m_forward_dir2d = Vec2f::UnitX();

    m_forward_length = length + 2.0f * radius;
    m_left_length  = 2.0f * radius;
    m_corner.x = P.x - m_forward_dir2d.x * radius + m_forward_dir2d.y * radius;
    m_corner.y = P.y - m_forward_dir2d.y * radius - m_forward_dir2d.x * radius;
    m_corner.z = -KyFloat32MAXVAL;
    m_height = KyFloat32MAXVAL;
}

inline bool OrientedBox2d::IsInside2d(const Vec3f& P)
{
    const Vec2f& AP = P.Get2d() - m_corner.Get2d();
    KyFloat32 along_dir = DotProduct(AP, m_forward_dir2d);
    KyFloat32 ortho_dir = DotProduct(AP, m_forward_dir2d.PerpCCW());
    if (along_dir < 0.0f || along_dir > m_forward_length || ortho_dir < 0.0f || ortho_dir > m_left_length)
        return false;

    return true;
}

inline Box3f OrientedBox2d::ComputeAABB() const
{
    Vec2f leftDir = m_forward_dir2d.PerpCCW();
    Vec3f B = m_corner + m_forward_dir2d * m_forward_length;
    Vec3f C = B + leftDir * m_left_length;
    Vec3f D = m_corner + leftDir * m_left_length;
    D.z += m_height; // used to expand altitude as well

    Box3f aabb;
    aabb.ExpandByPos(m_corner);
    aabb.ExpandByPos(B);
    aabb.ExpandByPos(C);
    aabb.ExpandByPos(D);
    return aabb;
}


}