diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/Gimpact/btBoxCollision.h')
| -rw-r--r-- | thirdparty/bullet/BulletCollision/Gimpact/btBoxCollision.h | 471 |
1 files changed, 223 insertions, 248 deletions
diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btBoxCollision.h b/thirdparty/bullet/BulletCollision/Gimpact/btBoxCollision.h index 0a0357e5a8..182835c3b4 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btBoxCollision.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btBoxCollision.h @@ -26,27 +26,21 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" - ///Swap numbers -#define BT_SWAP_NUMBERS(a,b){ \ - a = a+b; \ - b = a-b; \ - a = a-b; \ -}\ - - -#define BT_MAX(a,b) (a<b?b:a) -#define BT_MIN(a,b) (a>b?b:a) - -#define BT_GREATER(x, y) btFabs(x) > (y) - -#define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c)) -#define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c)) - - +#define BT_SWAP_NUMBERS(a, b) \ + { \ + a = a + b; \ + b = a - b; \ + a = a - b; \ + } +#define BT_MAX(a, b) (a < b ? b : a) +#define BT_MIN(a, b) (a > b ? b : a) +#define BT_GREATER(x, y) btFabs(x) > (y) +#define BT_MAX3(a, b, c) BT_MAX(a, BT_MAX(b, c)) +#define BT_MIN3(a, b, c) BT_MIN(a, BT_MIN(b, c)) enum eBT_PLANE_INTERSECTION_TYPE { @@ -115,152 +109,144 @@ enum eBT_PLANE_INTERSECTION_TYPE // return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1); //} +#define TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, i_dir_0, i_dir_1, i_comp_0, i_comp_1) \ + { \ + const btScalar dir0 = -edge[i_dir_0]; \ + const btScalar dir1 = edge[i_dir_1]; \ + btScalar pmin = pointa[i_comp_0] * dir0 + pointa[i_comp_1] * dir1; \ + btScalar pmax = pointb[i_comp_0] * dir0 + pointb[i_comp_1] * dir1; \ + if (pmin > pmax) \ + { \ + BT_SWAP_NUMBERS(pmin, pmax); \ + } \ + const btScalar abs_dir0 = absolute_edge[i_dir_0]; \ + const btScalar abs_dir1 = absolute_edge[i_dir_1]; \ + const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1; \ + if (pmin > rad || -rad > pmax) return false; \ + } -#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\ -{\ - const btScalar dir0 = -edge[i_dir_0];\ - const btScalar dir1 = edge[i_dir_1];\ - btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\ - btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\ - if(pmin>pmax)\ - {\ - BT_SWAP_NUMBERS(pmin,pmax); \ - }\ - const btScalar abs_dir0 = absolute_edge[i_dir_0];\ - const btScalar abs_dir1 = absolute_edge[i_dir_1];\ - const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\ - if(pmin>rad || -rad>pmax) return false;\ -}\ - - -#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\ -}\ - -#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\ -}\ +#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \ + { \ + TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 2, 1, 1, 2); \ + } -#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ -{\ - TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\ -}\ +#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \ + { \ + TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 0, 2, 2, 0); \ + } +#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \ + { \ + TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 1, 0, 0, 1); \ + } //! Returns the dot product between a vec3f and the col of a matrix SIMD_FORCE_INLINE btScalar bt_mat3_dot_col( -const btMatrix3x3 & mat, const btVector3 & vec3, int colindex) + const btMatrix3x3 &mat, const btVector3 &vec3, int colindex) { - return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex]; + return vec3[0] * mat[0][colindex] + vec3[1] * mat[1][colindex] + vec3[2] * mat[2][colindex]; } - //! Class for transforming a model1 to the space of model0 -ATTRIBUTE_ALIGNED16 (class) BT_BOX_BOX_TRANSFORM_CACHE +ATTRIBUTE_ALIGNED16(class) +BT_BOX_BOX_TRANSFORM_CACHE { public: - btVector3 m_T1to0;//!< Transforms translation of model1 to model 0 - btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal to R0' * R1 - btMatrix3x3 m_AR;//!< Absolute value of m_R1to0 + btVector3 m_T1to0; //!< Transforms translation of model1 to model 0 + btMatrix3x3 m_R1to0; //!< Transforms Rotation of model1 to model 0, equal to R0' * R1 + btMatrix3x3 m_AR; //!< Absolute value of m_R1to0 SIMD_FORCE_INLINE void calc_absolute_matrix() { -// static const btVector3 vepsi(1e-6f,1e-6f,1e-6f); -// m_AR[0] = vepsi + m_R1to0[0].absolute(); -// m_AR[1] = vepsi + m_R1to0[1].absolute(); -// m_AR[2] = vepsi + m_R1to0[2].absolute(); - - int i,j; + // static const btVector3 vepsi(1e-6f,1e-6f,1e-6f); + // m_AR[0] = vepsi + m_R1to0[0].absolute(); + // m_AR[1] = vepsi + m_R1to0[1].absolute(); + // m_AR[2] = vepsi + m_R1to0[2].absolute(); - for(i=0;i<3;i++) - { - for(j=0;j<3;j++ ) - { - m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]); - } - } + int i, j; + for (i = 0; i < 3; i++) + { + for (j = 0; j < 3; j++) + { + m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]); + } + } } BT_BOX_BOX_TRANSFORM_CACHE() { } - - //! Calc the transformation relative 1 to 0. Inverts matrics by transposing - SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1) + SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform &trans0, const btTransform &trans1) { - btTransform temp_trans = trans0.inverse(); temp_trans = temp_trans * trans1; m_T1to0 = temp_trans.getOrigin(); m_R1to0 = temp_trans.getBasis(); - calc_absolute_matrix(); } //! Calcs the full invertion of the matrices. Useful for scaling matrices - SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1) + SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform &trans0, const btTransform &trans1) { m_R1to0 = trans0.getBasis().inverse(); m_T1to0 = m_R1to0 * (-trans0.getOrigin()); - m_T1to0 += m_R1to0*trans1.getOrigin(); + m_T1to0 += m_R1to0 * trans1.getOrigin(); m_R1to0 *= trans1.getBasis(); calc_absolute_matrix(); } - SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const + SIMD_FORCE_INLINE btVector3 transform(const btVector3 &point) const { - return point.dot3( m_R1to0[0], m_R1to0[1], m_R1to0[2] ) + m_T1to0; + return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0; } }; - #define BOX_PLANE_EPSILON 0.000001f //! Axis aligned box -ATTRIBUTE_ALIGNED16 (class) btAABB +ATTRIBUTE_ALIGNED16(class) +btAABB { public: btVector3 m_min; btVector3 m_max; btAABB() - {} - + { + } - btAABB(const btVector3 & V1, - const btVector3 & V2, - const btVector3 & V3) + btAABB(const btVector3 &V1, + const btVector3 &V2, + const btVector3 &V3) { - m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]); } - btAABB(const btVector3 & V1, - const btVector3 & V2, - const btVector3 & V3, - btScalar margin) + btAABB(const btVector3 &V1, + const btVector3 &V2, + const btVector3 &V3, + btScalar margin) { - m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]); m_min[0] -= margin; m_min[1] -= margin; @@ -270,13 +256,11 @@ public: m_max[2] += margin; } - btAABB(const btAABB &other): - m_min(other.m_min),m_max(other.m_max) + btAABB(const btAABB &other) : m_min(other.m_min), m_max(other.m_max) { } - btAABB(const btAABB &other,btScalar margin ): - m_min(other.m_min),m_max(other.m_max) + btAABB(const btAABB &other, btScalar margin) : m_min(other.m_min), m_max(other.m_max) { m_min[0] -= margin; m_min[1] -= margin; @@ -317,34 +301,34 @@ public: m_max[2] = other.m_max[2] + margin; } - template<typename CLASS_POINT> + template <typename CLASS_POINT> SIMD_FORCE_INLINE void calc_from_triangle( - const CLASS_POINT & V1, - const CLASS_POINT & V2, - const CLASS_POINT & V3) + const CLASS_POINT &V1, + const CLASS_POINT &V2, + const CLASS_POINT &V3) { - m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]); } - template<typename CLASS_POINT> + template <typename CLASS_POINT> SIMD_FORCE_INLINE void calc_from_triangle_margin( - const CLASS_POINT & V1, - const CLASS_POINT & V2, - const CLASS_POINT & V3, btScalar margin) + const CLASS_POINT &V1, + const CLASS_POINT &V2, + const CLASS_POINT &V3, btScalar margin) { - m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]); m_min[0] -= margin; m_min[1] -= margin; @@ -355,91 +339,89 @@ public: } //! Apply a transform to an AABB - SIMD_FORCE_INLINE void appy_transform(const btTransform & trans) + SIMD_FORCE_INLINE void appy_transform(const btTransform &trans) { - btVector3 center = (m_max+m_min)*0.5f; + btVector3 center = (m_max + m_min) * 0.5f; btVector3 extends = m_max - center; // Compute new center center = trans(center); - btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), - trans.getBasis().getRow(1).absolute(), - trans.getBasis().getRow(2).absolute()); + btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), + trans.getBasis().getRow(1).absolute(), + trans.getBasis().getRow(2).absolute()); m_min = center - textends; m_max = center + textends; } - //! Apply a transform to an AABB - SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans) + SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE &trans) { - btVector3 center = (m_max+m_min)*0.5f; + btVector3 center = (m_max + m_min) * 0.5f; btVector3 extends = m_max - center; // Compute new center center = trans.transform(center); - btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(), - trans.m_R1to0.getRow(1).absolute(), - trans.m_R1to0.getRow(2).absolute()); - + btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(), + trans.m_R1to0.getRow(1).absolute(), + trans.m_R1to0.getRow(2).absolute()); + m_min = center - textends; m_max = center + textends; } //! Merges a Box - SIMD_FORCE_INLINE void merge(const btAABB & box) + SIMD_FORCE_INLINE void merge(const btAABB &box) { - m_min[0] = BT_MIN(m_min[0],box.m_min[0]); - m_min[1] = BT_MIN(m_min[1],box.m_min[1]); - m_min[2] = BT_MIN(m_min[2],box.m_min[2]); + m_min[0] = BT_MIN(m_min[0], box.m_min[0]); + m_min[1] = BT_MIN(m_min[1], box.m_min[1]); + m_min[2] = BT_MIN(m_min[2], box.m_min[2]); - m_max[0] = BT_MAX(m_max[0],box.m_max[0]); - m_max[1] = BT_MAX(m_max[1],box.m_max[1]); - m_max[2] = BT_MAX(m_max[2],box.m_max[2]); + m_max[0] = BT_MAX(m_max[0], box.m_max[0]); + m_max[1] = BT_MAX(m_max[1], box.m_max[1]); + m_max[2] = BT_MAX(m_max[2], box.m_max[2]); } //! Merges a point - template<typename CLASS_POINT> - SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point) + template <typename CLASS_POINT> + SIMD_FORCE_INLINE void merge_point(const CLASS_POINT &point) { - m_min[0] = BT_MIN(m_min[0],point[0]); - m_min[1] = BT_MIN(m_min[1],point[1]); - m_min[2] = BT_MIN(m_min[2],point[2]); + m_min[0] = BT_MIN(m_min[0], point[0]); + m_min[1] = BT_MIN(m_min[1], point[1]); + m_min[2] = BT_MIN(m_min[2], point[2]); - m_max[0] = BT_MAX(m_max[0],point[0]); - m_max[1] = BT_MAX(m_max[1],point[1]); - m_max[2] = BT_MAX(m_max[2],point[2]); + m_max[0] = BT_MAX(m_max[0], point[0]); + m_max[1] = BT_MAX(m_max[1], point[1]); + m_max[2] = BT_MAX(m_max[2], point[2]); } //! Gets the extend and center - SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const + SIMD_FORCE_INLINE void get_center_extend(btVector3 & center, btVector3 & extend) const { - center = (m_max+m_min)*0.5f; + center = (m_max + m_min) * 0.5f; extend = m_max - center; } //! Finds the intersecting box between this box and the other. - SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection) const + SIMD_FORCE_INLINE void find_intersection(const btAABB &other, btAABB &intersection) const { - intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]); - intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]); - intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]); + intersection.m_min[0] = BT_MAX(other.m_min[0], m_min[0]); + intersection.m_min[1] = BT_MAX(other.m_min[1], m_min[1]); + intersection.m_min[2] = BT_MAX(other.m_min[2], m_min[2]); - intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]); - intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]); - intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]); + intersection.m_max[0] = BT_MIN(other.m_max[0], m_max[0]); + intersection.m_max[1] = BT_MIN(other.m_max[1], m_max[1]); + intersection.m_max[2] = BT_MIN(other.m_max[2], m_max[2]); } - - SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const + SIMD_FORCE_INLINE bool has_collision(const btAABB &other) const { - if(m_min[0] > other.m_max[0] || - m_max[0] < other.m_min[0] || - m_min[1] > other.m_max[1] || - m_max[1] < other.m_min[1] || - m_min[2] > other.m_max[2] || - m_max[2] < other.m_min[2]) + if (m_min[0] > other.m_max[0] || + m_max[0] < other.m_min[0] || + m_min[1] > other.m_max[1] || + m_max[1] < other.m_min[1] || + m_min[2] > other.m_max[2] || + m_max[2] < other.m_min[2]) { return false; } @@ -451,35 +433,34 @@ public: \param vorigin A vec3f with the origin of the ray \param vdir A vec3f with the direction of the ray */ - SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir) const + SIMD_FORCE_INLINE bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) const { - btVector3 extents,center; - this->get_center_extend(center,extents);; + btVector3 extents, center; + this->get_center_extend(center, extents); + ; btScalar Dx = vorigin[0] - center[0]; - if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false; + if (BT_GREATER(Dx, extents[0]) && Dx * vdir[0] >= 0.0f) return false; btScalar Dy = vorigin[1] - center[1]; - if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false; + if (BT_GREATER(Dy, extents[1]) && Dy * vdir[1] >= 0.0f) return false; btScalar Dz = vorigin[2] - center[2]; - if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false; - + if (BT_GREATER(Dz, extents[2]) && Dz * vdir[2] >= 0.0f) return false; btScalar f = vdir[1] * Dz - vdir[2] * Dy; - if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false; + if (btFabs(f) > extents[1] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[1])) return false; f = vdir[2] * Dx - vdir[0] * Dz; - if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false; + if (btFabs(f) > extents[0] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[0])) return false; f = vdir[0] * Dy - vdir[1] * Dx; - if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false; + if (btFabs(f) > extents[0] * btFabs(vdir[1]) + extents[1] * btFabs(vdir[0])) return false; return true; } - - SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const + SIMD_FORCE_INLINE void projection_interval(const btVector3 &direction, btScalar &vmin, btScalar &vmax) const { - btVector3 center = (m_max+m_min)*0.5f; - btVector3 extend = m_max-center; + btVector3 center = (m_max + m_min) * 0.5f; + btVector3 extend = m_max - center; - btScalar _fOrigin = direction.dot(center); + btScalar _fOrigin = direction.dot(center); btScalar _fMaximumExtent = extend.dot(direction.absolute()); vmin = _fOrigin - _fMaximumExtent; vmax = _fOrigin + _fMaximumExtent; @@ -487,30 +468,30 @@ public: SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const { - btScalar _fmin,_fmax; - this->projection_interval(plane,_fmin,_fmax); + btScalar _fmin, _fmax; + this->projection_interval(plane, _fmin, _fmax); - if(plane[3] > _fmax + BOX_PLANE_EPSILON) + if (plane[3] > _fmax + BOX_PLANE_EPSILON) { - return BT_CONST_BACK_PLANE; // 0 + return BT_CONST_BACK_PLANE; // 0 } - if(plane[3]+BOX_PLANE_EPSILON >=_fmin) + if (plane[3] + BOX_PLANE_EPSILON >= _fmin) { - return BT_CONST_COLLIDE_PLANE; //1 + return BT_CONST_COLLIDE_PLANE; //1 } - return BT_CONST_FRONT_PLANE;//2 + return BT_CONST_FRONT_PLANE; //2 } - SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const + SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB &box, btTransform &trans1_to_0) const { btAABB tbox = box; tbox.appy_transform(trans1_to_0); return has_collision(tbox); } - SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box, - const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const + SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB &box, + const BT_BOX_BOX_TRANSFORM_CACHE &trans1_to_0) const { btAABB tbox = box; tbox.appy_transform_trans_cache(trans1_to_0); @@ -519,52 +500,50 @@ public: //! transcache is the transformation cache from box to this AABB SIMD_FORCE_INLINE bool overlapping_trans_cache( - const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const + const btAABB &box, const BT_BOX_BOX_TRANSFORM_CACHE &transcache, bool fulltest) const { - //Taken from OPCODE - btVector3 ea,eb;//extends - btVector3 ca,cb;//extends - get_center_extend(ca,ea); - box.get_center_extend(cb,eb); - + btVector3 ea, eb; //extends + btVector3 ca, cb; //extends + get_center_extend(ca, ea); + box.get_center_extend(cb, eb); btVector3 T; - btScalar t,t2; + btScalar t, t2; int i; // Class I : A's basis vectors - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i]; + T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i]; t = transcache.m_AR[i].dot(eb) + ea[i]; - if(BT_GREATER(T[i], t)) return false; + if (BT_GREATER(T[i], t)) return false; } // Class II : B's basis vectors - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - t = bt_mat3_dot_col(transcache.m_R1to0,T,i); - t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i]; - if(BT_GREATER(t,t2)) return false; + t = bt_mat3_dot_col(transcache.m_R1to0, T, i); + t2 = bt_mat3_dot_col(transcache.m_AR, ea, i) + eb[i]; + if (BT_GREATER(t, t2)) return false; } // Class III : 9 cross products - if(fulltest) + if (fulltest) { - int j,m,n,o,p,q,r; - for(i=0;i<3;i++) + int j, m, n, o, p, q, r; + for (i = 0; i < 3; i++) { - m = (i+1)%3; - n = (i+2)%3; - o = i==0?1:0; - p = i==2?1:2; - for(j=0;j<3;j++) + m = (i + 1) % 3; + n = (i + 2) % 3; + o = i == 0 ? 1 : 0; + p = i == 2 ? 1 : 2; + for (j = 0; j < 3; j++) { - q = j==2?1:2; - r = j==0?1:0; - t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j]; - t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] + - eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r]; - if(BT_GREATER(t,t2)) return false; + q = j == 2 ? 1 : 2; + r = j == 0 ? 1 : 0; + t = T[n] * transcache.m_R1to0[m][j] - T[m] * transcache.m_R1to0[n][j]; + t2 = ea[o] * transcache.m_AR[p][j] + ea[p] * transcache.m_AR[o][j] + + eb[r] * transcache.m_AR[i][q] + eb[q] * transcache.m_AR[i][r]; + if (BT_GREATER(t, t2)) return false; } } } @@ -573,7 +552,7 @@ public: //! Simple test for planes. SIMD_FORCE_INLINE bool collide_plane( - const btVector4 & plane) const + const btVector4 &plane) const { eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane); return (classify == BT_CONST_COLLIDE_PLANE); @@ -581,15 +560,15 @@ public: //! test for a triangle, with edges SIMD_FORCE_INLINE bool collide_triangle_exact( - const btVector3 & p1, - const btVector3 & p2, - const btVector3 & p3, - const btVector4 & triangle_plane) const + const btVector3 &p1, + const btVector3 &p2, + const btVector3 &p3, + const btVector4 &triangle_plane) const { - if(!collide_plane(triangle_plane)) return false; + if (!collide_plane(triangle_plane)) return false; - btVector3 center,extends; - this->get_center_extend(center,extends); + btVector3 center, extends; + this->get_center_extend(center, extends); const btVector3 v1(p1 - center); const btVector3 v2(p2 - center); @@ -599,47 +578,43 @@ public: btVector3 diff(v2 - v1); btVector3 abs_diff = diff.absolute(); //Test With X axis - TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends); + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v1, v3, extends); //Test With Y axis - TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends); + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v1, v3, extends); //Test With Z axis - TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends); - + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v1, v3, extends); diff = v3 - v2; abs_diff = diff.absolute(); //Test With X axis - TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends); + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v2, v1, extends); //Test With Y axis - TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends); + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v2, v1, extends); //Test With Z axis - TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends); + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v2, v1, extends); diff = v1 - v3; abs_diff = diff.absolute(); //Test With X axis - TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends); + TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v3, v2, extends); //Test With Y axis - TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends); + TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v3, v2, extends); //Test With Z axis - TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends); + TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v3, v2, extends); return true; } }; - //! Compairison of transformation objects -SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) +SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform &t1, const btTransform &t2) { - if(!(t1.getOrigin() == t2.getOrigin()) ) return false; + if (!(t1.getOrigin() == t2.getOrigin())) return false; - if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false; - if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false; - if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false; + if (!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0))) return false; + if (!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1))) return false; + if (!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2))) return false; return true; } - - -#endif // GIM_BOX_COLLISION_H_INCLUDED +#endif // GIM_BOX_COLLISION_H_INCLUDED |