mirror of https://github.com/TTimo/GtkRadiant.git
463 lines
13 KiB
C
463 lines
13 KiB
C
/*
|
|
Copyright (C) 2001-2006, William Joseph.
|
|
All Rights Reserved.
|
|
|
|
This file is part of GtkRadiant.
|
|
|
|
GtkRadiant is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation; either version 2 of the License, or
|
|
(at your option) any later version.
|
|
|
|
GtkRadiant is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with GtkRadiant; if not, write to the Free Software
|
|
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
|
*/
|
|
|
|
#include <float.h>
|
|
|
|
#include "mathlib.h"
|
|
|
|
const aabb_t g_aabb_null = {
|
|
{ 0, 0, 0, },
|
|
{ -FLT_MAX, -FLT_MAX, -FLT_MAX, },
|
|
};
|
|
|
|
void aabb_construct_for_vec3(aabb_t *aabb, const vec3_t min, const vec3_t max)
|
|
{
|
|
VectorMid(min, max, aabb->origin);
|
|
VectorSubtract(max, aabb->origin, aabb->extents);
|
|
}
|
|
|
|
void aabb_clear(aabb_t *aabb)
|
|
{
|
|
VectorCopy(g_aabb_null.origin, aabb->origin);
|
|
VectorCopy(g_aabb_null.extents, aabb->extents);
|
|
}
|
|
|
|
void aabb_extend_by_point(aabb_t *aabb, const vec3_t point)
|
|
{
|
|
#if 1
|
|
int i;
|
|
vec_t min, max, displacement;
|
|
for(i=0; i<3; i++)
|
|
{
|
|
displacement = point[i] - aabb->origin[i];
|
|
if(fabs(displacement) > aabb->extents[i])
|
|
{
|
|
if(aabb->extents[i] < 0) // degenerate
|
|
{
|
|
min = max = point[i];
|
|
}
|
|
else if(displacement > 0)
|
|
{
|
|
min = aabb->origin[i] - aabb->extents[i];
|
|
max = aabb->origin[i] + displacement;
|
|
}
|
|
else
|
|
{
|
|
max = aabb->origin[i] + aabb->extents[i];
|
|
min = aabb->origin[i] + displacement;
|
|
}
|
|
aabb->origin[i] = (min + max) * 0.5f;
|
|
aabb->extents[i] = max - aabb->origin[i];
|
|
}
|
|
}
|
|
#else
|
|
unsigned int i;
|
|
for(i=0; i<3; ++i)
|
|
{
|
|
if(aabb->extents[i] < 0) // degenerate
|
|
{
|
|
aabb->origin[i] = point[i];
|
|
aabb->extents[i] = 0;
|
|
}
|
|
else
|
|
{
|
|
vec_t displacement = point[i] - aabb->origin[i];
|
|
vec_t increment = (vec_t)fabs(displacement) - aabb->extents[i];
|
|
if(increment > 0)
|
|
{
|
|
increment *= (vec_t)((displacement > 0) ? 0.5 : -0.5);
|
|
aabb->extents[i] += increment;
|
|
aabb->origin[i] += increment;
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void aabb_extend_by_aabb(aabb_t *aabb, const aabb_t *aabb_src)
|
|
{
|
|
int i;
|
|
vec_t min, max, displacement, difference;
|
|
for(i=0; i<3; i++)
|
|
{
|
|
displacement = aabb_src->origin[i] - aabb->origin[i];
|
|
difference = aabb_src->extents[i] - aabb->extents[i];
|
|
if(aabb->extents[i] < 0
|
|
|| difference >= fabs(displacement))
|
|
{
|
|
// 2nd contains 1st
|
|
aabb->extents[i] = aabb_src->extents[i];
|
|
aabb->origin[i] = aabb_src->origin[i];
|
|
}
|
|
else if(aabb_src->extents[i] < 0
|
|
|| -difference >= fabs(displacement))
|
|
{
|
|
// 1st contains 2nd
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
// not contained
|
|
if(displacement > 0)
|
|
{
|
|
min = aabb->origin[i] - aabb->extents[i];
|
|
max = aabb_src->origin[i] + aabb_src->extents[i];
|
|
}
|
|
else
|
|
{
|
|
min = aabb_src->origin[i] - aabb_src->extents[i];
|
|
max = aabb->origin[i] + aabb->extents[i];
|
|
}
|
|
aabb->origin[i] = (min + max) * 0.5f;
|
|
aabb->extents[i] = max - aabb->origin[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
void aabb_extend_by_vec3(aabb_t *aabb, vec3_t extension)
|
|
{
|
|
VectorAdd(aabb->extents, extension, aabb->extents);
|
|
}
|
|
|
|
int aabb_test_point(const aabb_t *aabb, const vec3_t point)
|
|
{
|
|
int i;
|
|
for(i=0; i<3; i++)
|
|
if(fabs(point[i] - aabb->origin[i]) >= aabb->extents[i])
|
|
return 0;
|
|
return 1;
|
|
}
|
|
|
|
int aabb_test_aabb(const aabb_t *aabb, const aabb_t *aabb_src)
|
|
{
|
|
int i;
|
|
for (i=0; i<3; i++)
|
|
if ( fabs(aabb_src->origin[i] - aabb->origin[i]) > (fabs(aabb->extents[i]) + fabs(aabb_src->extents[i])) )
|
|
return 0;
|
|
return 1;
|
|
}
|
|
|
|
int aabb_test_plane(const aabb_t *aabb, const float *plane)
|
|
{
|
|
float fDist, fIntersect;
|
|
|
|
// calc distance of origin from plane
|
|
fDist = DotProduct(plane, aabb->origin) + plane[3];
|
|
|
|
// calc extents distance relative to plane normal
|
|
fIntersect = (vec_t)(fabs(plane[0] * aabb->extents[0]) + fabs(plane[1] * aabb->extents[1]) + fabs(plane[2] * aabb->extents[2]));
|
|
// accept if origin is less than or equal to this distance
|
|
if (fabs(fDist) < fIntersect) return 1; // partially inside
|
|
else if (fDist < 0) return 2; // totally inside
|
|
return 0; // totally outside
|
|
}
|
|
|
|
/*
|
|
Fast Ray-Box Intersection
|
|
by Andrew Woo
|
|
from "Graphics Gems", Academic Press, 1990
|
|
*/
|
|
|
|
#define NUMDIM 3
|
|
#define RIGHT 0
|
|
#define LEFT 1
|
|
#define MIDDLE 2
|
|
|
|
int aabb_intersect_ray(const aabb_t *aabb, const ray_t *ray, vec3_t intersection)
|
|
{
|
|
int inside = 1;
|
|
char quadrant[NUMDIM];
|
|
register int i;
|
|
int whichPlane;
|
|
double maxT[NUMDIM];
|
|
double candidatePlane[NUMDIM];
|
|
|
|
const float *origin = ray->origin;
|
|
const float *direction = ray->direction;
|
|
|
|
/* Find candidate planes; this loop can be avoided if
|
|
rays cast all from the eye(assume perpsective view) */
|
|
for (i=0; i<NUMDIM; i++)
|
|
{
|
|
if(origin[i] < (aabb->origin[i] - aabb->extents[i]))
|
|
{
|
|
quadrant[i] = LEFT;
|
|
candidatePlane[i] = (aabb->origin[i] - aabb->extents[i]);
|
|
inside = 0;
|
|
}
|
|
else if (origin[i] > (aabb->origin[i] + aabb->extents[i]))
|
|
{
|
|
quadrant[i] = RIGHT;
|
|
candidatePlane[i] = (aabb->origin[i] + aabb->extents[i]);
|
|
inside = 0;
|
|
}
|
|
else
|
|
{
|
|
quadrant[i] = MIDDLE;
|
|
}
|
|
}
|
|
|
|
/* Ray origin inside bounding box */
|
|
if(inside == 1)
|
|
{
|
|
VectorCopy(ray->origin, intersection);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/* Calculate T distances to candidate planes */
|
|
for (i = 0; i < NUMDIM; i++)
|
|
{
|
|
if (quadrant[i] != MIDDLE && direction[i] !=0.)
|
|
maxT[i] = (candidatePlane[i] - origin[i]) / direction[i];
|
|
else
|
|
maxT[i] = -1.;
|
|
}
|
|
|
|
/* Get largest of the maxT's for final choice of intersection */
|
|
whichPlane = 0;
|
|
for (i = 1; i < NUMDIM; i++)
|
|
if (maxT[whichPlane] < maxT[i])
|
|
whichPlane = i;
|
|
|
|
/* Check final candidate actually inside box */
|
|
if (maxT[whichPlane] < 0.)
|
|
return 0;
|
|
for (i = 0; i < NUMDIM; i++)
|
|
{
|
|
if (whichPlane != i)
|
|
{
|
|
intersection[i] = (vec_t)(origin[i] + maxT[whichPlane] * direction[i]);
|
|
if (fabs(intersection[i] - aabb->origin[i]) > aabb->extents[i])
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
intersection[i] = (vec_t)candidatePlane[i];
|
|
}
|
|
}
|
|
|
|
return 1; /* ray hits box */
|
|
}
|
|
|
|
int aabb_test_ray(const aabb_t* aabb, const ray_t* ray)
|
|
{
|
|
vec3_t displacement, ray_absolute;
|
|
vec_t f;
|
|
|
|
displacement[0] = ray->origin[0] - aabb->origin[0];
|
|
if(fabs(displacement[0]) > aabb->extents[0] && displacement[0] * ray->direction[0] >= 0.0f)
|
|
return 0;
|
|
|
|
displacement[1] = ray->origin[1] - aabb->origin[1];
|
|
if(fabs(displacement[1]) > aabb->extents[1] && displacement[1] * ray->direction[1] >= 0.0f)
|
|
return 0;
|
|
|
|
displacement[2] = ray->origin[2] - aabb->origin[2];
|
|
if(fabs(displacement[2]) > aabb->extents[2] && displacement[2] * ray->direction[2] >= 0.0f)
|
|
return 0;
|
|
|
|
ray_absolute[0] = (float)fabs(ray->direction[0]);
|
|
ray_absolute[1] = (float)fabs(ray->direction[1]);
|
|
ray_absolute[2] = (float)fabs(ray->direction[2]);
|
|
|
|
f = ray->direction[1] * displacement[2] - ray->direction[2] * displacement[1];
|
|
if((float)fabs(f) > aabb->extents[1] * ray_absolute[2] + aabb->extents[2] * ray_absolute[1])
|
|
return 0;
|
|
|
|
f = ray->direction[2] * displacement[0] - ray->direction[0] * displacement[2];
|
|
if((float)fabs(f) > aabb->extents[0] * ray_absolute[2] + aabb->extents[2] * ray_absolute[0])
|
|
return 0;
|
|
|
|
f = ray->direction[0] * displacement[1] - ray->direction[1] * displacement[0];
|
|
if((float)fabs(f) > aabb->extents[0] * ray_absolute[1] + aabb->extents[1] * ray_absolute[0])
|
|
return 0;
|
|
|
|
return 1;
|
|
}
|
|
|
|
void aabb_orthogonal_transform(aabb_t* dst, const aabb_t* src, const m4x4_t transform)
|
|
{
|
|
VectorCopy(src->origin, dst->origin);
|
|
m4x4_transform_point(transform, dst->origin);
|
|
|
|
dst->extents[0] = (vec_t)(fabs(transform[0] * src->extents[0])
|
|
+ fabs(transform[4] * src->extents[1])
|
|
+ fabs(transform[8] * src->extents[2]));
|
|
dst->extents[1] = (vec_t)(fabs(transform[1] * src->extents[0])
|
|
+ fabs(transform[5] * src->extents[1])
|
|
+ fabs(transform[9] * src->extents[2]));
|
|
dst->extents[2] = (vec_t)(fabs(transform[2] * src->extents[0])
|
|
+ fabs(transform[6] * src->extents[1])
|
|
+ fabs(transform[10] * src->extents[2]));
|
|
}
|
|
|
|
void aabb_for_bbox(aabb_t *aabb, const bbox_t *bbox)
|
|
{
|
|
int i;
|
|
vec3_t temp[3];
|
|
|
|
VectorCopy(bbox->aabb.origin, aabb->origin);
|
|
|
|
// calculate the AABB extents in local coord space from the OBB extents and axes
|
|
VectorScale(bbox->axes[0], bbox->aabb.extents[0], temp[0]);
|
|
VectorScale(bbox->axes[1], bbox->aabb.extents[1], temp[1]);
|
|
VectorScale(bbox->axes[2], bbox->aabb.extents[2], temp[2]);
|
|
for(i=0;i<3;i++) aabb->extents[i] = (vec_t)(fabs(temp[0][i]) + fabs(temp[1][i]) + fabs(temp[2][i]));
|
|
}
|
|
|
|
void aabb_for_area(aabb_t *aabb, vec3_t area_tl, vec3_t area_br, int axis)
|
|
{
|
|
aabb_clear(aabb);
|
|
aabb->extents[axis] = FLT_MAX;
|
|
aabb_extend_by_point(aabb, area_tl);
|
|
aabb_extend_by_point(aabb, area_br);
|
|
}
|
|
|
|
int aabb_oriented_intersect_plane(const aabb_t *aabb, const m4x4_t transform, const vec_t* plane)
|
|
{
|
|
vec_t fDist, fIntersect;
|
|
|
|
// calc distance of origin from plane
|
|
fDist = DotProduct(plane, aabb->origin) + plane[3];
|
|
|
|
// calc extents distance relative to plane normal
|
|
fIntersect = (vec_t)(fabs(aabb->extents[0] * DotProduct(plane, transform))
|
|
+ fabs(aabb->extents[1] * DotProduct(plane, transform+4))
|
|
+ fabs(aabb->extents[2] * DotProduct(plane, transform+8)));
|
|
// accept if origin is less than this distance
|
|
if (fabs(fDist) < fIntersect) return 1; // partially inside
|
|
else if (fDist < 0) return 2; // totally inside
|
|
return 0; // totally outside
|
|
}
|
|
|
|
void aabb_corners(const aabb_t* aabb, vec3_t corners[8])
|
|
{
|
|
vec3_t min, max;
|
|
VectorSubtract(aabb->origin, aabb->extents, min);
|
|
VectorAdd(aabb->origin, aabb->extents, max);
|
|
VectorSet(corners[0], min[0], max[1], max[2]);
|
|
VectorSet(corners[1], max[0], max[1], max[2]);
|
|
VectorSet(corners[2], max[0], min[1], max[2]);
|
|
VectorSet(corners[3], min[0], min[1], max[2]);
|
|
VectorSet(corners[4], min[0], max[1], min[2]);
|
|
VectorSet(corners[5], max[0], max[1], min[2]);
|
|
VectorSet(corners[6], max[0], min[1], min[2]);
|
|
VectorSet(corners[7], min[0], min[1], min[2]);
|
|
}
|
|
|
|
|
|
void bbox_update_radius(bbox_t *bbox)
|
|
{
|
|
bbox->radius = VectorLength(bbox->aabb.extents);
|
|
}
|
|
|
|
void aabb_for_transformed_aabb(aabb_t* dst, const aabb_t* src, const m4x4_t transform)
|
|
{
|
|
if(src->extents[0] < 0
|
|
|| src->extents[1] < 0
|
|
|| src->extents[2] < 0)
|
|
{
|
|
aabb_clear(dst);
|
|
return;
|
|
}
|
|
|
|
VectorCopy(src->origin, dst->origin);
|
|
m4x4_transform_point(transform, dst->origin);
|
|
|
|
dst->extents[0] = (vec_t)(fabs(transform[0] * src->extents[0])
|
|
+ fabs(transform[4] * src->extents[1])
|
|
+ fabs(transform[8] * src->extents[2]));
|
|
dst->extents[1] = (vec_t)(fabs(transform[1] * src->extents[0])
|
|
+ fabs(transform[5] * src->extents[1])
|
|
+ fabs(transform[9] * src->extents[2]));
|
|
dst->extents[2] = (vec_t)(fabs(transform[2] * src->extents[0])
|
|
+ fabs(transform[6] * src->extents[1])
|
|
+ fabs(transform[10] * src->extents[2]));
|
|
}
|
|
|
|
void bbox_for_oriented_aabb(bbox_t *bbox, const aabb_t *aabb, const m4x4_t matrix, const vec3_t euler, const vec3_t scale)
|
|
{
|
|
double rad[3];
|
|
double pi_180 = Q_PI / 180;
|
|
double A, B, C, D, E, F, AD, BD;
|
|
|
|
VectorCopy(aabb->origin, bbox->aabb.origin);
|
|
|
|
m4x4_transform_point(matrix, bbox->aabb.origin);
|
|
|
|
bbox->aabb.extents[0] = aabb->extents[0] * scale[0];
|
|
bbox->aabb.extents[1] = aabb->extents[1] * scale[1];
|
|
bbox->aabb.extents[2] = aabb->extents[2] * scale[2];
|
|
|
|
rad[0] = euler[0] * pi_180;
|
|
rad[1] = euler[1] * pi_180;
|
|
rad[2] = euler[2] * pi_180;
|
|
|
|
A = cos(rad[0]);
|
|
B = sin(rad[0]);
|
|
C = cos(rad[1]);
|
|
D = sin(rad[1]);
|
|
E = cos(rad[2]);
|
|
F = sin(rad[2]);
|
|
|
|
AD = A * -D;
|
|
BD = B * -D;
|
|
|
|
bbox->axes[0][0] = (vec_t)(C*E);
|
|
bbox->axes[0][1] = (vec_t)(-BD*E + A*F);
|
|
bbox->axes[0][2] = (vec_t)(AD*E + B*F);
|
|
bbox->axes[1][0] = (vec_t)(-C*F);
|
|
bbox->axes[1][1] = (vec_t)(BD*F + A*E);
|
|
bbox->axes[1][2] = (vec_t)(-AD*F + B*E);
|
|
bbox->axes[2][0] = (vec_t)D;
|
|
bbox->axes[2][1] = (vec_t)(-B*C);
|
|
bbox->axes[2][2] = (vec_t)(A*C);
|
|
|
|
bbox_update_radius(bbox);
|
|
}
|
|
|
|
int bbox_intersect_plane(const bbox_t *bbox, const vec_t* plane)
|
|
{
|
|
vec_t fDist, fIntersect;
|
|
|
|
// calc distance of origin from plane
|
|
fDist = DotProduct(plane, bbox->aabb.origin) + plane[3];
|
|
|
|
// trivial accept/reject using bounding sphere
|
|
if (fabs(fDist) > bbox->radius)
|
|
{
|
|
if (fDist < 0)
|
|
return 2; // totally inside
|
|
else
|
|
return 0; // totally outside
|
|
}
|
|
|
|
// calc extents distance relative to plane normal
|
|
fIntersect = (vec_t)(fabs(bbox->aabb.extents[0] * DotProduct(plane, bbox->axes[0]))
|
|
+ fabs(bbox->aabb.extents[1] * DotProduct(plane, bbox->axes[1]))
|
|
+ fabs(bbox->aabb.extents[2] * DotProduct(plane, bbox->axes[2])));
|
|
// accept if origin is less than this distance
|
|
if (fabs(fDist) < fIntersect) return 1; // partially inside
|
|
else if (fDist < 0) return 2; // totally inside
|
|
return 0; // totally outside
|
|
}
|