orientedbox2d.h

Go to the documentation of this file.

/*
* Copyright 2015 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.
*/


// Primary contact: JUBA - secondary contact: NOBODY
#ifndef Navigation_OrientedBox2d_H
#define Navigation_OrientedBox2d_H


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

namespace Kaim
{

/*
     ^       +--------------------+
     |       |                    |
     |width  |                    |
     |       |                    |
     |       |                    |        orientation
     v      A+--------------------+------------->

             <-------------------->
                   length
*/

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

    OrientedBox2d();

    OrientedBox2d(const Vec3f& a, const Vec2f& orientation, KyFloat32 length, KyFloat32 width, KyFloat32 thickness);

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

    bool IsValid();
    void Clear();

    void Set(const Vec3f& a, const Vec2f& orientation, KyFloat32 length, KyFloat32 width, KyFloat32 thickness);

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

    bool IsInside2d(const Vec3f& pos);

    // ---------------------------------- Public Data Members ----------------------------------

    Vec3f m_a; 
    Vec2f m_normalizedOrientation; 
    KyFloat32 m_length; 
    KyFloat32 m_width; 
    KyFloat32 m_thickness; 
};


KY_INLINE OrientedBox2d::OrientedBox2d() : m_length(0.f), m_width(0.f), m_thickness(0.f) {}

KY_INLINE OrientedBox2d::OrientedBox2d(const Vec3f& a, const Vec2f& orientation, KyFloat32 length, KyFloat32 width, KyFloat32 thickness) :
m_a(a), m_length(length), m_width(width), m_thickness(thickness)
{
    orientation.GetNormalized(m_normalizedOrientation);
}

KY_INLINE bool OrientedBox2d::IsValid() { return m_normalizedOrientation != Vec2f::Zero(); }
KY_INLINE void OrientedBox2d::Clear() { Set(Vec3f::Zero(), Vec2f::Zero(), 0.f, 0.f, 0.f);  }

KY_INLINE void OrientedBox2d::Set(const Vec3f& a, const Vec2f& orientation, KyFloat32 length, KyFloat32 width, KyFloat32 thickness)
{
    KY_DEBUG_ASSERTN(orientation.GetLength() == Vec2f(orientation).Normalize(), ("Wrong BoxObtacle parameter, the orientation should be normalized"));

    m_a.Set(a.x, a.y, a.z);
    m_normalizedOrientation.Set(orientation.x, orientation.y);
    m_length = length;
    m_width = width;
    m_thickness = thickness;
}

inline void OrientedBox2d::InitAs2dInflatedSegment(const Vec3f& start, const Vec3f& dest, KyFloat32 radius)
{
    Vec2f orientation = dest.Get2d() - start.Get2d();
    KyFloat32 length = orientation.GetNormalized(m_normalizedOrientation);

    if (length == 0.f)
        m_normalizedOrientation = Vec2f::UnitX();

    m_length = length + 2.f*radius;
    m_width = 2.f*radius;
    // a = start -  m_normalizedOrientation * radius - m_normalizedOrientation.PerpCCW() * radius
    m_a.x = start.x - m_normalizedOrientation.x * radius + m_normalizedOrientation.y * radius;
    m_a.y = start.y - m_normalizedOrientation.y * radius - m_normalizedOrientation.x * radius;
    m_a.z = -KyFloat32MAXVAL;
    m_thickness = KyFloat32MAXVAL;
}

inline bool OrientedBox2d::IsInside2d(const Vec3f& pos)
{
    const Vec2f AtoP = pos.Get2d() - m_a.Get2d();
    KyFloat32 dotProd1 = DotProduct(AtoP, m_normalizedOrientation);
    KyFloat32 dotProd2 = DotProduct(AtoP, m_normalizedOrientation.PerpCCW());
    if (dotProd1 < 0.f || dotProd1 > m_length ||
        dotProd2 < 0.f || dotProd2 > m_width )
        return false;

    return true;
}

}

#endif