gim_geometry.h

ODE v0.8 很好用的多平台几何物理模拟库源代码,内含多個示例

    (destaabb).minY = MIN((aabb).minY,(destaabb).minY);\
    (destaabb).minZ = MIN((aabb).minZ,(destaabb).minZ);\
    (destaabb).maxX = MAX((aabb).maxX,(destaabb).maxX);\
    (destaabb).maxY = MAX((aabb).maxY,(destaabb).maxY);\
    (destaabb).maxZ = MAX((aabb).maxZ,(destaabb).maxZ);\
}\

//! Extends the box
#define AABB_POINT_EXTEND(destaabb,p) {\
    (destaabb).minX = MIN(p[0],(destaabb).minX);\
    (destaabb).maxX = MAX(p[0],(destaabb).maxX);\
    (destaabb).minY = MIN(p[1],(destaabb).minY);\
    (destaabb).maxY = MAX(p[1],(destaabb).maxY);\
    (destaabb).minZ = MIN(p[2],(destaabb).minZ);\
    (destaabb).maxZ = MAX(p[2],(destaabb).maxZ);\
}\

//! Finds the intersection box of two boxes
#define BOXINTERSECTION(aabb1, aabb2, iaabb) {\
    (iaabb).minX = MAX((aabb1).minX,(aabb2).minX);\
    (iaabb).minY = MAX((aabb1).minY,(aabb2).minY);\
    (iaabb).minZ = MAX((aabb1).minZ,(aabb2).minZ);\
    (iaabb).maxX = MIN((aabb1).maxX,(aabb2).maxX);\
    (iaabb).maxY = MIN((aabb1).maxY,(aabb2).maxY);\
    (iaabb).maxZ = MIN((aabb1).maxZ,(aabb2).maxZ);\
}\

//! Determines if two aligned boxes do intersect
#define AABBCOLLISION(intersected,aabb1,aabb2) {\
	intersected = 1;\
	if ((aabb1).minX > (aabb2).maxX ||\
        (aabb1).maxX < (aabb2).minX ||\
        (aabb1).minY > (aabb2).maxY ||\
        (aabb1).maxY < (aabb2).minY ||\
        (aabb1).minZ > (aabb2).maxZ ||\
        (aabb1).maxZ < (aabb2).minZ )\
	{\
		intersected = 0;\
	}\
}\

#define AXIS_INTERSECT(min,max, a, d,tfirst, tlast,is_intersected) {\
	if(IS_ZERO(d))\
	{\
        is_intersected = !(a < min || a > max);\
	}\
	else\
	{\
        GREAL a0, a1;\
        a0 = (min - a) / (d);\
        a1 = (max - a) / (d);\
        if(a0 > a1)   SWAP_NUMBERS(a0, a1);\
        tfirst = MAX(a0, tfirst);\
        tlast = MIN(a1, tlast);\
        if (tlast < tfirst)\
        {\
             is_intersected = 0;\
        }\
        else\
        {\
            is_intersected = 1;\
        }\
	}\
}\

/*! \brief Finds the Ray intersection parameter.

\param aabb Aligned box
\param vorigin A vec3f with the origin of the ray
\param vdir A vec3f with the direction of the ray
\param tparam Output parameter
\param tmax Max lenght of the ray
\param is_intersected 1 if the ray collides the box, else false

*/
#define BOX_INTERSECTS_RAY(aabb, vorigin, vdir, tparam, tmax,is_intersected) { \
	GREAL _tfirst = 0.0f, _tlast = tmax;\
	AXIS_INTERSECT(aabb.minX,aabb.maxX,vorigin[0], vdir[0], _tfirst, _tlast,is_intersected);\
	if(is_intersected)\
	{\
        AXIS_INTERSECT(aabb.minY,aabb.maxY,vorigin[1], vdir[1], _tfirst, _tlast,is_intersected);\
	}\
	if(is_intersected)\
	{\
        AXIS_INTERSECT(aabb.minZ,aabb.maxZ,vorigin[2], vdir[2], _tfirst, _tlast,is_intersected);\
	}\
	tparam = _tfirst;\
}\

