#ifndef INCLUDED_IMATHBOXALGO_H
#define INCLUDED_IMATHBOXALGO_H
#include "ImathBox.h"
#include "ImathMatrix.h"
#include "ImathLineAlgo.h"
#include "ImathPlane.h"
namespace Imath {
template <class T>
inline T
clip (const T &p, const Box<T> &box)
{
T q;
for (int i = 0; i < int (box.min.dimensions()); i++)
{
if (p[i] < box.min[i])
q[i] = box.min[i];
else if (p[i] > box.max[i])
q[i] = box.max[i];
else
q[i] = p[i];
}
return q;
}
template <class T>
inline T
closestPointInBox (const T &p, const Box<T> &box)
{
return clip (p, box);
}
template <class T>
Vec3<T>
closestPointOnBox (const Vec3<T> &p, const Box< Vec3<T> > &box)
{
if (box.isEmpty())
return p;
Vec3<T> q = closestPointInBox (p, box);
if (q == p)
{
Vec3<T> d1 = p - box.min;
Vec3<T> d2 = box.max - p;
Vec3<T> d ((d1.x < d2.x)? d1.x: d2.x,
(d1.y < d2.y)? d1.y: d2.y,
(d1.z < d2.z)? d1.z: d2.z);
if (d.x < d.y && d.x < d.z)
{
q.x = (d1.x < d2.x)? box.min.x: box.max.x;
}
else if (d.y < d.z)
{
q.y = (d1.y < d2.y)? box.min.y: box.max.y;
}
else
{
q.z = (d1.z < d2.z)? box.min.z: box.max.z;
}
}
return q;
}
template <class S, class T>
Box< Vec3<S> >
transform (const Box< Vec3<S> > &box, const Matrix44<T> &m)
{
if (box.isEmpty() || box.isInfinite())
return box;
if (m[0][3] == 0 && m[1][3] == 0 && m[2][3] == 0 && m[3][3] == 1)
{
Box< Vec3<S> > newBox;
for (int i = 0; i < 3; i++)
{
newBox.min[i] = newBox.max[i] = (S) m[3][i];
for (int j = 0; j < 3; j++)
{
S a, b;
a = (S) m[j][i] * box.min[j];
b = (S) m[j][i] * box.max[j];
if (a < b)
{
newBox.min[i] += a;
newBox.max[i] += b;
}
else
{
newBox.min[i] += b;
newBox.max[i] += a;
}
}
}
return newBox;
}
Vec3<S> points[8];
points[0][0] = points[1][0] = points[2][0] = points[3][0] = box.min[0];
points[4][0] = points[5][0] = points[6][0] = points[7][0] = box.max[0];
points[0][1] = points[1][1] = points[4][1] = points[5][1] = box.min[1];
points[2][1] = points[3][1] = points[6][1] = points[7][1] = box.max[1];
points[0][2] = points[2][2] = points[4][2] = points[6][2] = box.min[2];
points[1][2] = points[3][2] = points[5][2] = points[7][2] = box.max[2];
Box< Vec3<S> > newBox;
for (int i = 0; i < 8; i++)
newBox.extendBy (points[i] * m);
return newBox;
}
template <class S, class T>
void
transform (const Box< Vec3<S> > &box,
const Matrix44<T> &m,
Box< Vec3<S> > &result)
{
if (box.isEmpty() || box.isInfinite())
{
return;
}
if (m[0][3] == 0 && m[1][3] == 0 && m[2][3] == 0 && m[3][3] == 1)
{
for (int i = 0; i < 3; i++)
{
result.min[i] = result.max[i] = (S) m[3][i];
for (int j = 0; j < 3; j++)
{
S a, b;
a = (S) m[j][i] * box.min[j];
b = (S) m[j][i] * box.max[j];
if (a < b)
{
result.min[i] += a;
result.max[i] += b;
}
else
{
result.min[i] += b;
result.max[i] += a;
}
}
}
return;
}
Vec3<S> points[8];
points[0][0] = points[1][0] = points[2][0] = points[3][0] = box.min[0];
points[4][0] = points[5][0] = points[6][0] = points[7][0] = box.max[0];
points[0][1] = points[1][1] = points[4][1] = points[5][1] = box.min[1];
points[2][1] = points[3][1] = points[6][1] = points[7][1] = box.max[1];
points[0][2] = points[2][2] = points[4][2] = points[6][2] = box.min[2];
points[1][2] = points[3][2] = points[5][2] = points[7][2] = box.max[2];
for (int i = 0; i < 8; i++)
result.extendBy (points[i] * m);
}
template <class S, class T>
Box< Vec3<S> >
affineTransform (const Box< Vec3<S> > &box, const Matrix44<T> &m)
{
if (box.isEmpty() || box.isInfinite())
{
return box;
}
Box< Vec3<S> > newBox;
for (int i = 0; i < 3; i++)
{
newBox.min[i] = newBox.max[i] = (S) m[3][i];
for (int j = 0; j < 3; j++)
{
S a, b;
a = (S) m[j][i] * box.min[j];
b = (S) m[j][i] * box.max[j];
if (a < b)
{
newBox.min[i] += a;
newBox.max[i] += b;
}
else
{
newBox.min[i] += b;
newBox.max[i] += a;
}
}
}
return newBox;
}
template <class S, class T>
void
affineTransform (const Box< Vec3<S> > &box,
const Matrix44<T> &m,
Box<Vec3<S> > &result)
{
if (box.isEmpty())
{
result.makeEmpty();
return;
}
if (box.isInfinite())
{
result.makeInfinite();
return;
}
for (int i = 0; i < 3; i++)
{
result.min[i] = result.max[i] = (S) m[3][i];
for (int j = 0; j < 3; j++)
{
S a, b;
a = (S) m[j][i] * box.min[j];
b = (S) m[j][i] * box.max[j];
if (a < b)
{
result.min[i] += a;
result.max[i] += b;
}
else
{
result.min[i] += b;
result.max[i] += a;
}
}
}
}
template <class T>
bool
findEntryAndExitPoints (const Line3<T> &r,
const Box<Vec3<T> > &b,
Vec3<T> &entry,
Vec3<T> &exit)
{
if (b.isEmpty())
{
return false;
}
const T TMAX = limits<T>::max();
T tFrontMax = -TMAX;
T tBackMin = TMAX;
if (r.dir.x >= 0)
{
T d1 = b.max.x - r.pos.x;
T d2 = b.min.x - r.pos.x;
if (r.dir.x > 1 ||
(abs (d1) < TMAX * r.dir.x &&
abs (d2) < TMAX * r.dir.x))
{
T t1 = d1 / r.dir.x;
T t2 = d2 / r.dir.x;
if (tBackMin > t1)
{
tBackMin = t1;
exit.x = b.max.x;
exit.y = clamp (r.pos.y + t1 * r.dir.y, b.min.y, b.max.y);
exit.z = clamp (r.pos.z + t1 * r.dir.z, b.min.z, b.max.z);
}
if (tFrontMax < t2)
{
tFrontMax = t2;
entry.x = b.min.x;
entry.y = clamp (r.pos.y + t2 * r.dir.y, b.min.y, b.max.y);
entry.z = clamp (r.pos.z + t2 * r.dir.z, b.min.z, b.max.z);
}
}
else if (r.pos.x < b.min.x || r.pos.x > b.max.x)
{
return false;
}
}
else
{
T d1 = b.min.x - r.pos.x;
T d2 = b.max.x - r.pos.x;
if (r.dir.x < -1 ||
(abs (d1) < -TMAX * r.dir.x &&
abs (d2) < -TMAX * r.dir.x))
{
T t1 = d1 / r.dir.x;
T t2 = d2 / r.dir.x;
if (tBackMin > t1)
{
tBackMin = t1;
exit.x = b.min.x;
exit.y = clamp (r.pos.y + t1 * r.dir.y, b.min.y, b.max.y);
exit.z = clamp (r.pos.z + t1 * r.dir.z, b.min.z, b.max.z);
}
if (tFrontMax < t2)
{
tFrontMax = t2;
entry.x = b.max.x;
entry.y = clamp (r.pos.y + t2 * r.dir.y, b.min.y, b.max.y);
entry.z = clamp (r.pos.z + t2 * r.dir.z, b.min.z, b.max.z);
}
}
else if (r.pos.x < b.min.x || r.pos.x > b.max.x)
{
return false;
}
}
if (r.dir.y >= 0)
{
T d1 = b.max.y - r.pos.y;
T d2 = b.min.y - r.pos.y;
if (r.dir.y > 1 ||
(abs (d1) < TMAX * r.dir.y &&
abs (d2) < TMAX * r.dir.y))
{
T t1 = d1 / r.dir.y;
T t2 = d2 / r.dir.y;
if (tBackMin > t1)
{
tBackMin = t1;
exit.x = clamp (r.pos.x + t1 * r.dir.x, b.min.x, b.max.x);
exit.y = b.max.y;
exit.z = clamp (r.pos.z + t1 * r.dir.z, b.min.z, b.max.z);
}
if (tFrontMax < t2)
{
tFrontMax = t2;
entry.x = clamp (r.pos.x + t2 * r.dir.x, b.min.x, b.max.x);
entry.y = b.min.y;
entry.z = clamp (r.pos.z + t2 * r.dir.z, b.min.z, b.max.z);
}
}
else if (r.pos.y < b.min.y || r.pos.y > b.max.y)
{
return false;
}
}
else
{
T d1 = b.min.y - r.pos.y;
T d2 = b.max.y - r.pos.y;
if (r.dir.y < -1 ||
(abs (d1) < -TMAX * r.dir.y &&
abs (d2) < -TMAX * r.dir.y))
{
T t1 = d1 / r.dir.y;
T t2 = d2 / r.dir.y;
if (tBackMin > t1)
{
tBackMin = t1;
exit.x = clamp (r.pos.x + t1 * r.dir.x, b.min.x, b.max.x);
exit.y = b.min.y;
exit.z = clamp (r.pos.z + t1 * r.dir.z, b.min.z, b.max.z);
}
if (tFrontMax < t2)
{
tFrontMax = t2;
entry.x = clamp (r.pos.x + t2 * r.dir.x, b.min.x, b.max.x);
entry.y = b.max.y;
entry.z = clamp (r.pos.z + t2 * r.dir.z, b.min.z, b.max.z);
}
}
else if (r.pos.y < b.min.y || r.pos.y > b.max.y)
{
return false;
}
}
if (r.dir.z >= 0)
{
T d1 = b.max.z - r.pos.z;
T d2 = b.min.z - r.pos.z;
if (r.dir.z > 1 ||
(abs (d1) < TMAX * r.dir.z &&
abs (d2) < TMAX * r.dir.z))
{
T t1 = d1 / r.dir.z;
T t2 = d2 / r.dir.z;
if (tBackMin > t1)
{
tBackMin = t1;
exit.x = clamp (r.pos.x + t1 * r.dir.x, b.min.x, b.max.x);
exit.y = clamp (r.pos.y + t1 * r.dir.y, b.min.y, b.max.y);
exit.z = b.max.z;
}
if (tFrontMax < t2)
{
tFrontMax = t2;
entry.x = clamp (r.pos.x + t2 * r.dir.x, b.min.x, b.max.x);
entry.y = clamp (r.pos.y + t2 * r.dir.y, b.min.y, b.max.y);
entry.z = b.min.z;
}
}
else if (r.pos.z < b.min.z || r.pos.z > b.max.z)
{
return false;
}
}
else
{
T d1 = b.min.z - r.pos.z;
T d2 = b.max.z - r.pos.z;
if (r.dir.z < -1 ||
(abs (d1) < -TMAX * r.dir.z &&
abs (d2) < -TMAX * r.dir.z))
{
T t1 = d1 / r.dir.z;
T t2 = d2 / r.dir.z;
if (tBackMin > t1)
{
tBackMin = t1;
exit.x = clamp (r.pos.x + t1 * r.dir.x, b.min.x, b.max.x);
exit.y = clamp (r.pos.y + t1 * r.dir.y, b.min.y, b.max.y);
exit.z = b.min.z;
}
if (tFrontMax < t2)
{
tFrontMax = t2;
entry.x = clamp (r.pos.x + t2 * r.dir.x, b.min.x, b.max.x);
entry.y = clamp (r.pos.y + t2 * r.dir.y, b.min.y, b.max.y);
entry.z = b.max.z;
}
}
else if (r.pos.z < b.min.z || r.pos.z > b.max.z)
{
return false;
}
}
return tFrontMax <= tBackMin;
}
template<class T>
bool
intersects (const Box< Vec3<T> > &b, const Line3<T> &r, Vec3<T> &ip)
{
if (b.isEmpty())
{
return false;
}
if (b.intersects (r.pos))
{
ip = r.pos;
return true;
}
const T TMAX = limits<T>::max();
T tFrontMax = -1;
T tBackMin = TMAX;
if (r.dir.x > 0)
{
if (r.pos.x > b.max.x)
return false;
T d = b.max.x - r.pos.x;
if (r.dir.x > 1 || d < TMAX * r.dir.x)
{
T t = d / r.dir.x;
if (tBackMin > t)
tBackMin = t;
}
if (r.pos.x <= b.min.x)
{
T d = b.min.x - r.pos.x;
T t = (r.dir.x > 1 || d < TMAX * r.dir.x)? d / r.dir.x: TMAX;
if (tFrontMax < t)
{
tFrontMax = t;
ip.x = b.min.x;
ip.y = clamp (r.pos.y + t * r.dir.y, b.min.y, b.max.y);
ip.z = clamp (r.pos.z + t * r.dir.z, b.min.z, b.max.z);
}
}
}
else if (r.dir.x < 0)
{
if (r.pos.x < b.min.x)
return false;
T d = b.min.x - r.pos.x;
if (r.dir.x < -1 || d > TMAX * r.dir.x)
{
T t = d / r.dir.x;
if (tBackMin > t)
tBackMin = t;
}
if (r.pos.x >= b.max.x)
{
T d = b.max.x - r.pos.x;
T t = (r.dir.x < -1 || d > TMAX * r.dir.x)? d / r.dir.x: TMAX;
if (tFrontMax < t)
{
tFrontMax = t;
ip.x = b.max.x;
ip.y = clamp (r.pos.y + t * r.dir.y, b.min.y, b.max.y);
ip.z = clamp (r.pos.z + t * r.dir.z, b.min.z, b.max.z);
}
}
}
else
{
if (r.pos.x < b.min.x || r.pos.x > b.max.x)
return false;
}
if (r.dir.y > 0)
{
if (r.pos.y > b.max.y)
return false;
T d = b.max.y - r.pos.y;
if (r.dir.y > 1 || d < TMAX * r.dir.y)
{
T t = d / r.dir.y;
if (tBackMin > t)
tBackMin = t;
}
if (r.pos.y <= b.min.y)
{
T d = b.min.y - r.pos.y;
T t = (r.dir.y > 1 || d < TMAX * r.dir.y)? d / r.dir.y: TMAX;
if (tFrontMax < t)
{
tFrontMax = t;
ip.x = clamp (r.pos.x + t * r.dir.x, b.min.x, b.max.x);
ip.y = b.min.y;
ip.z = clamp (r.pos.z + t * r.dir.z, b.min.z, b.max.z);
}
}
}
else if (r.dir.y < 0)
{
if (r.pos.y < b.min.y)
return false;
T d = b.min.y - r.pos.y;
if (r.dir.y < -1 || d > TMAX * r.dir.y)
{
T t = d / r.dir.y;
if (tBackMin > t)
tBackMin = t;
}
if (r.pos.y >= b.max.y)
{
T d = b.max.y - r.pos.y;
T t = (r.dir.y < -1 || d > TMAX * r.dir.y)? d / r.dir.y: TMAX;
if (tFrontMax < t)
{
tFrontMax = t;
ip.x = clamp (r.pos.x + t * r.dir.x, b.min.x, b.max.x);
ip.y = b.max.y;
ip.z = clamp (r.pos.z + t * r.dir.z, b.min.z, b.max.z);
}
}
}
else
{
if (r.pos.y < b.min.y || r.pos.y > b.max.y)
return false;
}
if (r.dir.z > 0)
{
if (r.pos.z > b.max.z)
return false;
T d = b.max.z - r.pos.z;
if (r.dir.z > 1 || d < TMAX * r.dir.z)
{
T t = d / r.dir.z;
if (tBackMin > t)
tBackMin = t;
}
if (r.pos.z <= b.min.z)
{
T d = b.min.z - r.pos.z;
T t = (r.dir.z > 1 || d < TMAX * r.dir.z)? d / r.dir.z: TMAX;
if (tFrontMax < t)
{
tFrontMax = t;
ip.x = clamp (r.pos.x + t * r.dir.x, b.min.x, b.max.x);
ip.y = clamp (r.pos.y + t * r.dir.y, b.min.y, b.max.y);
ip.z = b.min.z;
}
}
}
else if (r.dir.z < 0)
{
if (r.pos.z < b.min.z)
return false;
T d = b.min.z - r.pos.z;
if (r.dir.z < -1 || d > TMAX * r.dir.z)
{
T t = d / r.dir.z;
if (tBackMin > t)
tBackMin = t;
}
if (r.pos.z >= b.max.z)
{
T d = b.max.z - r.pos.z;
T t = (r.dir.z < -1 || d > TMAX * r.dir.z)? d / r.dir.z: TMAX;
if (tFrontMax < t)
{
tFrontMax = t;
ip.x = clamp (r.pos.x + t * r.dir.x, b.min.x, b.max.x);
ip.y = clamp (r.pos.y + t * r.dir.y, b.min.y, b.max.y);
ip.z = b.max.z;
}
}
}
else
{
if (r.pos.z < b.min.z || r.pos.z > b.max.z)
return false;
}
return tFrontMax <= tBackMin;
}
template<class T>
bool
intersects (const Box< Vec3<T> > &box, const Line3<T> &ray)
{
Vec3<T> ignored;
return intersects (box, ray, ignored);
}
}
#endif