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