#define AABB_PROJECTION_INTERVAL(aabb,direction, vmin, vmax)\
{\
    GREAL _center[] = {(aabb.minX + aabb.maxX)*0.5f, (aabb.minY + aabb.maxY)*0.5f, (aabb.minZ + aabb.maxZ)*0.5f};\
    \
    GREAL _extend[] = {aabb.maxX-_center[0],aabb.maxY-_center[1],aabb.maxZ-_center[2]};\
    GREAL _fOrigin =  VEC_DOT(direction,_center);\
	GREAL _fMaximumExtent = _extend[0]*fabsf(direction[0]) + \
                            _extend[1]*fabsf(direction[1]) + \
                            _extend[2]*fabsf(direction[2]); \
\
    vmin = _fOrigin - _fMaximumExtent; \
    vmax = _fOrigin + _fMaximumExtent; \
}\

/*!
classify values:
<ol>
<li> 0 : In back of plane
<li> 1 : Spanning
<li> 2 : In front of
</ol>
*/
#define PLANE_CLASSIFY_BOX(plane,aabb,classify)\
{\
	GREAL _fmin,_fmax; \
	AABB_PROJECTION_INTERVAL(aabb,plane, _fmin, _fmax); \
	if(plane[3] >= _fmax) \
	{ \
		classify = 0;/*In back of*/ \
	} \
	else \
	{ \
		if(plane[3]+0.000001f>=_fmin) \
		{ \
			classify = 1;/*Spanning*/ \
		} \
		else \
		{ \
			classify = 2;/*In front of*/ \
		} \
	} \
}\
//! @}

/*! \defgroup GEOMETRIC_OPERATIONS
*/
//! @{


#define PLANEDIREPSILON 0.0000001f
#define PARALELENORMALS 0.000001f

#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); \
}\

/// plane is a vec4f
#define TRIANGLE_PLANE(v1,v2,v3,plane) {\
    TRIANGLE_NORMAL(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);	\
}\

#define POINT_IN_HULL(point,planes,plane_count,outside)\
{\
	GREAL _dis;\
	outside = 0;\
	GUINT _i = 0;\
	do\
	{\
	    _dis = DISTANCE_PLANE_POINT(planes[_i],point);\
	    if(_dis>0.0f) outside = 1;\
	    _i++;\
	}while(_i<plane_count&&outside==0);\
}\


#define PLANE_CLIP_SEGMENT(s1,s2,plane,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);	\
}\

//! 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>
*/
#define PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped,intersection_type) \
{\
	GREAL _dis1,_dis2;\
	_dis1 = DISTANCE_PLANE_POINT(plane,s1);\
	_dis2 = DISTANCE_PLANE_POINT(plane,s2);\
	if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)\
	{\
	    if(_dis1<_dis2) intersection_type = 0;\
	    else  intersection_type = 1;\
	}\
	else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)\
	{\
	    if(_dis1>_dis2) intersection_type = 2;\
	    else  intersection_type = 3;	    \
	}\
	else\
	{\
	    if(_dis1<_dis2) intersection_type = 4;\
	    else  intersection_type = 5;\
        VEC_DIFF(clipped,s2,s1);\
        _dis2 = VEC_DOT(clipped,plane);\
        VEC_SCALE(clipped,-_dis1/_dis2,clipped);\
        VEC_SUM(clipped,clipped,s1);	\
	}\
}\

//! 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>
*/
#define PLANE_CLIP_SEGMENT_CLOSEST(s1,s2,plane,clipped1,clipped2,intersection_type)\
{\
	PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1,intersection_type);\
	if(intersection_type == 0)\
	{\
	    VEC_COPY(clipped1,s1);\
	    VEC_COPY(clipped2,s2);\
	}\
	else if(intersection_type == 1)\
	{\
	    VEC_COPY(clipped1,s2);\
	    VEC_COPY(clipped2,s1);\
	}\
	else if(intersection_type == 2)\
	{\
	    VEC_COPY(clipped1,s1);\
	    VEC_COPY(clipped2,s2);\
	}\
	else if(intersection_type == 3)\
	{\
	    VEC_COPY(clipped1,s2);\
	    VEC_COPY(clipped2,s1);\
	}\
	else if(intersection_type == 4)\
	{	    \
	    VEC_COPY(clipped2,s1);\
	}\
	else if(intersection_type == 5)\
	{	    \
	    VEC_COPY(clipped2,s2);\
	}\
}\


