547 lines
13 KiB
C++
547 lines
13 KiB
C++
#ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
|
|
#define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
|
|
|
|
/*! \file gim_basic_geometry_operations.h
|
|
*\author Francisco Leon Najera
|
|
type independant geometry routines
|
|
|
|
*/
|
|
/*
|
|
-----------------------------------------------------------------------------
|
|
This source file is part of GIMPACT Library.
|
|
|
|
For the latest info, see http://gimpact.sourceforge.net/
|
|
|
|
Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
|
|
email: projectileman@yahoo.com
|
|
|
|
This library is free software; you can redistribute it and/or
|
|
modify it under the terms of EITHER:
|
|
(1) The GNU Lesser General Public License as published by the Free
|
|
Software Foundation; either version 2.1 of the License, or (at
|
|
your option) any later version. The text of the GNU Lesser
|
|
General Public License is included with this library in the
|
|
file GIMPACT-LICENSE-LGPL.TXT.
|
|
(2) The BSD-style license that is included with this library in
|
|
the file GIMPACT-LICENSE-BSD.TXT.
|
|
(3) The zlib/libpng license that is included with this library in
|
|
the file GIMPACT-LICENSE-ZLIB.TXT.
|
|
|
|
This library 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 files
|
|
GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
|
|
|
|
-----------------------------------------------------------------------------
|
|
*/
|
|
|
|
|
|
#include "gim_linear_math.h"
|
|
|
|
|
|
|
|
|
|
#ifndef PLANEDIREPSILON
|
|
#define PLANEDIREPSILON 0.0000001f
|
|
#endif
|
|
|
|
#ifndef PARALELENORMALS
|
|
#define PARALELENORMALS 0.000001f
|
|
#endif
|
|
|
|
#define TRIANGLE_NORMAL(v1,v2,v3,n)\
|
|
{\
|
|
vec3f _dif1,_dif2;\
|
|
VEC_DIFF(_dif1,v2,v1);\
|
|
VEC_DIFF(_dif2,v3,v1);\
|
|
VEC_CROSS(n,_dif1,_dif2);\
|
|
VEC_NORMALIZE(n);\
|
|
}\
|
|
|
|
#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
|
|
vec3f _dif1,_dif2; \
|
|
VEC_DIFF(_dif1,v2,v1); \
|
|
VEC_DIFF(_dif2,v3,v1); \
|
|
VEC_CROSS(n,_dif1,_dif2); \
|
|
}\
|
|
|
|
/// plane is a vec4f
|
|
#define TRIANGLE_PLANE(v1,v2,v3,plane) {\
|
|
TRIANGLE_NORMAL(v1,v2,v3,plane);\
|
|
plane[3] = VEC_DOT(v1,plane);\
|
|
}\
|
|
|
|
/// plane is a vec4f
|
|
#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
|
|
TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
|
|
plane[3] = VEC_DOT(v1,plane);\
|
|
}\
|
|
|
|
/// Calc a plane from an edge an a normal. plane is a vec4f
|
|
#define EDGE_PLANE(e1,e2,n,plane) {\
|
|
vec3f _dif; \
|
|
VEC_DIFF(_dif,e2,e1); \
|
|
VEC_CROSS(plane,_dif,n); \
|
|
VEC_NORMALIZE(plane); \
|
|
plane[3] = VEC_DOT(e1,plane);\
|
|
}\
|
|
|
|
#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])
|
|
|
|
#define PROJECT_POINT_PLANE(point,plane,projected) {\
|
|
GREAL _dis;\
|
|
_dis = DISTANCE_PLANE_POINT(plane,point);\
|
|
VEC_SCALE(projected,-_dis,plane);\
|
|
VEC_SUM(projected,projected,point); \
|
|
}\
|
|
|
|
//! Verifies if a point is in the plane hull
|
|
template<typename CLASS_POINT,typename CLASS_PLANE>
|
|
SIMD_FORCE_INLINE bool POINT_IN_HULL(
|
|
const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
|
|
{
|
|
GREAL _dis;
|
|
for (GUINT _i = 0;_i< plane_count;++_i)
|
|
{
|
|
_dis = DISTANCE_PLANE_POINT(planes[_i],point);
|
|
if(_dis>0.0f) return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
template<typename CLASS_POINT,typename CLASS_PLANE>
|
|
SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
|
|
const CLASS_POINT& s1,
|
|
const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
|
|
{
|
|
GREAL _dis1,_dis2;
|
|
_dis1 = DISTANCE_PLANE_POINT(plane,s1);
|
|
VEC_DIFF(clipped,s2,s1);
|
|
_dis2 = VEC_DOT(clipped,plane);
|
|
VEC_SCALE(clipped,-_dis1/_dis2,clipped);
|
|
VEC_SUM(clipped,clipped,s1);
|
|
}
|
|
|
|
enum ePLANE_INTERSECTION_TYPE
|
|
{
|
|
G_BACK_PLANE = 0,
|
|
G_COLLIDE_PLANE,
|
|
G_FRONT_PLANE
|
|
};
|
|
|
|
enum eLINE_PLANE_INTERSECTION_TYPE
|
|
{
|
|
G_FRONT_PLANE_S1 = 0,
|
|
G_FRONT_PLANE_S2,
|
|
G_BACK_PLANE_S1,
|
|
G_BACK_PLANE_S2,
|
|
G_COLLIDE_PLANE_S1,
|
|
G_COLLIDE_PLANE_S2
|
|
};
|
|
|
|
//! Confirms if the plane intersect the edge or nor
|
|
/*!
|
|
intersection type must have the following values
|
|
<ul>
|
|
<li> 0 : Segment in front of plane, s1 closest
|
|
<li> 1 : Segment in front of plane, s2 closest
|
|
<li> 2 : Segment in back of plane, s1 closest
|
|
<li> 3 : Segment in back of plane, s2 closest
|
|
<li> 4 : Segment collides plane, s1 in back
|
|
<li> 5 : Segment collides plane, s2 in back
|
|
</ul>
|
|
*/
|
|
|
|
template<typename CLASS_POINT,typename CLASS_PLANE>
|
|
SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
|
|
const CLASS_POINT& s1,
|
|
const CLASS_POINT &s2,
|
|
const CLASS_PLANE &plane,CLASS_POINT &clipped)
|
|
{
|
|
GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
|
|
GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
|
|
if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
|
|
{
|
|
if(_dis1<_dis2) return G_FRONT_PLANE_S1;
|
|
return G_FRONT_PLANE_S2;
|
|
}
|
|
else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
|
|
{
|
|
if(_dis1>_dis2) return G_BACK_PLANE_S1;
|
|
return G_BACK_PLANE_S2;
|
|
}
|
|
|
|
VEC_DIFF(clipped,s2,s1);
|
|
_dis2 = VEC_DOT(clipped,plane);
|
|
VEC_SCALE(clipped,-_dis1/_dis2,clipped);
|
|
VEC_SUM(clipped,clipped,s1);
|
|
if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
|
|
return G_COLLIDE_PLANE_S2;
|
|
}
|
|
|
|
//! Confirms if the plane intersect the edge or not
|
|
/*!
|
|
clipped1 and clipped2 are the vertices behind the plane.
|
|
clipped1 is the closest
|
|
|
|
intersection_type must have the following values
|
|
<ul>
|
|
<li> 0 : Segment in front of plane, s1 closest
|
|
<li> 1 : Segment in front of plane, s2 closest
|
|
<li> 2 : Segment in back of plane, s1 closest
|
|
<li> 3 : Segment in back of plane, s2 closest
|
|
<li> 4 : Segment collides plane, s1 in back
|
|
<li> 5 : Segment collides plane, s2 in back
|
|
</ul>
|
|
*/
|
|
template<typename CLASS_POINT,typename CLASS_PLANE>
|
|
SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
|
|
const CLASS_POINT& s1,
|
|
const CLASS_POINT &s2,
|
|
const CLASS_PLANE &plane,
|
|
CLASS_POINT &clipped1,CLASS_POINT &clipped2)
|
|
{
|
|
eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
|
|
switch(intersection_type)
|
|
{
|
|
case G_FRONT_PLANE_S1:
|
|
VEC_COPY(clipped1,s1);
|
|
VEC_COPY(clipped2,s2);
|
|
break;
|
|
case G_FRONT_PLANE_S2:
|
|
VEC_COPY(clipped1,s2);
|
|
VEC_COPY(clipped2,s1);
|
|
break;
|
|
case G_BACK_PLANE_S1:
|
|
VEC_COPY(clipped1,s1);
|
|
VEC_COPY(clipped2,s2);
|
|
break;
|
|
case G_BACK_PLANE_S2:
|
|
VEC_COPY(clipped1,s2);
|
|
VEC_COPY(clipped2,s1);
|
|
break;
|
|
case G_COLLIDE_PLANE_S1:
|
|
VEC_COPY(clipped2,s1);
|
|
break;
|
|
case G_COLLIDE_PLANE_S2:
|
|
VEC_COPY(clipped2,s2);
|
|
break;
|
|
}
|
|
return intersection_type;
|
|
}
|
|
|
|
|
|
//! Finds the 2 smallest cartesian coordinates of a plane normal
|
|
#define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
|
|
|
|
//! Ray plane collision in one way
|
|
/*!
|
|
Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
|
|
It uses the PLANEDIREPSILON constant.
|
|
*/
|
|
template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
|
|
SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
|
|
const CLASS_PLANE & plane,
|
|
const CLASS_POINT & vDir,
|
|
const CLASS_POINT & vPoint,
|
|
CLASS_POINT & pout,T &tparam)
|
|
{
|
|
GREAL _dis,_dotdir;
|
|
_dotdir = VEC_DOT(plane,vDir);
|
|
if(_dotdir<PLANEDIREPSILON)
|
|
{
|
|
return false;
|
|
}
|
|
_dis = DISTANCE_PLANE_POINT(plane,vPoint);
|
|
tparam = -_dis/_dotdir;
|
|
VEC_SCALE(pout,tparam,vDir);
|
|
VEC_SUM(pout,vPoint,pout);
|
|
return true;
|
|
}
|
|
|
|
//! line collision
|
|
/*!
|
|
*\return
|
|
-0 if the ray never intersects
|
|
-1 if the ray collides in front
|
|
-2 if the ray collides in back
|
|
*/
|
|
template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
|
|
SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
|
|
const CLASS_PLANE & plane,
|
|
const CLASS_POINT & vDir,
|
|
const CLASS_POINT & vPoint,
|
|
CLASS_POINT & pout,
|
|
T &tparam,
|
|
T tmin, T tmax)
|
|
{
|
|
GREAL _dis,_dotdir;
|
|
_dotdir = VEC_DOT(plane,vDir);
|
|
if(btFabs(_dotdir)<PLANEDIREPSILON)
|
|
{
|
|
tparam = tmax;
|
|
return 0;
|
|
}
|
|
_dis = DISTANCE_PLANE_POINT(plane,vPoint);
|
|
char returnvalue = _dis<0.0f?2:1;
|
|
tparam = -_dis/_dotdir;
|
|
|
|
if(tparam<tmin)
|
|
{
|
|
returnvalue = 0;
|
|
tparam = tmin;
|
|
}
|
|
else if(tparam>tmax)
|
|
{
|
|
returnvalue = 0;
|
|
tparam = tmax;
|
|
}
|
|
|
|
VEC_SCALE(pout,tparam,vDir);
|
|
VEC_SUM(pout,vPoint,pout);
|
|
return returnvalue;
|
|
}
|
|
|
|
/*! \brief Returns the Ray on which 2 planes intersect if they do.
|
|
Written by Rodrigo Hernandez on ODE convex collision
|
|
|
|
\param p1 Plane 1
|
|
\param p2 Plane 2
|
|
\param p Contains the origin of the ray upon returning if planes intersect
|
|
\param d Contains the direction of the ray upon returning if planes intersect
|
|
\return true if the planes intersect, 0 if paralell.
|
|
|
|
*/
|
|
template<typename CLASS_POINT,typename CLASS_PLANE>
|
|
SIMD_FORCE_INLINE bool INTERSECT_PLANES(
|
|
const CLASS_PLANE &p1,
|
|
const CLASS_PLANE &p2,
|
|
CLASS_POINT &p,
|
|
CLASS_POINT &d)
|
|
{
|
|
VEC_CROSS(d,p1,p2);
|
|
GREAL denom = VEC_DOT(d, d);
|
|
if(GIM_IS_ZERO(denom)) return false;
|
|
vec3f _n;
|
|
_n[0]=p1[3]*p2[0] - p2[3]*p1[0];
|
|
_n[1]=p1[3]*p2[1] - p2[3]*p1[1];
|
|
_n[2]=p1[3]*p2[2] - p2[3]*p1[2];
|
|
VEC_CROSS(p,_n,d);
|
|
p[0]/=denom;
|
|
p[1]/=denom;
|
|
p[2]/=denom;
|
|
return true;
|
|
}
|
|
|
|
//***************** SEGMENT and LINE FUNCTIONS **********************************///
|
|
|
|
/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
|
|
*/
|
|
template<typename CLASS_POINT>
|
|
SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
|
|
CLASS_POINT & cp, const CLASS_POINT & v,
|
|
const CLASS_POINT &e1,const CLASS_POINT &e2)
|
|
{
|
|
vec3f _n;
|
|
VEC_DIFF(_n,e2,e1);
|
|
VEC_DIFF(cp,v,e1);
|
|
GREAL _scalar = VEC_DOT(cp, _n);
|
|
_scalar/= VEC_DOT(_n, _n);
|
|
if(_scalar <0.0f)
|
|
{
|
|
VEC_COPY(cp,e1);
|
|
}
|
|
else if(_scalar >1.0f)
|
|
{
|
|
VEC_COPY(cp,e2);
|
|
}
|
|
else
|
|
{
|
|
VEC_SCALE(cp,_scalar,_n);
|
|
VEC_SUM(cp,cp,e1);
|
|
}
|
|
}
|
|
|
|
|
|
/*! \brief Finds the line params where these lines intersect.
|
|
|
|
\param dir1 Direction of line 1
|
|
\param point1 Point of line 1
|
|
\param dir2 Direction of line 2
|
|
\param point2 Point of line 2
|
|
\param t1 Result Parameter for line 1
|
|
\param t2 Result Parameter for line 2
|
|
\param dointersect 0 if the lines won't intersect, else 1
|
|
|
|
*/
|
|
template<typename T,typename CLASS_POINT>
|
|
SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
|
|
const CLASS_POINT & dir1,
|
|
CLASS_POINT & point1,
|
|
const CLASS_POINT & dir2,
|
|
CLASS_POINT & point2,
|
|
T& t1,T& t2)
|
|
{
|
|
GREAL det;
|
|
GREAL e1e1 = VEC_DOT(dir1,dir1);
|
|
GREAL e1e2 = VEC_DOT(dir1,dir2);
|
|
GREAL e2e2 = VEC_DOT(dir2,dir2);
|
|
vec3f p1p2;
|
|
VEC_DIFF(p1p2,point1,point2);
|
|
GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
|
|
GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
|
|
det = e1e2*e1e2 - e1e1*e2e2;
|
|
if(GIM_IS_ZERO(det)) return false;
|
|
t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
|
|
t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
|
|
return true;
|
|
}
|
|
|
|
//! Find closest points on segments
|
|
template<typename CLASS_POINT>
|
|
SIMD_FORCE_INLINE void SEGMENT_COLLISION(
|
|
const CLASS_POINT & vA1,
|
|
const CLASS_POINT & vA2,
|
|
const CLASS_POINT & vB1,
|
|
const CLASS_POINT & vB2,
|
|
CLASS_POINT & vPointA,
|
|
CLASS_POINT & vPointB)
|
|
{
|
|
CLASS_POINT _AD,_BD,n;
|
|
vec4f _M;//plane
|
|
VEC_DIFF(_AD,vA2,vA1);
|
|
VEC_DIFF(_BD,vB2,vB1);
|
|
VEC_CROSS(n,_AD,_BD);
|
|
GREAL _tp = VEC_DOT(n,n);
|
|
if(_tp<G_EPSILON)//ARE PARALELE
|
|
{
|
|
//project B over A
|
|
bool invert_b_order = false;
|
|
_M[0] = VEC_DOT(vB1,_AD);
|
|
_M[1] = VEC_DOT(vB2,_AD);
|
|
if(_M[0]>_M[1])
|
|
{
|
|
invert_b_order = true;
|
|
GIM_SWAP_NUMBERS(_M[0],_M[1]);
|
|
}
|
|
_M[2] = VEC_DOT(vA1,_AD);
|
|
_M[3] = VEC_DOT(vA2,_AD);
|
|
//mid points
|
|
n[0] = (_M[0]+_M[1])*0.5f;
|
|
n[1] = (_M[2]+_M[3])*0.5f;
|
|
|
|
if(n[0]<n[1])
|
|
{
|
|
if(_M[1]<_M[2])
|
|
{
|
|
vPointB = invert_b_order?vB1:vB2;
|
|
vPointA = vA1;
|
|
}
|
|
else if(_M[1]<_M[3])
|
|
{
|
|
vPointB = invert_b_order?vB1:vB2;
|
|
CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
|
|
}
|
|
else
|
|
{
|
|
vPointA = vA2;
|
|
CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(_M[3]<_M[0])
|
|
{
|
|
vPointB = invert_b_order?vB2:vB1;
|
|
vPointA = vA2;
|
|
}
|
|
else if(_M[3]<_M[1])
|
|
{
|
|
vPointA = vA2;
|
|
CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
|
|
}
|
|
else
|
|
{
|
|
vPointB = invert_b_order?vB1:vB2;
|
|
CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
|
|
|
|
VEC_CROSS(_M,n,_BD);
|
|
_M[3] = VEC_DOT(_M,vB1);
|
|
|
|
LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
|
|
/*Closest point on segment*/
|
|
VEC_DIFF(vPointB,vPointA,vB1);
|
|
_tp = VEC_DOT(vPointB, _BD);
|
|
_tp/= VEC_DOT(_BD, _BD);
|
|
_tp = GIM_CLAMP(_tp,0.0f,1.0f);
|
|
VEC_SCALE(vPointB,_tp,_BD);
|
|
VEC_SUM(vPointB,vPointB,vB1);
|
|
}
|
|
|
|
|
|
|
|
|
|
//! Line box intersection in one dimension
|
|
/*!
|
|
|
|
*\param pos Position of the ray
|
|
*\param dir Projection of the Direction of the ray
|
|
*\param bmin Minimum bound of the box
|
|
*\param bmax Maximum bound of the box
|
|
*\param tfirst the minimum projection. Assign to 0 at first.
|
|
*\param tlast the maximum projection. Assign to INFINITY at first.
|
|
*\return true if there is an intersection.
|
|
*/
|
|
template<typename T>
|
|
SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
|
|
{
|
|
if(GIM_IS_ZERO(dir))
|
|
{
|
|
return !(pos < bmin || pos > bmax);
|
|
}
|
|
GREAL a0 = (bmin - pos) / dir;
|
|
GREAL a1 = (bmax - pos) / dir;
|
|
if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1);
|
|
tfirst = GIM_MAX(a0, tfirst);
|
|
tlast = GIM_MIN(a1, tlast);
|
|
if (tlast < tfirst) return false;
|
|
return true;
|
|
}
|
|
|
|
|
|
//! Sorts 3 componets
|
|
template<typename T>
|
|
SIMD_FORCE_INLINE void SORT_3_INDICES(
|
|
const T * values,
|
|
GUINT * order_indices)
|
|
{
|
|
//get minimum
|
|
order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
|
|
|
|
//get second and third
|
|
GUINT i0 = (order_indices[0] + 1)%3;
|
|
GUINT i1 = (i0 + 1)%3;
|
|
|
|
if(values[i0] < values[i1])
|
|
{
|
|
order_indices[1] = i0;
|
|
order_indices[2] = i1;
|
|
}
|
|
else
|
|
{
|
|
order_indices[1] = i1;
|
|
order_indices[2] = i0;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
#endif // GIM_VECTOR_H_INCLUDED
|