Blender V2.61 - r43446

btGeometryOperations.h

Go to the documentation of this file.
00001 #ifndef BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
00002 #define BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
00003 
00008 /*
00009 This source file is part of GIMPACT Library.
00010 
00011 For the latest info, see http://gimpact.sourceforge.net/
00012 
00013 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
00014 email: projectileman@yahoo.com
00015 
00016 
00017 This software is provided 'as-is', without any express or implied warranty.
00018 In no event will the authors be held liable for any damages arising from the use of this software.
00019 Permission is granted to anyone to use this software for any purpose,
00020 including commercial applications, and to alter it and redistribute it freely,
00021 subject to the following restrictions:
00022 
00023 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.
00024 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00025 3. This notice may not be removed or altered from any source distribution.
00026 */
00027 
00028 #include "btBoxCollision.h"
00029 
00030 
00031 
00032 
00033 
00034 #define PLANEDIREPSILON 0.0000001f
00035 #define PARALELENORMALS 0.000001f
00036 
00037 
00038 #define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
00039 
00041 SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 &  e2, const btVector3 & normal,btVector4 & plane)
00042 {
00043     btVector3 planenormal = (e2-e1).cross(normal);
00044     planenormal.normalize();
00045     plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal));
00046 }
00047 
00048 
00049 
00050 //***************** SEGMENT and LINE FUNCTIONS **********************************///
00051 
00054 SIMD_FORCE_INLINE void bt_closest_point_on_segment(
00055     btVector3 & cp, const btVector3 & v,
00056     const btVector3  &e1,const btVector3 &e2)
00057 {
00058     btVector3 n = e2-e1;
00059     cp = v - e1;
00060     btScalar _scalar = cp.dot(n)/n.dot(n);
00061     if(_scalar <0.0f)
00062     {
00063         cp = e1;
00064     }
00065     else if(_scalar >1.0f)
00066     {
00067         cp = e2;
00068     }
00069     else
00070     {
00071         cp = _scalar*n + e1;
00072     }
00073 }
00074 
00075 
00077 
00084 SIMD_FORCE_INLINE int bt_line_plane_collision(
00085     const btVector4 & plane,
00086     const btVector3 & vDir,
00087     const btVector3 & vPoint,
00088     btVector3 & pout,
00089     btScalar &tparam,
00090     btScalar tmin, btScalar tmax)
00091 {
00092 
00093     btScalar _dotdir = vDir.dot(plane);
00094 
00095     if(btFabs(_dotdir)<PLANEDIREPSILON)
00096     {
00097         tparam = tmax;
00098         return 0;
00099     }
00100 
00101     btScalar _dis = bt_distance_point_plane(plane,vPoint);
00102     char returnvalue = _dis<0.0f? 2:1;
00103     tparam = -_dis/_dotdir;
00104 
00105     if(tparam<tmin)
00106     {
00107         returnvalue = 0;
00108         tparam = tmin;
00109     }
00110     else if(tparam>tmax)
00111     {
00112         returnvalue = 0;
00113         tparam = tmax;
00114     }
00115     pout = tparam*vDir + vPoint;
00116     return returnvalue;
00117 }
00118 
00119 
00121 SIMD_FORCE_INLINE void bt_segment_collision(
00122     const btVector3 & vA1,
00123     const btVector3 & vA2,
00124     const btVector3 & vB1,
00125     const btVector3 & vB2,
00126     btVector3 & vPointA,
00127     btVector3 & vPointB)
00128 {
00129     btVector3 AD = vA2 - vA1;
00130     btVector3 BD = vB2 - vB1;
00131     btVector3 N = AD.cross(BD);
00132     btScalar tp = N.length2();
00133 
00134     btVector4 _M;//plane
00135 
00136     if(tp<SIMD_EPSILON)//ARE PARALELE
00137     {
00138         //project B over A
00139         bool invert_b_order = false;
00140         _M[0] = vB1.dot(AD);
00141         _M[1] = vB2.dot(AD);
00142 
00143         if(_M[0]>_M[1])
00144         {
00145             invert_b_order  = true;
00146             BT_SWAP_NUMBERS(_M[0],_M[1]);
00147         }
00148         _M[2] = vA1.dot(AD);
00149         _M[3] = vA2.dot(AD);
00150         //mid points
00151         N[0] = (_M[0]+_M[1])*0.5f;
00152         N[1] = (_M[2]+_M[3])*0.5f;
00153 
00154         if(N[0]<N[1])
00155         {
00156             if(_M[1]<_M[2])
00157             {
00158                 vPointB = invert_b_order?vB1:vB2;
00159                 vPointA = vA1;
00160             }
00161             else if(_M[1]<_M[3])
00162             {
00163                 vPointB = invert_b_order?vB1:vB2;
00164                 bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
00165             }
00166             else
00167             {
00168                 vPointA = vA2;
00169                 bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
00170             }
00171         }
00172         else
00173         {
00174             if(_M[3]<_M[0])
00175             {
00176                 vPointB = invert_b_order?vB2:vB1;
00177                 vPointA = vA2;
00178             }
00179             else if(_M[3]<_M[1])
00180             {
00181                 vPointA = vA2;
00182                 bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
00183             }
00184             else
00185             {
00186                 vPointB = invert_b_order?vB1:vB2;
00187                 bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
00188             }
00189         }
00190         return;
00191     }
00192 
00193     N = N.cross(BD);
00194     _M.setValue(N[0],N[1],N[2],vB1.dot(N));
00195 
00196     // get point A as the plane collision point
00197     bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1));
00198 
00199     /*Closest point on segment*/
00200     vPointB = vPointA - vB1;
00201     tp = vPointB.dot(BD);
00202     tp/= BD.dot(BD);
00203     tp = BT_CLAMP(tp,0.0f,1.0f);
00204 
00205     vPointB = tp*BD + vB1;
00206 }
00207 
00208 
00209 
00210 
00211 
00212 #endif // GIM_VECTOR_H_INCLUDED