| #ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED |
| #define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED |
| |
| /*! \file gim_basic_geometry_operations.h |
| *\author Francisco Len Nßjera |
| 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" |
| |
| |
| |
| |
| |
| #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);\ |
| }\ |
| |
| #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 |