//! Finds the 2 smallest cartesian coordinates of a plane normal
#define PLANE_MINOR_AXES(plane, i0, i1)\
{\
    GREAL A[] = {fabs(plane[0]),fabs(plane[1]),fabs(plane[2])};\
    if(A[0]>A[1])\
	{\
		if(A[0]>A[2])\
		{\
			i0=1;  /* A[0] is greatest */ \
			i1=2;\
		}\
		else \
		{\
			i0=0;      /* A[2] is greatest */ \
			i1=1; \
		}\
	}\
	else   /* A[0]<=A[1] */ \
	{\
		if(A[2]>A[1]) \
		{ \
			i0=0;      /* A[2] is greatest */ \
			i1=1; \
		}\
		else \
		{\
			i0=0;      /* A[1] is greatest */ \
			i1=2; \
		}\
	} \
}\

//! Ray plane collision
#define RAY_PLANE_COLLISION(plane,vDir,vPoint,pout,tparam,does_intersect)\
{\
	GREAL _dis,_dotdir;	\
	_dotdir = VEC_DOT(plane,vDir);\
	if(_dotdir<PLANEDIREPSILON)\
	{\
	    does_intersect = 0;\
	}\
	else\
	{\
        _dis = DISTANCE_PLANE_POINT(plane,vPoint);	\
        tparam = -_dis/_dotdir;\
        VEC_SCALE(pout,tparam,vDir);\
        VEC_SUM(pout,vPoint,pout);	\
        does_intersect = 1;\
	}\
}\

//! Bidireccional ray
#define LINE_PLANE_COLLISION(plane,vDir,vPoint,pout,tparam, tmin, tmax)\
{\
    tparam = -DISTANCE_PLANE_POINT(plane,vPoint);\
	tparam /= VEC_DOT(plane,vDir);\
    tparam = CLAMP(tparam,tmin,tmax);\
    VEC_SCALE(pout,tparam,vDir);\
    VEC_SUM(pout,vPoint,pout);	\
}\

/*! \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
  \param dointersect 1 if the planes intersect, 0 if paralell.

*/
#define INTERSECT_PLANES(p1,p2,p,d,dointersect) \
{ \
  VEC_CROSS(d,p1,p2); \
  GREAL denom = VEC_DOT(d, d);\
  if (IS_ZERO(denom)) \
  { \
      dointersect = 0; \
  } \
  else \
  { \
      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; \
      dointersect = 1; \
  }\
}\

//***************** SEGMENT and LINE FUNCTIONS **********************************///

/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
 */
#define CLOSEST_POINT_ON_SEGMENT(cp,v,e1,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

*/
#define LINE_INTERSECTION_PARAMS(dir1,point1, dir2, point2,t1,t2,dointersect) {\
    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(IS_ZERO(det))\
	{\
	     dointersect = 0;\
	}\
	else\
	{\
        t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;\
        t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;\
        dointersect = 1;\
	}\
}\

//! Find closest points on segments
#define SEGMENT_COLLISION(vA1,vA2,vB1,vB2,vPointA,vPointB)\
{\
    vec3f _AD,_BD,_N;\
    vec4f _M;\
    VEC_DIFF(_AD,vA2,vA1);\
    VEC_DIFF(_BD,vB2,vB1);\
    VEC_CROSS(_N,_AD,_BD);\
    VEC_CROSS(_M,_N,_BD);\
    _M[3] = VEC_DOT(_M,vB1);\
    float _tp; \
    LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,0.0f, 1.0f);\
    /*Closest point on segment*/ \
    VEC_DIFF(vPointB,vPointA,vB1);\
	_tp = VEC_DOT(vPointB, _BD);			\
	_tp/= VEC_DOT(_BD, _BD); \
	_tp = CLAMP(_tp,0.0f,1.0f);	\
    VEC_SCALE(vPointB,_tp,_BD);\
    VEC_SUM(vPointB,vPointB,vB1);\
}\

//! @}

///Additional Headers for Collision
#include "GIMPACT/gim_tri_collision.h"
#include "GIMPACT/gim_tri_sphere_collision.h"
#include "GIMPACT/gim_tri_capsule_collision.h"

#endif // GIM_VECTOR_H_INCLUDED