/*
 *  OrientedBoundingBox.hpp
 *
 *  Created by Thant on 1/5/2011
 *  Copyright 2011 TeleNav Inc. All rights reserved.
 *
 *  Implements oriented bounding boxes along with functions to
 *  transform and test for intersection.
 *
 *  An oriented bounding box is a rectangular volume represented by a
 *  center point and three (presumably orthogonal) vectors. The length
 *  of the vectors indicate the dimensions along those axes.
 *
 */

#ifndef Foundation_Math_OrientedBoundingBox_INCLUDED
#define Foundation_Math_OrientedBoundingBox_INCLUDED

#include <assert.h>
#include <foundation/Math/Point.hpp>
#include <foundation/Math/Vec.hpp>
#include <foundation/Math/Matrix.hpp>
#include <foundation/Math/transform.hpp>
#include <foundation/Math/OrientedBoundingRect.hpp>

namespace Tn {

namespace math {

template <typename T>
struct OrientedBoundingBox
{
    OrientedBoundingBox(Point<3,T> const & center, T width, T height, T depth);
    OrientedBoundingBox(Point<3,T> const & lower_left_far, Point<3,T> const & upper_right_near);

    // For construction of non-axis-aligned bounding box
    OrientedBoundingBox(Point<3,T> const & center, 
                        Vec<3,T> const & x_axis,
                        Vec<3,T> const & y_axis,
                        Vec<3,T> const & z_axis);

    Point<3,T> center;
    Vec<3,T> axes[3];

    // [0-7] eight corners
    Point<3,T> corner(int i) const;

    // [0-2] three faces facing viewer (positive z axis)
    OrientedBoundingRect<T> face(int i) const;
};



typedef OrientedBoundingBox<float> OrientedBoundingBoxf;
typedef OrientedBoundingBox<double> OrientedBoundingBoxd;

template <typename T>
OrientedBoundingBox<T>::OrientedBoundingBox(Point<3,T> const & center_,
                                            Vec<3,T> const & x_axis,
                                            Vec<3,T> const & y_axis,
                                            Vec<3,T> const & z_axis)
    : center(center_)
{
    axes[0] = x_axis;
    axes[1] = y_axis;
    axes[2] = z_axis;
}

template <typename T>
OrientedBoundingBox<T>::OrientedBoundingBox(Point<3,T> const & center_, T width, T height, T depth) 
  : center(center_)
{
    axes[0] = Vec3<T>(width/T(2),        T(0),       T(0));
    axes[1] = Vec3<T>(      T(0), height/T(2),       T(0));
    axes[2] = Vec3<T>(      T(0),        T(0), depth/T(2));
}


template <typename T>
OrientedBoundingBox<T>::OrientedBoundingBox(Point<3,T> const & llf, Point<3,T> const & urn)
    : center(Point3<T>((llf[0]+urn[0])/T(2),
                       (llf[1]+urn[1])/T(2),
                       (llf[2]+urn[2])/T(2)))
{
    T width  = urn[0] - llf[0];
    T height = urn[1] - llf[1];
    T depth  = urn[2] - llf[2];

    axes[0] = Vec3<T>(width/T(2),        T(0),       T(0));
    axes[1] = Vec3<T>(      T(0), height/T(2),       T(0));
    axes[2] = Vec3<T>(      T(0),        T(0), depth/T(2));
}


template <typename T>
Point<3,T> OrientedBoundingBox<T>::corner(int i) const
{
    assert((i>=0) && (i<8));
    return
        center
        + ((i & (0x1 << 0)) ? axes[0] : -axes[0])
        + ((i & (0x1 << 1)) ? axes[1] : -axes[1])
        + ((i & (0x1 << 2)) ? axes[2] : -axes[2]);
}


template <typename T>
OrientedBoundingRect<T> OrientedBoundingBox<T>::face(int i) const
{
    assert((i>=0) && (i<3));
    int i0 = i % 3;
    int i1 = (i+1) % 3;
    int i2 = (i+2) % 3;

    return 
        (axes[i0][2]>0)
        ?
        // axis pointing toward viewer
        OrientedBoundingRect<T>(center + axes[i0],
                                axes[i1],
                                axes[i2])
        :
        // axis pointing away from view, use other side
        OrientedBoundingRect<T>(center - axes[i0],
                                -axes[i2],
                                -axes[i1]);
}


template <typename T>
OrientedBoundingBox<T> transform(Vec<3,T> const & v, OrientedBoundingBox<T> const & box)
{
    return OrientedBoundingBox<T>(box.center + v,
                                  box.axes[0],
                                  box.axes[1],
                                  box.axes[2]);
}


template <typename T>
OrientedBoundingBox<T> transform(Matrix<T> const & m, OrientedBoundingBox<T> const & box)
{
    Point<3,T> new_center = transform(m, box.center);

    return OrientedBoundingBox<T>(new_center,
                                  transform(m, box.center + box.axes[0]) - new_center,
                                  transform(m, box.center + box.axes[1]) - new_center,
                                  transform(m, box.center + box.axes[2]) - new_center);
}


template <typename T>
bool collide(OrientedBoundingBox<T> const & box, Point<3,T> const & face, Vec<3,T> const & axis)
{
    for (int i=0; i<8; ++i)
    {
        Vec<3,T> v = box.corner(i) - face;
        if (dot(v, axis) < 0) return true;
    }
    return false;
}


template <typename T>
bool collide(OrientedBoundingBox<T> const & a, OrientedBoundingBox<T> const & b)
{
    // First do a bounding sphere test.

    T r_a = magnitude(a.axes[0] + a.axes[1] + a.axes[2]);
    T r_b = magnitude(b.axes[0] + b.axes[1] + b.axes[2]);

    Vec<3,T> a2b = b.center - a.center;

    T r = magnitude(a2b);

    if (r > r_a + r_b) return false;

    // Test a's separation axes against b's corners. If we find one
    // that works, we know the bounding boxes don't intersect.

    for (int i=0; i<3; ++i)
    {
        Vec<3,T> axis = (0 < dot(a.axes[i], a2b)) ? a.axes[i] : -a.axes[i];
        Point<3,T> face = a.center + axis;
        if (!collide(b, face, axis)) return false;  // found one that works
    }

    // Test b's separation axes against a's corners.

    for (int i=0; i<3; ++i)
    {
        Vec<3,T> axis = (0 < dot(b.axes[i], -a2b)) ? b.axes[i] : -b.axes[i];
        Point<3,T> face = b.center + axis;
        if (!collide(a, face, axis)) return false;  // found one that works
    }

    return true;
}


template <typename T>
OrientedBoundingBox<T> operator*(OrientedBoundingBox<T> const & a, T s)
{
    return OrientedBoundingBox<T>(a.center, a.axes[0] * s, a.axes[1] * s, a.axes[2] * s);
}


} // namespace math

} // namespace Tn

#endif

