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 |
---|