[2431] | 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 | |
---|
| 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 |
---|