Changeset View
Changeset View
Standalone View
Standalone View
extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h
| Show All 21 Lines | |||||
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. | ||||
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | ||||
| 3. This notice may not be removed or altered from any source distribution. | 3. This notice may not be removed or altered from any source distribution. | ||||
| */ | */ | ||||
| #include "btBoxCollision.h" | #include "btBoxCollision.h" | ||||
| #define PLANEDIREPSILON 0.0000001f | #define PLANEDIREPSILON 0.0000001f | ||||
| #define PARALELENORMALS 0.000001f | #define PARALELENORMALS 0.000001f | ||||
| #define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number)) | #define BT_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number)) | ||||
| /// Calc a plane from a triangle edge an a normal. plane is a vec4f | /// Calc a plane from a triangle edge an a normal. plane is a vec4f | ||||
| SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 & e2, const btVector3 & normal,btVector4 & plane) | SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 &e1, const btVector3 &e2, const btVector3 &normal, btVector4 &plane) | ||||
| { | { | ||||
| btVector3 planenormal = (e2-e1).cross(normal); | btVector3 planenormal = (e2 - e1).cross(normal); | ||||
| planenormal.normalize(); | planenormal.normalize(); | ||||
| plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal)); | plane.setValue(planenormal[0], planenormal[1], planenormal[2], e2.dot(planenormal)); | ||||
| } | } | ||||
| //***************** SEGMENT and LINE FUNCTIONS **********************************/// | //***************** SEGMENT and LINE FUNCTIONS **********************************/// | ||||
| /*! Finds the closest point(cp) to (v) on a segment (e1,e2) | /*! Finds the closest point(cp) to (v) on a segment (e1,e2) | ||||
| */ | */ | ||||
| SIMD_FORCE_INLINE void bt_closest_point_on_segment( | SIMD_FORCE_INLINE void bt_closest_point_on_segment( | ||||
| btVector3 & cp, const btVector3 & v, | btVector3 &cp, const btVector3 &v, | ||||
| const btVector3 &e1,const btVector3 &e2) | const btVector3 &e1, const btVector3 &e2) | ||||
| { | { | ||||
| btVector3 n = e2-e1; | btVector3 n = e2 - e1; | ||||
| cp = v - e1; | cp = v - e1; | ||||
| btScalar _scalar = cp.dot(n)/n.dot(n); | btScalar _scalar = cp.dot(n) / n.dot(n); | ||||
| if(_scalar <0.0f) | if (_scalar < 0.0f) | ||||
| { | { | ||||
| cp = e1; | cp = e1; | ||||
| } | } | ||||
| else if(_scalar >1.0f) | else if (_scalar > 1.0f) | ||||
| { | { | ||||
| cp = e2; | cp = e2; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| cp = _scalar*n + e1; | cp = _scalar * n + e1; | ||||
| } | } | ||||
| } | } | ||||
| //! line plane collision | //! line plane collision | ||||
| /*! | /*! | ||||
| *\return | *\return | ||||
| -0 if the ray never intersects | -0 if the ray never intersects | ||||
| -1 if the ray collides in front | -1 if the ray collides in front | ||||
| -2 if the ray collides in back | -2 if the ray collides in back | ||||
| */ | */ | ||||
| SIMD_FORCE_INLINE int bt_line_plane_collision( | SIMD_FORCE_INLINE int bt_line_plane_collision( | ||||
| const btVector4 & plane, | const btVector4 &plane, | ||||
| const btVector3 & vDir, | const btVector3 &vDir, | ||||
| const btVector3 & vPoint, | const btVector3 &vPoint, | ||||
| btVector3 & pout, | btVector3 &pout, | ||||
| btScalar &tparam, | btScalar &tparam, | ||||
| btScalar tmin, btScalar tmax) | btScalar tmin, btScalar tmax) | ||||
| { | { | ||||
| btScalar _dotdir = vDir.dot(plane); | btScalar _dotdir = vDir.dot(plane); | ||||
| if(btFabs(_dotdir)<PLANEDIREPSILON) | if (btFabs(_dotdir) < PLANEDIREPSILON) | ||||
| { | { | ||||
| tparam = tmax; | tparam = tmax; | ||||
| return 0; | return 0; | ||||
| } | } | ||||
| btScalar _dis = bt_distance_point_plane(plane,vPoint); | btScalar _dis = bt_distance_point_plane(plane, vPoint); | ||||
| char returnvalue = _dis<0.0f? 2:1; | char returnvalue = _dis < 0.0f ? 2 : 1; | ||||
| tparam = -_dis/_dotdir; | tparam = -_dis / _dotdir; | ||||
| if(tparam<tmin) | if (tparam < tmin) | ||||
| { | { | ||||
| returnvalue = 0; | returnvalue = 0; | ||||
| tparam = tmin; | tparam = tmin; | ||||
| } | } | ||||
| else if(tparam>tmax) | else if (tparam > tmax) | ||||
| { | { | ||||
| returnvalue = 0; | returnvalue = 0; | ||||
| tparam = tmax; | tparam = tmax; | ||||
| } | } | ||||
| pout = tparam*vDir + vPoint; | pout = tparam * vDir + vPoint; | ||||
| return returnvalue; | return returnvalue; | ||||
| } | } | ||||
| //! Find closest points on segments | //! Find closest points on segments | ||||
| SIMD_FORCE_INLINE void bt_segment_collision( | SIMD_FORCE_INLINE void bt_segment_collision( | ||||
| const btVector3 & vA1, | const btVector3 &vA1, | ||||
| const btVector3 & vA2, | const btVector3 &vA2, | ||||
| const btVector3 & vB1, | const btVector3 &vB1, | ||||
| const btVector3 & vB2, | const btVector3 &vB2, | ||||
| btVector3 & vPointA, | btVector3 &vPointA, | ||||
| btVector3 & vPointB) | btVector3 &vPointB) | ||||
| { | { | ||||
| btVector3 AD = vA2 - vA1; | btVector3 AD = vA2 - vA1; | ||||
| btVector3 BD = vB2 - vB1; | btVector3 BD = vB2 - vB1; | ||||
| btVector3 N = AD.cross(BD); | btVector3 N = AD.cross(BD); | ||||
| btScalar tp = N.length2(); | btScalar tp = N.length2(); | ||||
| btVector4 _M;//plane | btVector4 _M; //plane | ||||
| if(tp<SIMD_EPSILON)//ARE PARALELE | if (tp < SIMD_EPSILON) //ARE PARALELE | ||||
| { | { | ||||
| //project B over A | //project B over A | ||||
| bool invert_b_order = false; | bool invert_b_order = false; | ||||
| _M[0] = vB1.dot(AD); | _M[0] = vB1.dot(AD); | ||||
| _M[1] = vB2.dot(AD); | _M[1] = vB2.dot(AD); | ||||
| if(_M[0]>_M[1]) | if (_M[0] > _M[1]) | ||||
| { | { | ||||
| invert_b_order = true; | invert_b_order = true; | ||||
| BT_SWAP_NUMBERS(_M[0],_M[1]); | BT_SWAP_NUMBERS(_M[0], _M[1]); | ||||
| } | } | ||||
| _M[2] = vA1.dot(AD); | _M[2] = vA1.dot(AD); | ||||
| _M[3] = vA2.dot(AD); | _M[3] = vA2.dot(AD); | ||||
| //mid points | //mid points | ||||
| N[0] = (_M[0]+_M[1])*0.5f; | N[0] = (_M[0] + _M[1]) * 0.5f; | ||||
| N[1] = (_M[2]+_M[3])*0.5f; | N[1] = (_M[2] + _M[3]) * 0.5f; | ||||
| if(N[0]<N[1]) | if (N[0] < N[1]) | ||||
| { | { | ||||
| if(_M[1]<_M[2]) | if (_M[1] < _M[2]) | ||||
| { | { | ||||
| vPointB = invert_b_order?vB1:vB2; | vPointB = invert_b_order ? vB1 : vB2; | ||||
| vPointA = vA1; | vPointA = vA1; | ||||
| } | } | ||||
| else if(_M[1]<_M[3]) | else if (_M[1] < _M[3]) | ||||
| { | { | ||||
| vPointB = invert_b_order?vB1:vB2; | vPointB = invert_b_order ? vB1 : vB2; | ||||
| bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2); | bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2); | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| vPointA = vA2; | vPointA = vA2; | ||||
| bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2); | bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2); | ||||
| } | } | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| if(_M[3]<_M[0]) | if (_M[3] < _M[0]) | ||||
| { | { | ||||
| vPointB = invert_b_order?vB2:vB1; | vPointB = invert_b_order ? vB2 : vB1; | ||||
| vPointA = vA2; | vPointA = vA2; | ||||
| } | } | ||||
| else if(_M[3]<_M[1]) | else if (_M[3] < _M[1]) | ||||
| { | { | ||||
| vPointA = vA2; | vPointA = vA2; | ||||
| bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2); | bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2); | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| vPointB = invert_b_order?vB1:vB2; | vPointB = invert_b_order ? vB1 : vB2; | ||||
| bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2); | bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2); | ||||
| } | } | ||||
| } | } | ||||
| return; | return; | ||||
| } | } | ||||
| N = N.cross(BD); | N = N.cross(BD); | ||||
| _M.setValue(N[0],N[1],N[2],vB1.dot(N)); | _M.setValue(N[0], N[1], N[2], vB1.dot(N)); | ||||
| // get point A as the plane collision point | // get point A as the plane collision point | ||||
| bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1)); | bt_line_plane_collision(_M, AD, vA1, vPointA, tp, btScalar(0), btScalar(1)); | ||||
| /*Closest point on segment*/ | /*Closest point on segment*/ | ||||
| vPointB = vPointA - vB1; | vPointB = vPointA - vB1; | ||||
| tp = vPointB.dot(BD); | tp = vPointB.dot(BD); | ||||
| tp/= BD.dot(BD); | tp /= BD.dot(BD); | ||||
| tp = BT_CLAMP(tp,0.0f,1.0f); | tp = BT_CLAMP(tp, 0.0f, 1.0f); | ||||
| vPointB = tp*BD + vB1; | vPointB = tp * BD + vB1; | ||||
| } | } | ||||
| #endif // GIM_VECTOR_H_INCLUDED | #endif // GIM_VECTOR_H_INCLUDED | ||||