[1963] | 1 | #ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED |
---|
| 2 | #define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED |
---|
| 3 | |
---|
| 4 | /*! \file gim_basic_geometry_operations.h |
---|
| 5 | *\author Francisco Len Nßjera |
---|
| 6 | type independant geometry routines |
---|
| 7 | |
---|
| 8 | */ |
---|
| 9 | /* |
---|
| 10 | ----------------------------------------------------------------------------- |
---|
| 11 | This source file is part of GIMPACT Library. |
---|
| 12 | |
---|
| 13 | For the latest info, see http://gimpact.sourceforge.net/ |
---|
| 14 | |
---|
| 15 | Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. |
---|
| 16 | email: projectileman@yahoo.com |
---|
| 17 | |
---|
| 18 | This library is free software; you can redistribute it and/or |
---|
| 19 | modify it under the terms of EITHER: |
---|
| 20 | (1) The GNU Lesser General Public License as published by the Free |
---|
| 21 | Software Foundation; either version 2.1 of the License, or (at |
---|
| 22 | your option) any later version. The text of the GNU Lesser |
---|
| 23 | General Public License is included with this library in the |
---|
| 24 | file GIMPACT-LICENSE-LGPL.TXT. |
---|
| 25 | (2) The BSD-style license that is included with this library in |
---|
| 26 | the file GIMPACT-LICENSE-BSD.TXT. |
---|
| 27 | (3) The zlib/libpng license that is included with this library in |
---|
| 28 | the file GIMPACT-LICENSE-ZLIB.TXT. |
---|
| 29 | |
---|
| 30 | This library is distributed in the hope that it will be useful, |
---|
| 31 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
---|
| 32 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files |
---|
| 33 | GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. |
---|
| 34 | |
---|
| 35 | ----------------------------------------------------------------------------- |
---|
| 36 | */ |
---|
| 37 | |
---|
| 38 | |
---|
| 39 | #include "gim_linear_math.h" |
---|
| 40 | |
---|
| 41 | |
---|
| 42 | |
---|
| 43 | /*! \defgroup GEOMETRIC_OPERATIONS |
---|
| 44 | */ |
---|
| 45 | //! @{ |
---|
| 46 | |
---|
| 47 | |
---|
| 48 | #define PLANEDIREPSILON 0.0000001f |
---|
| 49 | #define PARALELENORMALS 0.000001f |
---|
| 50 | |
---|
| 51 | |
---|
| 52 | #define TRIANGLE_NORMAL(v1,v2,v3,n)\ |
---|
| 53 | {\ |
---|
| 54 | vec3f _dif1,_dif2;\ |
---|
| 55 | VEC_DIFF(_dif1,v2,v1);\ |
---|
| 56 | VEC_DIFF(_dif2,v3,v1);\ |
---|
| 57 | VEC_CROSS(n,_dif1,_dif2);\ |
---|
| 58 | VEC_NORMALIZE(n);\ |
---|
| 59 | }\ |
---|
| 60 | |
---|
| 61 | #define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\ |
---|
| 62 | vec3f _dif1,_dif2; \ |
---|
| 63 | VEC_DIFF(_dif1,v2,v1); \ |
---|
| 64 | VEC_DIFF(_dif2,v3,v1); \ |
---|
| 65 | VEC_CROSS(n,_dif1,_dif2); \ |
---|
| 66 | }\ |
---|
| 67 | |
---|
| 68 | /// plane is a vec4f |
---|
| 69 | #define TRIANGLE_PLANE(v1,v2,v3,plane) {\ |
---|
| 70 | TRIANGLE_NORMAL(v1,v2,v3,plane);\ |
---|
| 71 | plane[3] = VEC_DOT(v1,plane);\ |
---|
| 72 | }\ |
---|
| 73 | |
---|
| 74 | /// plane is a vec4f |
---|
| 75 | #define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\ |
---|
| 76 | TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\ |
---|
| 77 | plane[3] = VEC_DOT(v1,plane);\ |
---|
| 78 | }\ |
---|
| 79 | |
---|
| 80 | /// Calc a plane from an edge an a normal. plane is a vec4f |
---|
| 81 | #define EDGE_PLANE(e1,e2,n,plane) {\ |
---|
| 82 | vec3f _dif; \ |
---|
| 83 | VEC_DIFF(_dif,e2,e1); \ |
---|
| 84 | VEC_CROSS(plane,_dif,n); \ |
---|
| 85 | VEC_NORMALIZE(plane); \ |
---|
| 86 | plane[3] = VEC_DOT(e1,plane);\ |
---|
| 87 | }\ |
---|
| 88 | |
---|
| 89 | #define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3]) |
---|
| 90 | |
---|
| 91 | #define PROJECT_POINT_PLANE(point,plane,projected) {\ |
---|
| 92 | GREAL _dis;\ |
---|
| 93 | _dis = DISTANCE_PLANE_POINT(plane,point);\ |
---|
| 94 | VEC_SCALE(projected,-_dis,plane);\ |
---|
| 95 | VEC_SUM(projected,projected,point); \ |
---|
| 96 | }\ |
---|
| 97 | |
---|
| 98 | //! Verifies if a point is in the plane hull |
---|
| 99 | template<typename CLASS_POINT,typename CLASS_PLANE> |
---|
| 100 | SIMD_FORCE_INLINE bool POINT_IN_HULL( |
---|
| 101 | const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count) |
---|
| 102 | { |
---|
| 103 | GREAL _dis; |
---|
| 104 | for (GUINT _i = 0;_i< plane_count;++_i) |
---|
| 105 | { |
---|
| 106 | _dis = DISTANCE_PLANE_POINT(planes[_i],point); |
---|
| 107 | if(_dis>0.0f) return false; |
---|
| 108 | } |
---|
| 109 | return true; |
---|
| 110 | } |
---|
| 111 | |
---|
| 112 | template<typename CLASS_POINT,typename CLASS_PLANE> |
---|
| 113 | SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT( |
---|
| 114 | const CLASS_POINT& s1, |
---|
| 115 | const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped) |
---|
| 116 | { |
---|
| 117 | GREAL _dis1,_dis2; |
---|
| 118 | _dis1 = DISTANCE_PLANE_POINT(plane,s1); |
---|
| 119 | VEC_DIFF(clipped,s2,s1); |
---|
| 120 | _dis2 = VEC_DOT(clipped,plane); |
---|
| 121 | VEC_SCALE(clipped,-_dis1/_dis2,clipped); |
---|
| 122 | VEC_SUM(clipped,clipped,s1); |
---|
| 123 | } |
---|
| 124 | |
---|
| 125 | enum ePLANE_INTERSECTION_TYPE |
---|
| 126 | { |
---|
| 127 | G_BACK_PLANE = 0, |
---|
| 128 | G_COLLIDE_PLANE, |
---|
| 129 | G_FRONT_PLANE |
---|
| 130 | }; |
---|
| 131 | |
---|
| 132 | enum eLINE_PLANE_INTERSECTION_TYPE |
---|
| 133 | { |
---|
| 134 | G_FRONT_PLANE_S1 = 0, |
---|
| 135 | G_FRONT_PLANE_S2, |
---|
| 136 | G_BACK_PLANE_S1, |
---|
| 137 | G_BACK_PLANE_S2, |
---|
| 138 | G_COLLIDE_PLANE_S1, |
---|
| 139 | G_COLLIDE_PLANE_S2 |
---|
| 140 | }; |
---|
| 141 | |
---|
| 142 | //! Confirms if the plane intersect the edge or nor |
---|
| 143 | /*! |
---|
| 144 | intersection type must have the following values |
---|
| 145 | <ul> |
---|
| 146 | <li> 0 : Segment in front of plane, s1 closest |
---|
| 147 | <li> 1 : Segment in front of plane, s2 closest |
---|
| 148 | <li> 2 : Segment in back of plane, s1 closest |
---|
| 149 | <li> 3 : Segment in back of plane, s2 closest |
---|
| 150 | <li> 4 : Segment collides plane, s1 in back |
---|
| 151 | <li> 5 : Segment collides plane, s2 in back |
---|
| 152 | </ul> |
---|
| 153 | */ |
---|
| 154 | |
---|
| 155 | template<typename CLASS_POINT,typename CLASS_PLANE> |
---|
| 156 | SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2( |
---|
| 157 | const CLASS_POINT& s1, |
---|
| 158 | const CLASS_POINT &s2, |
---|
| 159 | const CLASS_PLANE &plane,CLASS_POINT &clipped) |
---|
| 160 | { |
---|
| 161 | GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1); |
---|
| 162 | GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2); |
---|
| 163 | if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON) |
---|
| 164 | { |
---|
| 165 | if(_dis1<_dis2) return G_FRONT_PLANE_S1; |
---|
| 166 | return G_FRONT_PLANE_S2; |
---|
| 167 | } |
---|
| 168 | else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON) |
---|
| 169 | { |
---|
| 170 | if(_dis1>_dis2) return G_BACK_PLANE_S1; |
---|
| 171 | return G_BACK_PLANE_S2; |
---|
| 172 | } |
---|
| 173 | |
---|
| 174 | VEC_DIFF(clipped,s2,s1); |
---|
| 175 | _dis2 = VEC_DOT(clipped,plane); |
---|
| 176 | VEC_SCALE(clipped,-_dis1/_dis2,clipped); |
---|
| 177 | VEC_SUM(clipped,clipped,s1); |
---|
| 178 | if(_dis1<_dis2) return G_COLLIDE_PLANE_S1; |
---|
| 179 | return G_COLLIDE_PLANE_S2; |
---|
| 180 | } |
---|
| 181 | |
---|
| 182 | //! Confirms if the plane intersect the edge or not |
---|
| 183 | /*! |
---|
| 184 | clipped1 and clipped2 are the vertices behind the plane. |
---|
| 185 | clipped1 is the closest |
---|
| 186 | |
---|
| 187 | intersection_type must have the following values |
---|
| 188 | <ul> |
---|
| 189 | <li> 0 : Segment in front of plane, s1 closest |
---|
| 190 | <li> 1 : Segment in front of plane, s2 closest |
---|
| 191 | <li> 2 : Segment in back of plane, s1 closest |
---|
| 192 | <li> 3 : Segment in back of plane, s2 closest |
---|
| 193 | <li> 4 : Segment collides plane, s1 in back |
---|
| 194 | <li> 5 : Segment collides plane, s2 in back |
---|
| 195 | </ul> |
---|
| 196 | */ |
---|
| 197 | template<typename CLASS_POINT,typename CLASS_PLANE> |
---|
| 198 | SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST( |
---|
| 199 | const CLASS_POINT& s1, |
---|
| 200 | const CLASS_POINT &s2, |
---|
| 201 | const CLASS_PLANE &plane, |
---|
| 202 | CLASS_POINT &clipped1,CLASS_POINT &clipped2) |
---|
| 203 | { |
---|
| 204 | eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1); |
---|
| 205 | switch(intersection_type) |
---|
| 206 | { |
---|
| 207 | case G_FRONT_PLANE_S1: |
---|
| 208 | VEC_COPY(clipped1,s1); |
---|
| 209 | VEC_COPY(clipped2,s2); |
---|
| 210 | break; |
---|
| 211 | case G_FRONT_PLANE_S2: |
---|
| 212 | VEC_COPY(clipped1,s2); |
---|
| 213 | VEC_COPY(clipped2,s1); |
---|
| 214 | break; |
---|
| 215 | case G_BACK_PLANE_S1: |
---|
| 216 | VEC_COPY(clipped1,s1); |
---|
| 217 | VEC_COPY(clipped2,s2); |
---|
| 218 | break; |
---|
| 219 | case G_BACK_PLANE_S2: |
---|
| 220 | VEC_COPY(clipped1,s2); |
---|
| 221 | VEC_COPY(clipped2,s1); |
---|
| 222 | break; |
---|
| 223 | case G_COLLIDE_PLANE_S1: |
---|
| 224 | VEC_COPY(clipped2,s1); |
---|
| 225 | break; |
---|
| 226 | case G_COLLIDE_PLANE_S2: |
---|
| 227 | VEC_COPY(clipped2,s2); |
---|
| 228 | break; |
---|
| 229 | } |
---|
| 230 | return intersection_type; |
---|
| 231 | } |
---|
| 232 | |
---|
| 233 | |
---|
| 234 | //! Finds the 2 smallest cartesian coordinates of a plane normal |
---|
| 235 | #define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1) |
---|
| 236 | |
---|
| 237 | //! Ray plane collision in one way |
---|
| 238 | /*! |
---|
| 239 | Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/> |
---|
| 240 | It uses the PLANEDIREPSILON constant. |
---|
| 241 | */ |
---|
| 242 | template<typename T,typename CLASS_POINT,typename CLASS_PLANE> |
---|
| 243 | SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION( |
---|
| 244 | const CLASS_PLANE & plane, |
---|
| 245 | const CLASS_POINT & vDir, |
---|
| 246 | const CLASS_POINT & vPoint, |
---|
| 247 | CLASS_POINT & pout,T &tparam) |
---|
| 248 | { |
---|
| 249 | GREAL _dis,_dotdir; |
---|
| 250 | _dotdir = VEC_DOT(plane,vDir); |
---|
| 251 | if(_dotdir<PLANEDIREPSILON) |
---|
| 252 | { |
---|
| 253 | return false; |
---|
| 254 | } |
---|
| 255 | _dis = DISTANCE_PLANE_POINT(plane,vPoint); |
---|
| 256 | tparam = -_dis/_dotdir; |
---|
| 257 | VEC_SCALE(pout,tparam,vDir); |
---|
| 258 | VEC_SUM(pout,vPoint,pout); |
---|
| 259 | return true; |
---|
| 260 | } |
---|
| 261 | |
---|
| 262 | //! line collision |
---|
| 263 | /*! |
---|
| 264 | *\return |
---|
| 265 | -0 if the ray never intersects |
---|
| 266 | -1 if the ray collides in front |
---|
| 267 | -2 if the ray collides in back |
---|
| 268 | */ |
---|
| 269 | template<typename T,typename CLASS_POINT,typename CLASS_PLANE> |
---|
| 270 | SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION( |
---|
| 271 | const CLASS_PLANE & plane, |
---|
| 272 | const CLASS_POINT & vDir, |
---|
| 273 | const CLASS_POINT & vPoint, |
---|
| 274 | CLASS_POINT & pout, |
---|
| 275 | T &tparam, |
---|
| 276 | T tmin, T tmax) |
---|
| 277 | { |
---|
| 278 | GREAL _dis,_dotdir; |
---|
| 279 | _dotdir = VEC_DOT(plane,vDir); |
---|
| 280 | if(btFabs(_dotdir)<PLANEDIREPSILON) |
---|
| 281 | { |
---|
| 282 | tparam = tmax; |
---|
| 283 | return 0; |
---|
| 284 | } |
---|
| 285 | _dis = DISTANCE_PLANE_POINT(plane,vPoint); |
---|
| 286 | char returnvalue = _dis<0.0f?2:1; |
---|
| 287 | tparam = -_dis/_dotdir; |
---|
| 288 | |
---|
| 289 | if(tparam<tmin) |
---|
| 290 | { |
---|
| 291 | returnvalue = 0; |
---|
| 292 | tparam = tmin; |
---|
| 293 | } |
---|
| 294 | else if(tparam>tmax) |
---|
| 295 | { |
---|
| 296 | returnvalue = 0; |
---|
| 297 | tparam = tmax; |
---|
| 298 | } |
---|
| 299 | |
---|
| 300 | VEC_SCALE(pout,tparam,vDir); |
---|
| 301 | VEC_SUM(pout,vPoint,pout); |
---|
| 302 | return returnvalue; |
---|
| 303 | } |
---|
| 304 | |
---|
| 305 | /*! \brief Returns the Ray on which 2 planes intersect if they do. |
---|
| 306 | Written by Rodrigo Hernandez on ODE convex collision |
---|
| 307 | |
---|
| 308 | \param p1 Plane 1 |
---|
| 309 | \param p2 Plane 2 |
---|
| 310 | \param p Contains the origin of the ray upon returning if planes intersect |
---|
| 311 | \param d Contains the direction of the ray upon returning if planes intersect |
---|
| 312 | \return true if the planes intersect, 0 if paralell. |
---|
| 313 | |
---|
| 314 | */ |
---|
| 315 | template<typename CLASS_POINT,typename CLASS_PLANE> |
---|
| 316 | SIMD_FORCE_INLINE bool INTERSECT_PLANES( |
---|
| 317 | const CLASS_PLANE &p1, |
---|
| 318 | const CLASS_PLANE &p2, |
---|
| 319 | CLASS_POINT &p, |
---|
| 320 | CLASS_POINT &d) |
---|
| 321 | { |
---|
| 322 | VEC_CROSS(d,p1,p2); |
---|
| 323 | GREAL denom = VEC_DOT(d, d); |
---|
| 324 | if(GIM_IS_ZERO(denom)) return false; |
---|
| 325 | vec3f _n; |
---|
| 326 | _n[0]=p1[3]*p2[0] - p2[3]*p1[0]; |
---|
| 327 | _n[1]=p1[3]*p2[1] - p2[3]*p1[1]; |
---|
| 328 | _n[2]=p1[3]*p2[2] - p2[3]*p1[2]; |
---|
| 329 | VEC_CROSS(p,_n,d); |
---|
| 330 | p[0]/=denom; |
---|
| 331 | p[1]/=denom; |
---|
| 332 | p[2]/=denom; |
---|
| 333 | return true; |
---|
| 334 | } |
---|
| 335 | |
---|
| 336 | //***************** SEGMENT and LINE FUNCTIONS **********************************/// |
---|
| 337 | |
---|
| 338 | /*! Finds the closest point(cp) to (v) on a segment (e1,e2) |
---|
| 339 | */ |
---|
| 340 | template<typename CLASS_POINT> |
---|
| 341 | SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT( |
---|
| 342 | CLASS_POINT & cp, const CLASS_POINT & v, |
---|
| 343 | const CLASS_POINT &e1,const CLASS_POINT &e2) |
---|
| 344 | { |
---|
| 345 | vec3f _n; |
---|
| 346 | VEC_DIFF(_n,e2,e1); |
---|
| 347 | VEC_DIFF(cp,v,e1); |
---|
| 348 | GREAL _scalar = VEC_DOT(cp, _n); |
---|
| 349 | _scalar/= VEC_DOT(_n, _n); |
---|
| 350 | if(_scalar <0.0f) |
---|
| 351 | { |
---|
| 352 | VEC_COPY(cp,e1); |
---|
| 353 | } |
---|
| 354 | else if(_scalar >1.0f) |
---|
| 355 | { |
---|
| 356 | VEC_COPY(cp,e2); |
---|
| 357 | } |
---|
| 358 | else |
---|
| 359 | { |
---|
| 360 | VEC_SCALE(cp,_scalar,_n); |
---|
| 361 | VEC_SUM(cp,cp,e1); |
---|
| 362 | } |
---|
| 363 | } |
---|
| 364 | |
---|
| 365 | |
---|
| 366 | /*! \brief Finds the line params where these lines intersect. |
---|
| 367 | |
---|
| 368 | \param dir1 Direction of line 1 |
---|
| 369 | \param point1 Point of line 1 |
---|
| 370 | \param dir2 Direction of line 2 |
---|
| 371 | \param point2 Point of line 2 |
---|
| 372 | \param t1 Result Parameter for line 1 |
---|
| 373 | \param t2 Result Parameter for line 2 |
---|
| 374 | \param dointersect 0 if the lines won't intersect, else 1 |
---|
| 375 | |
---|
| 376 | */ |
---|
| 377 | template<typename T,typename CLASS_POINT> |
---|
| 378 | SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS( |
---|
| 379 | const CLASS_POINT & dir1, |
---|
| 380 | CLASS_POINT & point1, |
---|
| 381 | const CLASS_POINT & dir2, |
---|
| 382 | CLASS_POINT & point2, |
---|
| 383 | T& t1,T& t2) |
---|
| 384 | { |
---|
| 385 | GREAL det; |
---|
| 386 | GREAL e1e1 = VEC_DOT(dir1,dir1); |
---|
| 387 | GREAL e1e2 = VEC_DOT(dir1,dir2); |
---|
| 388 | GREAL e2e2 = VEC_DOT(dir2,dir2); |
---|
| 389 | vec3f p1p2; |
---|
| 390 | VEC_DIFF(p1p2,point1,point2); |
---|
| 391 | GREAL p1p2e1 = VEC_DOT(p1p2,dir1); |
---|
| 392 | GREAL p1p2e2 = VEC_DOT(p1p2,dir2); |
---|
| 393 | det = e1e2*e1e2 - e1e1*e2e2; |
---|
| 394 | if(GIM_IS_ZERO(det)) return false; |
---|
| 395 | t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det; |
---|
| 396 | t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det; |
---|
| 397 | return true; |
---|
| 398 | } |
---|
| 399 | |
---|
| 400 | //! Find closest points on segments |
---|
| 401 | template<typename CLASS_POINT> |
---|
| 402 | SIMD_FORCE_INLINE void SEGMENT_COLLISION( |
---|
| 403 | const CLASS_POINT & vA1, |
---|
| 404 | const CLASS_POINT & vA2, |
---|
| 405 | const CLASS_POINT & vB1, |
---|
| 406 | const CLASS_POINT & vB2, |
---|
| 407 | CLASS_POINT & vPointA, |
---|
| 408 | CLASS_POINT & vPointB) |
---|
| 409 | { |
---|
| 410 | CLASS_POINT _AD,_BD,_N; |
---|
| 411 | vec4f _M;//plane |
---|
| 412 | VEC_DIFF(_AD,vA2,vA1); |
---|
| 413 | VEC_DIFF(_BD,vB2,vB1); |
---|
| 414 | VEC_CROSS(_N,_AD,_BD); |
---|
| 415 | GREAL _tp = VEC_DOT(_N,_N); |
---|
| 416 | if(_tp<G_EPSILON)//ARE PARALELE |
---|
| 417 | { |
---|
| 418 | //project B over A |
---|
| 419 | bool invert_b_order = false; |
---|
| 420 | _M[0] = VEC_DOT(vB1,_AD); |
---|
| 421 | _M[1] = VEC_DOT(vB2,_AD); |
---|
| 422 | if(_M[0]>_M[1]) |
---|
| 423 | { |
---|
| 424 | invert_b_order = true; |
---|
| 425 | GIM_SWAP_NUMBERS(_M[0],_M[1]); |
---|
| 426 | } |
---|
| 427 | _M[2] = VEC_DOT(vA1,_AD); |
---|
| 428 | _M[3] = VEC_DOT(vA2,_AD); |
---|
| 429 | //mid points |
---|
| 430 | _N[0] = (_M[0]+_M[1])*0.5f; |
---|
| 431 | _N[1] = (_M[2]+_M[3])*0.5f; |
---|
| 432 | |
---|
| 433 | if(_N[0]<_N[1]) |
---|
| 434 | { |
---|
| 435 | if(_M[1]<_M[2]) |
---|
| 436 | { |
---|
| 437 | vPointB = invert_b_order?vB1:vB2; |
---|
| 438 | vPointA = vA1; |
---|
| 439 | } |
---|
| 440 | else if(_M[1]<_M[3]) |
---|
| 441 | { |
---|
| 442 | vPointB = invert_b_order?vB1:vB2; |
---|
| 443 | CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2); |
---|
| 444 | } |
---|
| 445 | else |
---|
| 446 | { |
---|
| 447 | vPointA = vA2; |
---|
| 448 | CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2); |
---|
| 449 | } |
---|
| 450 | } |
---|
| 451 | else |
---|
| 452 | { |
---|
| 453 | if(_M[3]<_M[0]) |
---|
| 454 | { |
---|
| 455 | vPointB = invert_b_order?vB2:vB1; |
---|
| 456 | vPointA = vA2; |
---|
| 457 | } |
---|
| 458 | else if(_M[3]<_M[1]) |
---|
| 459 | { |
---|
| 460 | vPointA = vA2; |
---|
| 461 | CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2); |
---|
| 462 | } |
---|
| 463 | else |
---|
| 464 | { |
---|
| 465 | vPointB = invert_b_order?vB1:vB2; |
---|
| 466 | CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2); |
---|
| 467 | } |
---|
| 468 | } |
---|
| 469 | return; |
---|
| 470 | } |
---|
| 471 | |
---|
| 472 | |
---|
| 473 | VEC_CROSS(_M,_N,_BD); |
---|
| 474 | _M[3] = VEC_DOT(_M,vB1); |
---|
| 475 | |
---|
| 476 | LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1)); |
---|
| 477 | /*Closest point on segment*/ |
---|
| 478 | VEC_DIFF(vPointB,vPointA,vB1); |
---|
| 479 | _tp = VEC_DOT(vPointB, _BD); |
---|
| 480 | _tp/= VEC_DOT(_BD, _BD); |
---|
| 481 | _tp = GIM_CLAMP(_tp,0.0f,1.0f); |
---|
| 482 | VEC_SCALE(vPointB,_tp,_BD); |
---|
| 483 | VEC_SUM(vPointB,vPointB,vB1); |
---|
| 484 | } |
---|
| 485 | |
---|
| 486 | |
---|
| 487 | |
---|
| 488 | |
---|
| 489 | //! Line box intersection in one dimension |
---|
| 490 | /*! |
---|
| 491 | |
---|
| 492 | *\param pos Position of the ray |
---|
| 493 | *\param dir Projection of the Direction of the ray |
---|
| 494 | *\param bmin Minimum bound of the box |
---|
| 495 | *\param bmax Maximum bound of the box |
---|
| 496 | *\param tfirst the minimum projection. Assign to 0 at first. |
---|
| 497 | *\param tlast the maximum projection. Assign to INFINITY at first. |
---|
| 498 | *\return true if there is an intersection. |
---|
| 499 | */ |
---|
| 500 | template<typename T> |
---|
| 501 | SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast) |
---|
| 502 | { |
---|
| 503 | if(GIM_IS_ZERO(dir)) |
---|
| 504 | { |
---|
| 505 | return !(pos < bmin || pos > bmax); |
---|
| 506 | } |
---|
| 507 | GREAL a0 = (bmin - pos) / dir; |
---|
| 508 | GREAL a1 = (bmax - pos) / dir; |
---|
| 509 | if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1); |
---|
| 510 | tfirst = GIM_MAX(a0, tfirst); |
---|
| 511 | tlast = GIM_MIN(a1, tlast); |
---|
| 512 | if (tlast < tfirst) return false; |
---|
| 513 | return true; |
---|
| 514 | } |
---|
| 515 | |
---|
| 516 | |
---|
| 517 | //! Sorts 3 componets |
---|
| 518 | template<typename T> |
---|
| 519 | SIMD_FORCE_INLINE void SORT_3_INDICES( |
---|
| 520 | const T * values, |
---|
| 521 | GUINT * order_indices) |
---|
| 522 | { |
---|
| 523 | //get minimum |
---|
| 524 | order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2); |
---|
| 525 | |
---|
| 526 | //get second and third |
---|
| 527 | GUINT i0 = (order_indices[0] + 1)%3; |
---|
| 528 | GUINT i1 = (i0 + 1)%3; |
---|
| 529 | |
---|
| 530 | if(values[i0] < values[i1]) |
---|
| 531 | { |
---|
| 532 | order_indices[1] = i0; |
---|
| 533 | order_indices[2] = i1; |
---|
| 534 | } |
---|
| 535 | else |
---|
| 536 | { |
---|
| 537 | order_indices[1] = i1; |
---|
| 538 | order_indices[2] = i0; |
---|
| 539 | } |
---|
| 540 | } |
---|
| 541 | |
---|
| 542 | |
---|
| 543 | |
---|
| 544 | //! @} |
---|
| 545 | |
---|
| 546 | |
---|
| 547 | #endif // GIM_VECTOR_H_INCLUDED |
---|